Clean localization

bitbots_localization/include/bitbots_localization/ObservationModel.hpp

@@ -54,8 +54,6 @@ class RobotPoseObservationModel : public particle_filter::ObservationModel<Robot
 
   std::vector<std::pair<double, double>> get_measurement_field_boundary() const;
 
-  void set_min_weight(double min_weight);
-
   double get_min_weight() const override;
 
   void clear_measurement();
@@ -67,18 +65,17 @@ class RobotPoseObservationModel : public particle_filter::ObservationModel<Robot
                                     const std::vector<std::pair<double, double>> &last_measurement,
                                     std::shared_ptr<Map> map, double element_weight) const;
 
+  // Measurements
   std::vector<std::pair<double, double>> last_measurement_lines_;
-
   std::vector<std::pair<double, double>> last_measurement_goal_;
-
   std::vector<std::pair<double, double>> last_measurement_field_boundary_;
 
-  double min_weight_ = 0;
-
+  // Reference to the maps for the different classes
   std::shared_ptr<Map> map_lines_;
   std::shared_ptr<Map> map_goals_;
   std::shared_ptr<Map> map_field_boundary_;
 
+  // Parameters
   bitbots_localization::Params config_;
 };
 };  // namespace bitbots_localization

bitbots_localization/include/bitbots_localization/localization.hpp

