Make min motion detection more consistent and add unit

bitbots_localization/src/localization.cpp

@@ -344,10 +344,17 @@ void Localization::getMotion() {
     rotational_movement_.z =
         tf2::getYaw(transformStampedNow.transform.rotation) - tf2::getYaw(previousOdomTransform_.transform.rotation);
 
+    // Get the time delta between the two transforms
+    double time_delta = rclcpp::Time(transformStampedNow.header.stamp).seconds() -
+                        rclcpp::Time(previousOdomTransform_.header.stamp).seconds();
+
     // Check if robot moved
-    if (linear_movement_.x > config_.misc.min_motion_linear or linear_movement_.y > config_.misc.min_motion_linear or
-        rotational_movement_.z > config_.misc.min_motion_angular) {
-      robot_moved = true;
+    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;
+    } else {
+      RCLCPP_WARN(this->get_logger(), "Time step delta of zero encountered! This should not happen!");
     }
 
     // Set the variable for the transform of the previous step to the transform of the current step, because we finished

bitbots_localization/src/parameters.yml

@@ -12,12 +12,12 @@ bitbots_localization:
         bounds<>: [0, 100]
     min_motion_linear:
       type: double
-      description: "Minimum linear motion which is considered to be a movement. This is relevant if we want to deactivate the filter if no movement is detected"
+      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]
     min_motion_angular:
       type: double
-      description: "Minimum angular motion which is considered to be a movement. This is relevant if we want to deactivate the filter if no movement is detected"
+      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]
     filter_only_with_motion: