Account for measurement delay in localization (#650)

Author: val-ba

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/ball_kick_area.py

@@ -25,6 +25,9 @@ def perform(self, reevaluate=False):
         ball_position = self.blackboard.world_model.get_ball_position_uv()
 
         self.publish_debug_data("ball_position", {"u": ball_position[0], "v": ball_position[1]})
+        self.publish_debug_data(
+            "smoothing_close", f"{self.no_near_decisions} ({self.no_near_decisions / self.smoothing * 10}%)"
+        )
 
         # Check if the ball is in the enter area
         if 0 <= ball_position[0] <= self.kick_x_enter and 0 <= abs(ball_position[1]) <= self.kick_y_enter:

bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml

@@ -146,7 +146,7 @@ body_behavior:
     dribble_ball_distance_threshold: 0.5
     dribble_kick_angle: 0.6
 
-    kick_decision_smoothing: 5.0
+    kick_decision_smoothing: 1.0
 
     ##################
     # costmap params #

bitbots_navigation/bitbots_localization/config/config.yaml

@@ -3,15 +3,11 @@ bitbots_localization:
     misc:
       init_mode: 0
       percentage_best_particles: 100
-      min_motion_linear: 0.0
-      min_motion_angular: 0.0
       max_motion_linear: 0.5
       max_motion_angular: 3.14
-      filter_only_with_motion: false
     ros:
       line_pointcloud_topic: 'line_mask_relative_pc'
       goal_topic: 'goals_simulated'
-      fieldboundary_topic: 'field_boundary_relative'
       particle_publishing_topic: 'pose_particles'
       debug_visualization: true
     particle_filter:
@@ -46,10 +42,6 @@ bitbots_localization:
         goal:
           factor: 0.0
           out_of_field_score: 0.0
-        field_boundary:
-          factor: 0.0
-          out_of_field_score: 0.0
       confidences:
         line_element: 0.01
         goal_element: 0.0
-        field_boundary_element: 0.0

bitbots_navigation/bitbots_localization/include/bitbots_localization/ObservationModel.hpp

@@ -6,17 +6,15 @@
 #define BITBOTS_LOCALIZATION_OBSERVATIONMODEL_H
 
 #include <particle_filter/ParticleFilter.h>
+#include <tf2_ros/buffer.h>
 
 #include <bitbots_localization/RobotState.hpp>
 #include <bitbots_localization/map.hpp>
 #include <bitbots_localization/tools.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp>
-#include <soccer_vision_3d_msgs/msg/field_boundary.hpp>
 #include <soccer_vision_3d_msgs/msg/goalpost.hpp>
 #include <soccer_vision_3d_msgs/msg/goalpost_array.hpp>
-#include <soccer_vision_3d_msgs/msg/marking_array.hpp>
-#include <soccer_vision_3d_msgs/msg/marking_intersection.hpp>
 
 #include "localization_parameters.hpp"
 
@@ -31,8 +29,7 @@ class RobotPoseObservationModel : public particle_filter::ObservationModel<Robot
    * empty
    */
   RobotPoseObservationModel(std::shared_ptr<Map> map_lines, std::shared_ptr<Map> map_goals,
-                            std::shared_ptr<Map> map_field_boundary, const bitbots_localization::Params &config,
-                            const FieldDimensions &field_dimensions);
+                            const bitbots_localization::Params &config, const FieldDimensions &field_dimensions);
 
   /**
    *
@@ -45,36 +42,36 @@ class RobotPoseObservationModel : public particle_filter::ObservationModel<Robot
 
   void set_measurement_goalposts(sv3dm::msg::GoalpostArray measurement);
 
-  void set_measurement_field_boundary(sv3dm::msg::FieldBoundary measurement);
+  const std::vector<std::pair<double, double>> get_measurement_lines() const;
 
-  void set_measurement_markings(sv3dm::msg::MarkingArray measurement);
-
-  std::vector<std::pair<double, double>> get_measurement_lines() const;
-
-  std::vector<std::pair<double, double>> get_measurement_goals() const;
-
-  std::vector<std::pair<double, double>> get_measurement_field_boundary() const;
+  const std::vector<std::pair<double, double>> get_measurement_goals() const;
 
   double get_min_weight() const override;
 
   void clear_measurement();
 
   bool measurements_available() override;
 
+  void set_movement_since_line_measurement(const tf2::Transform movement);
+  void set_movement_since_goal_measurement(const tf2::Transform movement);
+
  private:
   double calculate_weight_for_class(const RobotState &state,
                                     const std::vector<std::pair<double, double>> &last_measurement,
-                                    std::shared_ptr<Map> map, double element_weight) const;
+                                    std::shared_ptr<Map> map, double element_weight,
+                                    const tf2::Transform &movement_since_measurement) 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_;
+
+  // Movement since last measurement
+  tf2::Transform movement_since_line_measurement_ = tf2::Transform::getIdentity();
+  tf2::Transform movement_since_goal_measurement_ = tf2::Transform::getIdentity();
 
   // 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_;

bitbots_navigation/bitbots_localization/include/bitbots_localization/RobotState.hpp

@@ -7,6 +7,8 @@
 
 #include <particle_filter/ParticleFilter.h>
 #include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Transform.h>
+#include <tf2/utils.h>
 
 #include <Eigen/Core>
 #include <bitbots_localization/tools.hpp>
@@ -25,10 +27,17 @@ class RobotState {
   /**
    * @param x Position of the robot.
    * @param y Position of the robot.
-   * @param T Orientaion of the robot in radians.
+   * @param T Orientation of the robot in radians.
    */
   RobotState(double x, double y, double T);
 
