diff --git a/bitbots_localization/src/localization.cpp b/bitbots_localization/src/localization.cpp index e6e8457..376ede9 100644 --- a/bitbots_localization/src/localization.cpp +++ b/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 diff --git a/bitbots_localization/src/parameters.yml b/bitbots_localization/src/parameters.yml index 4476dcc..3facc04 100644 --- a/bitbots_localization/src/parameters.yml +++ b/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: