Limit unrealistically large motion updates (#415)

bitbots_navigation/bitbots_localization/config/config.yaml

@@ -5,6 +5,8 @@ bitbots_localization:
       percentage_best_particles: 50
       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'

bitbots_navigation/bitbots_localization/src/localization.cpp

@@ -344,13 +344,30 @@ void Localization::getMotion() {
     double time_delta = rclcpp::Time(transformStampedNow.header.stamp).seconds() -
                         rclcpp::Time(previousOdomTransform_.header.stamp).seconds();
 
-    // Check if robot moved
+    // Check if we already applied the motion for this time step
     if (time_delta > 0) {
-      robot_moved = linear_movement_.x / time_delta >= config_.misc.min_motion_linear or
-                    linear_movement_.y / time_delta >= config_.misc.min_motion_linear or
-                    rotational_movement_.z / time_delta >= config_.misc.min_motion_angular;
+      // Calculate normalized motion (motion per second)
+      auto linear_movement_normalized_x = linear_movement_.x / time_delta;
+      auto linear_movement_normalized_y = linear_movement_.y / time_delta;
+      auto rotational_movement_normalized_z = rotational_movement_.z / time_delta;
+
+      // Check if the robot moved an unreasonable amount and drop the motion if it did
+      if (std::abs(linear_movement_normalized_x) > config_.misc.max_motion_linear or
+          std::abs(linear_movement_normalized_y) > config_.misc.max_motion_linear or
+          std::abs(std::remainder(rotational_movement_normalized_z, 2 * M_PI)) > config_.misc.max_motion_angular) {
+        rotational_movement_.z = 0;
+        linear_movement_.x = 0;
+        linear_movement_.y = 0;
+        RCLCPP_WARN(this->get_logger(), "Robot moved an unreasonable amount, dropping motion.");
+      }
+
+      // Check if robot moved
+      robot_moved = linear_movement_normalized_x >= config_.misc.min_motion_linear or
+                    linear_movement_normalized_y >= config_.misc.min_motion_linear or
+                    rotational_movement_normalized_z >= config_.misc.min_motion_angular;
     } else {
-      RCLCPP_WARN(this->get_logger(), "Time step delta of zero encountered! This should not happen!");
+      RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
+                           "Time step delta of zero encountered! Odometry is unavailable.");
       robot_moved = false;
     }
 

bitbots_navigation/bitbots_localization/src/parameters.yml

@@ -1,6 +1,6 @@
 bitbots_localization:
   misc:
-    init_mode: 
+    init_mode:
       type: int
       description: "Initialization mode for the filters particle distribution"
       validation:
@@ -14,12 +14,22 @@ bitbots_localization:
       type: double
       description: "Minimum linear motion (m/s) which is considered to be a movement. This is relevant if we want to deactivate the filter if no movement is detected"
       validation:
-        bounds<>: [0.0, 1.0]
+        gt_eq<>: [0.0]
+    max_motion_linear:
+      type: double
+      description: "Maximum linear motion (m/s) which is considered to be a movement. Updates larger than this are deemed unrealistic and are ignored"
+      validation:
+        gt_eq<>: [0.0]
     min_motion_angular:
       type: double
       description: "Minimum angular motion (rad/s) which is considered to be a movement. This is relevant if we want to deactivate the filter if no movement is detected"
       validation:
-        bounds<>: [0.0, 1.0]
+        gt_eq<>: [0.0]
+    max_motion_angular:
+      type: double
+      description: "Maximum angular motion (rad/s) which is considered to be a movement. Updates larger than this are deemed unrealistic and are ignored"
+      validation:
+        gt_eq<>: [0.0]
     filter_only_with_motion:
       type: bool
       description: "If true, the filter is only active if a movement is detected"
@@ -171,7 +181,7 @@ bitbots_localization:
         validation:
           bounds<>: [-5.0, 5.0]
     scoring:
-      lines: 
+      lines:
         factor:
           type: double
           description: "Weighing how much the line information is considered for the scoring of a particle"