Marcouto thesis implementation

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()

cone_viz/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>cone_viz</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="flopes.sp55@gmail.com">filipelopes</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>rclpy</depend>
+  <depend>visualization_msgs</depend>
+  <depend>custom_interfaces</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

cone_viz/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/cone_viz
+[install]
+install_scripts=$base/lib/cone_viz

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",
+        ],
+    },
+)

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'

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)

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'

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

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

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

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

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