+  /**
+   * @brief Constructor for a robot state based on a tf2::Transform.
+   *
+   * @param transform Transform of the robots base_footprint in the map frame.
+   */
+  explicit RobotState(tf2::Transform transform);
+
   RobotState operator*(float factor) const;
 
   RobotState &operator+=(const RobotState &other);
@@ -63,6 +72,8 @@ class RobotState {
   visualization_msgs::msg::Marker renderMarker(std::string n_space, std::string frame, rclcpp::Duration lifetime,
                                                std_msgs::msg::ColorRGBA color, rclcpp::Time stamp) const;
 
+  tf2::Transform getTransform() const;
+
  private:
   double m_XPos;
   double m_YPos;

bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp

@@ -47,7 +47,6 @@
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
-#include <soccer_vision_3d_msgs/msg/field_boundary.hpp>
 #include <soccer_vision_3d_msgs/msg/goalpost_array.hpp>
 #include <std_msgs/msg/color_rgba.hpp>
 #include <std_srvs/srv/trigger.hpp>
@@ -110,12 +109,6 @@ class Localization {
    */
   void GoalPostsCallback(const sv3dm::msg::GoalpostArray &msg);  // TODO
 
-  /**
-   * Callback for the relative field boundary measurements
-   * @param msg Message containing the field boundary points.
-   */
-  void FieldboundaryCallback(const sv3dm::msg::FieldBoundary &msg);
-
   /**
    * Resets the state distribution of the state space
    * @param distribution The type of the distribution
@@ -158,17 +151,13 @@ class Localization {
   // 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_;
   rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr line_ratings_publisher_;
   rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr goal_ratings_publisher_;
-  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr fieldboundary_ratings_publisher_;
   rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr field_publisher_;
 
   // Declare services
@@ -179,8 +168,8 @@ class Localization {
   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::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
   std::shared_ptr<tf2_ros::TransformBroadcaster> br;
 
   // Declare particle filter components
@@ -201,7 +190,6 @@ class Localization {
   // Declare input message buffers
   sm::msg::PointCloud2 line_pointcloud_relative_;
   sv3dm::msg::GoalpostArray goal_posts_relative_;
-  sv3dm::msg::FieldBoundary fieldboundary_relative_;
 
   // Declare time stamps
   rclcpp::Time last_stamp_lines = rclcpp::Time(0);
@@ -219,16 +207,12 @@ class Localization {
   // 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_;
 
   // RNG that is used for the different sampling steps
   particle_filter::CRandomNumberGenerator random_number_generator_;
@@ -271,8 +255,8 @@ class Localization {
    * @param map map for this class
    * @param publisher ros publisher for the type visualization_msgs::msg::Marker
    */
-  void publish_debug_rating(std::vector<std::pair<double, double>> measurements, double scale, const char name[24],
-                            std::shared_ptr<Map> map,
+  void publish_debug_rating(const std::vector<std::pair<double, double>> &measurements, double scale,
+                            const char name[24], std::shared_ptr<Map> map,
                             rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr &publisher);
 
   /**

bitbots_navigation/bitbots_localization/include/bitbots_localization/map.hpp

@@ -45,8 +45,8 @@ class Map {
 
   double get_occupancy(double x, double y);
 
-  std::pair<double, double> observationRelative(std::pair<double, double> observation, double stateX, double stateY,
-                                                double stateT);
+  std::pair<double, double> getObservationCoordinatesInMapFrame(std::pair<double, double> observation, double stateX,
+                                                                double stateY, double stateT);
 
   nav_msgs::msg::OccupancyGrid get_map_msg(std::string frame_id, int threshold = -1);
 

bitbots_navigation/bitbots_localization/src/ObservationModel.cpp

@@ -7,24 +7,25 @@
 namespace bitbots_localization {
 
 RobotPoseObservationModel::RobotPoseObservationModel(std::shared_ptr<Map> map_lines, std::shared_ptr<Map> map_goals,
-                                                     std::shared_ptr<Map> map_field_boundary,
                                                      const bitbots_localization::Params &config,
                                                      const FieldDimensions &field_dimensions)
     : particle_filter::ObservationModel<RobotState>(),
       map_lines_(map_lines),
       map_goals_(map_goals),
-      map_field_boundary_(map_field_boundary),
       config_(config),
       field_dimensions_(field_dimensions) {
   particle_filter::ObservationModel<RobotState>::accumulate_weights_ = true;
 }
 
 double RobotPoseObservationModel::calculate_weight_for_class(
     const RobotState &state, const std::vector<std::pair<double, double>> &last_measurement, std::shared_ptr<Map> map,
-    double element_weight) const {
+    double element_weight, const tf2::Transform &movement_since_measurement) const {
   double particle_weight_for_class;
   if (!last_measurement.empty()) {
-    std::vector<double> ratings = map->Map::provideRating(state, last_measurement);
+    // Subtract (reverse) the movement from our state to get the hypothetical state at the time of the measurement
+    const RobotState state_at_measurement(state.getTransform() * movement_since_measurement.inverse());
+    // Get the ratings for for all points in the measurement based on the map
+    const std::vector<double> ratings = map->Map::provideRating(state_at_measurement, last_measurement);
     // Take the average of the ratings (functional)
     particle_weight_for_class = std::pow(std::accumulate(ratings.begin(), ratings.end(), 0.0) / ratings.size(), 2);
   } else {
@@ -34,22 +35,19 @@ double RobotPoseObservationModel::calculate_weight_for_class(
 }
 
 double RobotPoseObservationModel::measure(const RobotState &state) const {
-  double particle_weight_lines = calculate_weight_for_class(state, last_measurement_lines_, map_lines_,
-                                                            config_.particle_filter.confidences.line_element);
-  double particle_weight_goal = calculate_weight_for_class(state, last_measurement_goal_, map_goals_,
-                                                           config_.particle_filter.confidences.goal_element);
-  double particle_weight_field_boundary =
-      calculate_weight_for_class(state, last_measurement_field_boundary_, map_field_boundary_,
-                                 config_.particle_filter.confidences.field_boundary_element);
+  double particle_weight_lines =
+      calculate_weight_for_class(state, last_measurement_lines_, map_lines_,
+                                 config_.particle_filter.confidences.line_element, movement_since_line_measurement_);
+  double particle_weight_goal =
+      calculate_weight_for_class(state, last_measurement_goal_, map_goals_,
+                                 config_.particle_filter.confidences.goal_element, movement_since_goal_measurement_);
 
   // Get relevant config values
   auto scoring_config = config_.particle_filter.scoring;
 
   // Calculate weight for the particle
   double weight = (((1 - scoring_config.lines.factor) + scoring_config.lines.factor * particle_weight_lines) *
-                   ((1 - scoring_config.goal.factor) + scoring_config.goal.factor * particle_weight_goal) *
-                   ((1 - scoring_config.field_boundary.factor) +
-                    scoring_config.field_boundary.factor * particle_weight_field_boundary));
+                   ((1 - scoring_config.goal.factor) + scoring_config.goal.factor * particle_weight_goal));
 
   if (weight < config_.particle_filter.weighting.min_weight) {
     weight = config_.particle_filter.weighting.min_weight;
@@ -68,6 +66,7 @@ double RobotPoseObservationModel::measure(const RobotState &state) const {
 }
 
 void RobotPoseObservationModel::set_measurement_lines_pc(sm::msg::PointCloud2 measurement) {
+  // convert to polar
   for (sm::PointCloud2ConstIterator<float> iter_xyz(measurement, "x"); iter_xyz != iter_xyz.end(); ++iter_xyz) {
     std::pair<double, double> linePolar = cartesianToPolar(iter_xyz[0], iter_xyz[1]);
     last_measurement_lines_.push_back(linePolar);
@@ -82,39 +81,33 @@ void RobotPoseObservationModel::set_measurement_goalposts(sv3dm::msg::GoalpostAr
   }
 }
 
-void RobotPoseObservationModel::set_measurement_field_boundary(sv3dm::msg::FieldBoundary measurement) {
-  // convert to polar
-  for (gm::msg::Point &point : measurement.points) {
-    std::pair<double, double> fieldBoundaryPointPolar = cartesianToPolar(point.x, point.y);
-    last_measurement_field_boundary_.push_back(fieldBoundaryPointPolar);
-  }
-}
-
-std::vector<std::pair<double, double>> RobotPoseObservationModel::get_measurement_lines() const {
+const std::vector<std::pair<double, double>> RobotPoseObservationModel::get_measurement_lines() const {
   return last_measurement_lines_;
 }
 
-std::vector<std::pair<double, double>> RobotPoseObservationModel::get_measurement_goals() const {
+const std::vector<std::pair<double, double>> RobotPoseObservationModel::get_measurement_goals() const {
   return last_measurement_goal_;
 }
 
-std::vector<std::pair<double, double>> RobotPoseObservationModel::get_measurement_field_boundary() const {
-  return last_measurement_field_boundary_;
-}
-
 double RobotPoseObservationModel::get_min_weight() const { return config_.particle_filter.weighting.min_weight; }
 
 void RobotPoseObservationModel::clear_measurement() {
   last_measurement_lines_.clear();
   last_measurement_goal_.clear();
-  last_measurement_field_boundary_.clear();
 }
 
 bool RobotPoseObservationModel::measurements_available() {
   bool available = false;
   available |= !last_measurement_lines_.empty();
   available |= !last_measurement_goal_.empty();
-  available |= !last_measurement_field_boundary_.empty();
   return available;
 }
+
+void RobotPoseObservationModel::set_movement_since_line_measurement(const tf2::Transform movement) {
+  movement_since_line_measurement_ = movement;
+}
+
+void RobotPoseObservationModel::set_movement_since_goal_measurement(const tf2::Transform movement) {
+  movement_since_goal_measurement_ = movement;
+}
 }  // namespace bitbots_localization