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 22cd72e3b..f16cfdb98 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 @@ -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; diff --git a/src/slam/src/adapter_slam/pacsim.cpp b/src/slam/src/adapter_slam/pacsim.cpp index 85cc5647d..5544f52c6 100644 --- a/src/slam/src/adapter_slam/pacsim.cpp +++ b/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."); diff --git a/src/slam/src/ros_node/slam_node.cpp b/src/slam/src/ros_node/slam_node.cpp index 0d8e06cd8..78621aa74 100644 --- a/src/slam/src/ros_node/slam_node.cpp +++ b/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,7 +166,6 @@ 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"); @@ -174,7 +173,7 @@ void SLAMNode::_perception_subscription_callback( } 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 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) { diff --git a/src/slam/src/slam_solver/ekf_slam_solver.cpp b/src/slam/src/slam_solver/ekf_slam_solver.cpp index 051183eb1..73cd221e6 100644 --- a/src/slam/src/slam_solver/ekf_slam_solver.cpp +++ b/src/slam/src/slam_solver/ekf_slam_solver.cpp @@ -84,11 +84,12 @@ void EKFSLAMSolver::add_observations(const std::vectorcorrect(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 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 a9f8df0da..d4a47209d 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 @@ -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(); 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 b95a4ba12..5055d119d 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 @@ -206,7 +206,6 @@ void GraphSLAMSolver::add_observations(const std::vector_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_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(observations), @@ -264,7 +259,6 @@ void GraphSLAMSolver::add_observations(const std::vector_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_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) { 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 ebfd60de8..675aa18b2 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 @@ -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()); } } } diff --git a/src/slam/src/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.cpp b/src/slam/src/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.cpp index 1b306fcd0..318f5b5ed 100644 --- a/src/slam/src/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.cpp +++ b/src/slam/src/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.cpp @@ -41,12 +41,16 @@ std::shared_ptr 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(); 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 index c8c5047a3..37c7d0e10 100644 --- 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 @@ -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 diff --git a/src/slam/src/slam_solver/slam_solver.cpp b/src/slam/src/slam_solver/slam_solver.cpp index b3bb32c3f..91a949be3 100644 --- a/src/slam/src/slam_solver/slam_solver.cpp +++ b/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"); } } }