@@ -147,12 +147,14 @@ class Localization : public rclcpp::Node {
   // Data structure to hold all parameters, which is build from the schema in the 'parameters.yaml'
   bitbots_localization::Params config_;
 
+  // Declare subscribers
   rclcpp::Subscription<sm::msg::PointCloud2>::SharedPtr line_point_cloud_subscriber_;
   rclcpp::Subscription<sv3dm::msg::GoalpostArray>::SharedPtr goal_subscriber_;
   rclcpp::Subscription<sv3dm::msg::FieldBoundary>::SharedPtr fieldboundary_subscriber_;
 
   rclcpp::Subscription<gm::msg::PoseWithCovarianceStamped>::SharedPtr rviz_initial_pose_subscriber_;
 
+  // Declare publishers
   rclcpp::Publisher<gm::msg::PoseWithCovarianceStamped>::SharedPtr pose_with_covariance_publisher_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pose_particles_publisher_;
   rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr lines_publisher_;
@@ -161,35 +163,43 @@ class Localization : public rclcpp::Node {
   rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr fieldboundary_ratings_publisher_;
   rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr field_publisher_;
 
+  // Declare services
   rclcpp::Service<bl::srv::ResetFilter>::SharedPtr reset_service_;
   rclcpp::Service<bl::srv::SetPaused>::SharedPtr pause_service_;
+
+  // Declare timers
   rclcpp::TimerBase::SharedPtr publishing_timer_;
+
+  // Declare tf2 objects
   std::unique_ptr<tf2_ros::Buffer> tfBuffer;
   std::shared_ptr<tf2_ros::TransformListener> tfListener;
   std::shared_ptr<tf2_ros::TransformBroadcaster> br;
 
+  // Declare particle filter components
   std::shared_ptr<pf::ImportanceResampling<RobotState>> resampling_;
   std::shared_ptr<RobotPoseObservationModel> robot_pose_observation_model_;
   std::shared_ptr<RobotMotionModel> robot_motion_model_;
+  std::shared_ptr<particle_filter::ParticleFilter<RobotState>> robot_pf_;
+
+  // Declare initial state distributions
   std::shared_ptr<RobotStateDistributionStartLeft> robot_state_distribution_start_left_;
   std::shared_ptr<RobotStateDistributionStartRight> robot_state_distribution_start_right_;
   std::shared_ptr<RobotStateDistributionRightHalf> robot_state_distribution_right_half_;
   std::shared_ptr<RobotStateDistributionLeftHalf> robot_state_distribution_left_half_;
   std::shared_ptr<RobotStateDistributionPosition> robot_state_distribution_position_;
   std::shared_ptr<RobotStateDistributionPose> robot_state_distribution_pose_;
-  std::shared_ptr<particle_filter::ParticleFilter<RobotState>> robot_pf_;
+
+  // Declare filter estimate
   RobotState estimate_;
   std::vector<double> estimate_cov_;
 
-  bool resampled_ = false;
-
+  // Declare input message buffers
   sm::msg::PointCloud2 line_pointcloud_relative_;
   sv3dm::msg::GoalpostArray goal_posts_relative_;
   sv3dm::msg::FieldBoundary fieldboundary_relative_;
-  sm::msg::CameraInfo cam_info_;
 
+  // Declare time stamps
   rclcpp::Time last_stamp_lines = rclcpp::Time(0);
-  rclcpp::Time last_stamp_lines_pc = rclcpp::Time(0);
   rclcpp::Time last_stamp_goals = rclcpp::Time(0);
   rclcpp::Time last_stamp_fb_points = rclcpp::Time(0);
   rclcpp::Time last_stamp_all_measurements = rclcpp::Time(0);
@@ -198,21 +208,31 @@ class Localization : public rclcpp::Node {
   builtin_interfaces::msg::Time localization_tf_last_published_time_ =
       builtin_interfaces::msg::Time(rclcpp::Time(0, 0, RCL_ROS_TIME));
 
-  /**
-   * Runs the filter for one step
-   */
-  void run_filter_one_step();
+  // Declare robot movement variables
+  geometry_msgs::msg::Vector3 linear_movement_;
+  geometry_msgs::msg::Vector3 rotational_movement_;
+
+  // Keep track of the odometry transform in the last step
+  geometry_msgs::msg::TransformStamped previousOdomTransform_;
 
+  // Flag that checks if the robot is moving
+  bool robot_moved = false;
+
+  // Keep track of the number of filter steps
+  int timer_callback_count_ = 0;
+
+  // Maps for the different measurement classes
   std::shared_ptr<Map> lines_;
   std::shared_ptr<Map> goals_;
   std::shared_ptr<Map> field_boundary_;
-  std::shared_ptr<Map> corner_;
-  std::shared_ptr<Map> t_crossings_map_;
-  std::shared_ptr<Map> crosses_map_;
 
-  gmms::GaussianMixtureModel pose_gmm_;
+  // RNG that is used for the different sampling steps
   particle_filter::CRandomNumberGenerator random_number_generator_;
-  std_msgs::msg::ColorRGBA marker_color;
+
+  /**
+   * Runs the filter for one step
+   */
+  void run_filter_one_step();
 
   /**
    * Publishes the position as a transform
@@ -260,16 +280,6 @@ class Localization : public rclcpp::Node {
    * Gets the and convert motion / odometry information
    */
   void getMotion();
-
-  geometry_msgs::msg::TransformStamped transformOdomBaseLink;
-  bool initialization = true;
-
-  geometry_msgs::msg::Vector3 linear_movement_;
-  geometry_msgs::msg::Vector3 rotational_movement_;
-  geometry_msgs::msg::TransformStamped previousOdomTransform_;
-  bool new_linepoints_ = false;
-  bool robot_moved = false;
-  int timer_callback_count_ = 0;
 };
 };  // namespace bitbots_localization
 

bitbots_localization/src/ObservationModel.cpp

@@ -50,8 +50,8 @@ double RobotPoseObservationModel::measure(const RobotState &state) const {
                    ((1 - scoring_config.field_boundary.factor) +
                     scoring_config.field_boundary.factor * particle_weight_field_boundary));
 
-  if (weight < min_weight_) {
-    weight = min_weight_;
+  if (weight < config_.particle_filter.weighting.min_weight) {
+    weight = config_.particle_filter.weighting.min_weight;
   }
 
   // reduce weight if particle is too far outside of the field:
@@ -101,9 +101,7 @@ std::vector<std::pair<double, double>> RobotPoseObservationModel::get_measuremen
   return last_measurement_field_boundary_;
 }
 
-void RobotPoseObservationModel::set_min_weight(double min_weight) { min_weight_ = min_weight; }
-
-double RobotPoseObservationModel::get_min_weight() const { return min_weight_; }
+double RobotPoseObservationModel::get_min_weight() const { return config_.particle_filter.weighting.min_weight; }
 
 void RobotPoseObservationModel::clear_measurement() {
   last_measurement_lines_.clear();

bitbots_localization/src/localization.cpp

@@ -106,7 +106,6 @@ void Localization::updateParams(bool force_reload) {
 
   // Init observation model
   robot_pose_observation_model_.reset(new RobotPoseObservationModel(lines_, goals_, field_boundary_, config_));
-  robot_pose_observation_model_->set_min_weight(config_.particle_filter.weighting.min_weight);
 
   // Init motion model
   auto drift_config = config_.particle_filter.drift;
@@ -166,7 +165,6 @@ void Localization::updateParams(bool force_reload) {
 
 void Localization::run_filter_one_step() {
   timer_callback_count_++;
-  resampled_ = false;
 
   // Check for new parameters and recreate necessary components if needed
   updateParams();
@@ -194,7 +192,6 @@ void Localization::run_filter_one_step() {
   // Check if its resampling time!
   if (timer_callback_count_ % config_.particle_filter.resampling_interval == 0) {
     robot_pf_->resample();
-    resampled_ = true;
   }
   // Publish transforms
   publish_transforms();
@@ -298,7 +295,7 @@ void Localization::reset_filter(int distribution, double x, double y, double ang
 
 void Localization::updateMeasurements() {
   // Sets the measurements in the observation model
-  if (line_pointcloud_relative_.header.stamp != last_stamp_lines_pc && config_.particle_filter.scoring.lines.factor) {
+  if (line_pointcloud_relative_.header.stamp != last_stamp_lines && config_.particle_filter.scoring.lines.factor) {
     robot_pose_observation_model_->set_measurement_lines_pc(line_pointcloud_relative_);
   }
   if (config_.particle_filter.scoring.goal.factor && goal_posts_relative_.header.stamp != last_stamp_goals) {
@@ -310,15 +307,14 @@ void Localization::updateMeasurements() {
   }
 
   // Set timestamps to mark past messages
-  last_stamp_lines_pc = line_pointcloud_relative_.header.stamp;
+  last_stamp_lines = line_pointcloud_relative_.header.stamp;
   last_stamp_goals = goal_posts_relative_.header.stamp;
   last_stamp_fb_points = fieldboundary_relative_.header.stamp;
   // Maximum time stamp of the last measurements
-  last_stamp_all_measurements = std::max({last_stamp_lines_pc, last_stamp_goals, last_stamp_fb_points});
+  last_stamp_all_measurements = std::max({last_stamp_lines, last_stamp_goals, last_stamp_fb_points});
 }
 
 void Localization::getMotion() {
-  robot_moved = false;
   geometry_msgs::msg::TransformStamped transformStampedNow;
 
   try {
@@ -356,6 +352,7 @@ void Localization::getMotion() {
                     rotational_movement_.z / time_delta >= config_.misc.min_motion_angular;
     } else {
       RCLCPP_WARN(this->get_logger(), "Time step delta of zero encountered! This should not happen!");
+      robot_moved = false;
     }
 
     // Set the variable for the transform of the previous step to the transform of the current step, because we finished
@@ -423,6 +420,7 @@ void Localization::publish_transforms() {
     localization_transform.transform.rotation.z = q.z();
     localization_transform.transform.rotation.w = q.w();
 
+    // Check if a transform for the current timestamp was already published
     if (localization_tf_last_published_time_ != localization_transform.header.stamp) {
       // Do not resend a transform for the same timestamp
       localization_tf_last_published_time_ = localization_transform.header.stamp;
@@ -446,6 +444,7 @@ void Localization::publish_transforms() {
 
     RCLCPP_DEBUG(this->get_logger(), "Transform %s", geometry_msgs::msg::to_yaml(map_odom_transform).c_str());
 
+    // Check if a transform for the current timestamp was already published
     if (map_odom_tf_last_published_time_ != map_odom_transform.header.stamp) {
       // Do not resend a transform for the same timestamp
       map_odom_tf_last_published_time_ = map_odom_transform.header.stamp;