sonar cloud issues + removed prints

src/slam/include/slam_solver/graph_slam_solver/optimizer/base_optimizer.hpp

@@ -18,7 +18,9 @@ class BaseOptimizer {
   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
+    if (this == &other) {
+      return *this;  // Prevent self-assignment
+    }
 
     // Note: _params_ is a reference, so we do not copy it
     return *this;

src/slam/src/adapter_slam/pacsim.cpp

@@ -77,6 +77,9 @@ void PacsimAdapter::fetch_discipline() {
               mission_result = common_lib::competition_logic::Mission::ACCELERATION;
             } else if (discipline == "trackdrive") {
               mission_result = common_lib::competition_logic::Mission::TRACKDRIVE;
+            } else {
+              RCLCPP_ERROR(this->get_logger(), "Unknown discipline received: %s",
+                           discipline.c_str());
             }
           } else {
             RCLCPP_ERROR(this->get_logger(), "Failed to retrieve discipline parameter.");

src/slam/src/ros_node/slam_node.cpp

@@ -116,7 +116,7 @@ void SLAMNode::init() {
 
 void SLAMNode::_perception_subscription_callback(
     const custom_interfaces::msg::PerceptionOutput &msg) {
-  RCLCPP_INFO(this->get_logger(), "SLAMNode::_perception_subscription_callback called");
+  RCLCPP_DEBUG(this->get_logger(), "SLAMNode::_perception_subscription_callback called");
   auto const &cone_array = msg.cones.cone_array;
   double perception_exec_time = msg.exec_time;
 
@@ -166,15 +166,14 @@ void SLAMNode::_perception_subscription_callback(
           cone.position.x, cone.position.y, cone.color, cone.confidence, msg.header.stamp));
     }
   }
-  RCLCPP_INFO(this->get_logger(), "ATTR - Perception map size: %ld", _perception_map_.size());
 
   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_, cones_time);
-  RCLCPP_INFO(this->get_logger(), "ATTR - Observations added to SLAM solver");
+  RCLCPP_DEBUG(this->get_logger(), "ATTR - Observations added to SLAM solver");
 
   std::vector<common_lib::structures::Cone> track_map;
   common_lib::structures::Pose vehicle_pose;
@@ -195,7 +194,6 @@ void SLAMNode::_perception_subscription_callback(
   if (this->_is_mission_finished()) {
     this->finish();
   }
-  RCLCPP_INFO(this->get_logger(), "ATTR - Mission finished check done");
 
   // this->_publish_covariance(); // TODO: get covariance to work fast. If uncommented -> update
   // mutex logics
@@ -214,7 +212,7 @@ void SLAMNode::_perception_subscription_callback(
     execution_time_msg.data = *this->_execution_times_;
   }
   this->_execution_time_publisher_->publish(execution_time_msg);
-  RCLCPP_INFO(this->get_logger(), "ATTR - Perception callback finished");
+  RCLCPP_DEBUG(this->get_logger(), "ATTR - Perception callback finished");
 }
 
 void SLAMNode::_velocities_subscription_callback(const custom_interfaces::msg::Velocities &msg) {

src/slam/src/slam_solver/ekf_slam_solver.cpp

@@ -84,11 +84,12 @@ void EKFSLAMSolver::add_observations(const std::vector<common_lib::structures::C
   if (!matched_landmarks_indices.empty()) {
     this->correct(state, covariance, matched_landmarks_indices, matched_observations);
   }
+  const bool is_preloaded_map_skidpad_or_accel =
+      this->_params_.using_preloaded_map_ &&
+      (this->_mission_ == common_lib::competition_logic::Mission::SKIDPAD ||
+       this->_mission_ == common_lib::competition_logic::Mission::ACCELERATION);
   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) {
+      !is_preloaded_map_skidpad_or_accel && this->lap_counter_ == 0) {
     this->state_augmentation(state, covariance, filtered_landmarks);
   }
   // Finally update the state

src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp

@@ -114,7 +114,9 @@ GraphSLAMInstance::GraphSLAMInstance(const GraphSLAMInstance& other) {
 }
 
 GraphSLAMInstance& GraphSLAMInstance::operator=(const GraphSLAMInstance& other) {
-  if (this == &other) return *this;  // Prevent self-assignment
+  if (this == &other) {
+    return *this;  // Prevent self-assignment
+  }
 
   // Copy each member individually
   _factor_graph_ = other._factor_graph_;
@@ -200,8 +202,6 @@ void GraphSLAMInstance::process_observations(const ObservationData& observation_
   bool new_observation_factors = false;
   unsigned int pose_id_at_observation_time =
       this->get_landmark_id_at_time(observation_data.timestamp_);
-  RCLCPP_INFO(rclcpp::get_logger("slam"), "locked_landmarks= %s",
-              this->_locked_landmarks_ ? "true" : "false");
   for (unsigned int i = 0; i < observations.size() / 2; i++) {
     gtsam::Point2 landmark;
     gtsam::Symbol landmark_symbol;
@@ -288,7 +288,9 @@ 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;
+  if (!this->_new_observation_factors_) {
+    return;
+  }
 
   this->_graph_values_ = this->_optimizer_->optimize(
       this->_factor_graph_, this->_graph_values_, this->_pose_counter_, this->_landmark_counter_);
@@ -322,7 +324,9 @@ unsigned int GraphSLAMInstance::get_landmark_id_at_time(const rclcpp::Time& time
     const TimedPose& curr = _pose_timestamps_.from_end(i);
 
     if (curr.timestamp.seconds() <= timestamp.seconds()) {
-      if (i == 0) return curr.pose_id;  // Avoid out-of-bounds
+      if (i == 0) {
+        return curr.pose_id;  // Avoid out-of-bounds
+      }
       const TimedPose& next = _pose_timestamps_.from_end(i - 1);
 
       auto diff_curr = timestamp.seconds() - curr.timestamp.seconds();

src/slam/src/slam_solver/graph_slam_solver/graph_slam_solver.cpp

@@ -206,7 +206,6 @@ void GraphSLAMSolver::add_observations(const std::vector<common_lib::structures:
     covariance = this->_graph_slam_instance_->get_covariance_matrix();
     covariance_time = rclcpp::Clock().now();
   }
-  RCLCPP_INFO(rclcpp::get_logger("slam"), "add_observations - State and covariance obtained");
   Eigen::VectorXi associations;
   Eigen::VectorXd filtered_new_observations;
   {
@@ -224,12 +223,9 @@ void GraphSLAMSolver::add_observations(const std::vector<common_lib::structures:
     association_time = rclcpp::Clock().now();
     RCLCPP_DEBUG(rclcpp::get_logger("slam"), "add_observations - Associations calculated");
 
-    RCLCPP_INFO(rclcpp::get_logger("slam"), "add_observations - Associations:");
-
     // Landmark filtering
     filtered_new_observations = this->_landmark_filter_->filter(
         observations_global, observations_confidences, associations);
-    RCLCPP_INFO(rclcpp::get_logger("slam"), "add_observations - Filtered new observations");
 
     // Loop closure detection
     LoopClosure::Result result =
@@ -244,7 +240,6 @@ void GraphSLAMSolver::add_observations(const std::vector<common_lib::structures:
       }
       RCLCPP_INFO(rclcpp::get_logger("slam"), "Lap counter: %d", lap_counter_);
     }
-    RCLCPP_INFO(rclcpp::get_logger("slam"), "add_observations - Loop closure checked");
 
     // Add observations to the graph
     ObservationData observation_data(std::make_shared<Eigen::VectorXd>(observations),
@@ -264,7 +259,6 @@ void GraphSLAMSolver::add_observations(const std::vector<common_lib::structures:
     this->_landmark_filter_->delete_landmarks(filtered_new_observations);
   }
 
-  RCLCPP_INFO(rclcpp::get_logger("slam"), "add_observations - Factors added to graph");
   factor_graph_time = rclcpp::Clock().now();
   RCLCPP_DEBUG(rclcpp::get_logger("slam"), "add_observations - Mutex unlocked - Factors added");
 
@@ -279,7 +273,6 @@ void GraphSLAMSolver::add_observations(const std::vector<common_lib::structures:
     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");
   }
-  RCLCPP_INFO(rclcpp::get_logger("slam"), "add_observations - Graph optimized if needed");
 
   // Timekeeping
   if (this->_execution_times_ == nullptr) {

src/slam/src/slam_solver/graph_slam_solver/optimizer/isam2_optimizer.cpp

@@ -36,7 +36,9 @@ ISAM2Optimizer::ISAM2Optimizer(const ISAM2Optimizer& other) : BaseOptimizer(othe
 }
 
 ISAM2Optimizer& ISAM2Optimizer::operator=(const ISAM2Optimizer& other) {
-  if (this == &other) return *this;  // Prevent self-assignment
+  if (this == &other) {
+    return *this;  // Prevent self-assignment
+  }
 
   // Copy each member individually
   BaseOptimizer::operator=(other);
@@ -70,6 +72,9 @@ gtsam::Values ISAM2Optimizer::optimize(gtsam::NonlinearFactorGraph& factor_graph
           _new_values_.insert(key, gtsam::Pose2(0, 0, 0));
         } else if (gtsam::Symbol(key).chr() == 'l') {
           _new_values_.insert(key, gtsam::Point2(0, 0));
+        } else {
+          RCLCPP_WARN(rclcpp::get_logger("slam"), "ISAM2Optimizer - Unknown key type: %c",
+                      gtsam::Symbol(key).chr());
         }
       }
     }

src/slam/src/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.cpp

@@ -41,12 +41,16 @@ std::shared_ptr<PoseUpdater> PoseUpdater::clone() const {
 }
 
 Eigen::Vector3d PoseUpdater::get_pose_at_timestamp(const rclcpp::Time& timestamp) const {
-  if (_pose_buffer_.size() == 0) throw std::out_of_range("Pose buffer is empty");
+  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.seconds() <= timestamp.seconds()) {
-      if (i == 0) return curr.pose;
+      if (i == 0) {
+        return curr.pose;
+      }
       const auto& prev = _pose_buffer_.from_end(i - 1);
       auto diff_curr = timestamp.seconds() - curr.timestamp.seconds();
       auto diff_prev = prev.timestamp.seconds() - timestamp.seconds();

src/slam/src/slam_solver/graph_slam_solver/pose_updater/difference_based_ready_pose_updater.cpp

@@ -17,7 +17,9 @@ DifferenceBasedReadyPoseUpdater::~DifferenceBasedReadyPoseUpdater() = default;
 
 DifferenceBasedReadyPoseUpdater& DifferenceBasedReadyPoseUpdater::operator=(
     const DifferenceBasedReadyPoseUpdater& other) {
-  if (this == &other) return *this;  // Prevent self-assignment
+  if (this == &other) {
+    return *this;  // Prevent self-assignment
+  }
 
   // Copy each member individually
   PoseUpdater::operator=(other);  // Call base class assignment operator

src/slam/src/slam_solver/slam_solver.cpp

@@ -30,6 +30,8 @@ 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 {
+      throw std::runtime_error("Unsupported mission for preloaded map");
     }
   }
 }