Use imu rotation to estimate orientation of odometry (#623)

Author: jaagut

.vscode/settings.json

@@ -215,7 +215,8 @@
         "future": "cpp",
         "*.ipp": "cpp",
         "span": "cpp",
-        "ranges": "cpp"
+        "ranges": "cpp",
+        "core": "cpp"
     },
     // Tell the ROS extension where to find the setup.bash
     // This also utilizes the COLCON_WS environment variable, which needs to be set

bitbots_navigation/bitbots_localization/config/config.yaml

@@ -3,8 +3,8 @@ bitbots_localization:
     misc:
       init_mode: 0
       percentage_best_particles: 100
-      max_motion_linear: 0.5
-      max_motion_angular: 3.14
+      max_motion_linear: 1.5
+      max_motion_angular: 7.0
     ros:
       line_pointcloud_topic: 'line_mask_relative_pc'
       goal_topic: 'goals_simulated'
@@ -26,8 +26,8 @@ bitbots_localization:
         rotation_to_direction: 0.0
         distance_to_distance: 0.1
         rotation_to_distance: 0.2
-        distance_to_rotation: 0.2
-        rotation_to_rotation: 1.5
+        distance_to_rotation: 0.05
+        rotation_to_rotation: 0.2
         max_rotation: 0.45
         max_translation: 0.09
       weighting:

bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp

@@ -214,6 +214,9 @@ class Localization {
   std::shared_ptr<Map> lines_;
   std::shared_ptr<Map> goals_;
 
+  // Flag that enables or disables observations
+  bool observe_ = true;
+
   // RNG that is used for the different sampling steps
   particle_filter::CRandomNumberGenerator random_number_generator_;
 

bitbots_navigation/bitbots_localization/src/localization.cpp

@@ -152,7 +152,9 @@ void Localization::run_filter_one_step() {
   updateParams();
 
   // Set the measurements in the observation model
-  updateMeasurements();
+  if (observe_) {
+    updateMeasurements();
+  }
 
   // Get the odometry offset since the last cycle
   getMotion();
@@ -204,11 +206,7 @@ void Localization::SetInitialPositionCallback(const gm::msg::PoseWithCovarianceS
 
 bool Localization::set_paused_callback(const std::shared_ptr<bl::srv::SetPaused::Request> req,
                                        std::shared_ptr<bl::srv::SetPaused::Response> res) {
-  if (req->paused) {
-    publishing_timer_->cancel();
-  } else {
-    publishing_timer_->reset();
-  }
+  observe_ = !req->paused;
   res->success = true;
   return true;
 }

bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/actions/initialize.py

@@ -1,40 +1,9 @@
-import math
-
-import tf2_ros as tf2
 from bitbots_localization.srv import ResetFilter
-from rclpy.duration import Duration
-from rclpy.time import Time
 
 from bitbots_localization_handler.localization_dsd.actions import AbstractLocalizationActionElement
 
 
-class AbstractInitialize(AbstractLocalizationActionElement):
-    def __init__(self, blackboard, dsd, parameters):
-        super().__init__(blackboard, dsd, parameters)
-
-        # Save the type the instance that called super, so we know what was the last init mode
-        if not isinstance(self, RedoLastInit):
-            self.blackboard.last_init_action_type = self.__class__
-            try:
-                # only need this in simulation
-                if self.blackboard.use_sim_time:
-                    self.blackboard.last_init_odom_transform = self.blackboard.tf_buffer.lookup_transform(
-                        self.blackboard.odom_frame,
-                        self.blackboard.base_footprint_frame,
-                        Time(),
-                        Duration(seconds=1.0),
-                    )  # wait up to 1 second for odom data
-            except (tf2.LookupException, tf2.ConnectivityException, tf2.ExtrapolationException) as e:
-                self.blackboard.node.get_logger().warn(f"Not able to save the odom position due to a tf error: {e}")
-            self.blackboard.node.get_logger().debug(
-                f"Set last init action type to {self.blackboard.last_init_action_type}"
-            )
-
-    def perform(self, reevaluate=False):
-        raise NotImplementedError
-
-
-class InitOpponentHalf(AbstractInitialize):
+class InitOpponentHalf(AbstractLocalizationActionElement):
     def perform(self, reevaluate=False):
         self.do_not_reevaluate()
         self.blackboard.node.get_logger().debug("Initializing on the opponent half")
@@ -44,7 +13,7 @@ def perform(self, reevaluate=False):
         return self.pop()
 
 
-class InitOwnHalf(AbstractInitialize):
+class InitOwnHalf(AbstractLocalizationActionElement):
     def perform(self, reevaluate=False):
         self.do_not_reevaluate()
         self.blackboard.node.get_logger().debug("Initializing on our half")
@@ -54,7 +23,7 @@ def perform(self, reevaluate=False):
         return self.pop()
 
 
-class InitPosition(AbstractInitialize):
+class InitPosition(AbstractLocalizationActionElement):
     def perform(self, reevaluate=False):
         self.do_not_reevaluate()
         self.blackboard.node.get_logger().debug("Initializing position")
@@ -79,42 +48,7 @@ def perform(self, reevaluate=False):
         return self.pop()
 
 
-class InitPoseAfterFall(AbstractInitialize):
-    def perform(self, reevaluate=False):
-        self.do_not_reevaluate()
-        self.blackboard.node.get_logger().debug("Initializing at pose after fall")
-        while not self.blackboard.reset_filter_proxy.wait_for_service(timeout_sec=3.0):
-            self.blackboard.node.get_logger().info("Localization reset service not available, waiting again...")
-
-        # Abort if we don't know the current pose
-        if self.blackboard.robot_pose is None:
-            self.blackboard.node.get_logger().warn(
-                "Can't initialize position, because we don't know the current position"
-            )
-            return self.pop()
-
-        # Calculate the angle of the robot after the fall by adding the yaw difference during the fall
-        # (estimated by the IMU) to the last known localization yaw
-        esimated_angle = (
-            self.blackboard.get_imu_yaw()
-            - self.blackboard.imu_yaw_before_fall
-            + self.blackboard.get_localization_yaw()
-            + math.tau
-        ) % math.tau
-
-        # Tell the localization to reset to the last position and the estimated angle
-        self.blackboard.reset_filter_proxy.call_async(
-            ResetFilter.Request(
-                init_mode=ResetFilter.Request.POSE,
-                x=self.blackboard.robot_pose.pose.pose.position.x,
-                y=self.blackboard.robot_pose.pose.pose.position.y,
-                angle=esimated_angle,
-            )
-        )
-        return self.pop()
-
-
-class InitSide(AbstractInitialize):
+class InitSide(AbstractLocalizationActionElement):
     def perform(self, reevaluate=False):
         self.do_not_reevaluate()
         self.blackboard.node.get_logger().debug("Initializing on the side lines of our side")
@@ -124,7 +58,7 @@ def perform(self, reevaluate=False):
         return self.pop()
 
 
-class InitGoal(AbstractInitialize):
+class InitGoal(AbstractLocalizationActionElement):
     def perform(self, reevaluate=False):
         self.do_not_reevaluate()
         self.blackboard.node.get_logger().debug("Initializing on the side lines of our side")
@@ -136,7 +70,7 @@ def perform(self, reevaluate=False):
         return self.pop()
 
 
-class InitPenaltyKick(AbstractInitialize):
+class InitPenaltyKick(AbstractLocalizationActionElement):
     def perform(self, reevaluate=False):
         self.do_not_reevaluate()
         self.blackboard.node.get_logger().debug("Initializing behind penalty mark")
@@ -148,32 +82,3 @@ def perform(self, reevaluate=False):
             )
         )
         return self.pop()
-
-
-class RedoLastInit(AbstractInitialize):
-    """
-    Executes an action with the type of the last action
-    """
-
-    def __init__(self, blackboard, dsd, parameters):
-        super().__init__(blackboard, dsd, parameters)
-        if self.blackboard.last_init_action_type is None:
-            raise ValueError("There is no last init action to redo")
-
-        # Creates an instance of the last init action
-        self.sub_action: AbstractInitialize = self.blackboard.last_init_action_type(blackboard, dsd, parameters)
-
-    def perform(self, reevaluate=False):
-        self.blackboard.node.get_logger().debug("Redoing the last init")
-        return self.sub_action.perform(reevaluate)
-
-
-class StoreCurrentIMUYaw(AbstractLocalizationActionElement):
-    def perform(self, reevaluate=False):
-        self.do_not_reevaluate()
-        self.blackboard.node.get_logger().debug("Storing current IMU yaw")
-        # Get yaw component of the current orientation before the fall
-        # It drifts over time, so this is not an absolute measurement, but it will help to overcome
-        # The short time during the fall at which we have no odometry
-        self.blackboard.imu_yaw_before_fall = self.blackboard.get_imu_yaw()
-        return self.pop()

bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/decisions/walk.py

@@ -9,10 +9,8 @@ $SecondaryStateDecider
 
 -->Localization
 $GettingUpState
-    YES --> @StoreCurrentIMUYaw, @LocalizationStop, @DoNothing
-    GOTUP --> $WalkedSinceLastInit + dist:%walking_moved_distance
-        YES --> @InitPoseAfterFall, @LocalizationStart, @DoNothing
-        NO --> @RedoLastInit, @LocalizationStart, @DoNothing
+    YES --> @LocalizationStop, @DoNothing
+    GOTUP --> @LocalizationStart, @DoNothing
     NO --> $CheckGameStateReceived
         NO_GAMESTATE_INIT --> @InitSide, @DoNothing
         DO_NOTHING --> @DoNothing

bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/localization.dsd

@@ -1,18 +1,12 @@
-from typing import Optional, Type
-
 import numpy as np
-import tf2_ros as tf2
 from bitbots_blackboard.capsules.game_status_capsule import GameStatusCapsule
 from bitbots_localization.srv import ResetFilter, SetPaused
-from bitbots_utils.transforms import quat2euler, xyzw2wxyz
 from bitbots_utils.utils import get_parameters_from_other_node
 from geometry_msgs.msg import PoseWithCovarianceStamped, Quaternion
 from jaxtyping import Float
-from rclpy.duration import Duration
 from rclpy.node import Node
 from ros2_numpy import numpify
 from sensor_msgs.msg import Imu
-from tf2_geometry_msgs import TransformStamped
 
 from bitbots_msgs.msg import RobotControlState
 
@@ -22,16 +16,6 @@ def __init__(self, node: Node):
         self.node = node
 
         self.initialized = False
-        self.use_sim_time = self.node.get_parameter("use_sim_time").value
-
-        # we only need tf in simulation. don't use it otherwise to safe performance
-        if self.use_sim_time:
-            self.tf_buffer = tf2.Buffer(cache_time=Duration(seconds=10))
-            self.tf_listener = tf2.TransformListener(self.tf_buffer, node)
-
-        # Get names of relevant frames
-        self.odom_frame: str = node.get_parameter("odom_frame").value
-        self.base_footprint_frame: str = node.get_parameter("base_footprint_frame").value
 
         # Get the length of the field
         self.field_length = get_parameters_from_other_node(self.node, "parameter_blackboard", ["field.size.x"])[
@@ -58,17 +42,10 @@ def __init__(self, node: Node):
         self.accel: Float[np.ndarray, "3"] = np.array([0.0, 0.0, 0.0])
         self.imu_orientation = Quaternion(w=1.0)
 
-        # Falling odometry / imu interpolation during falls
-        self.imu_yaw_before_fall: float = 0.0
-
         # Picked up
         self.pickup_accel_buffer: list[Float[np.ndarray, "3"]] = []
         self.pickup_accel_buffer_long: list[Float[np.ndarray, "3"]] = []
 
-        # Last init action
-        self.last_init_action_type: Optional[Type] = None
-        self.last_init_odom_transform: TransformStamped | None = None
-
     def _callback_pose(self, msg: PoseWithCovarianceStamped):
         self.robot_pose = msg
 
@@ -111,14 +88,3 @@ def picked_up(self) -> bool:
         absolute_diff = abs(mean_long - mean)
 
         return bool(absolute_diff > 1.0)
-
-    def get_imu_yaw(self) -> float:
-        """Returns the current yaw of the IMU (this is not an absolute measurement!!! It drifts over time!)"""
-
-        return quat2euler(xyzw2wxyz(numpify(self.imu_orientation)), axes="szxy")[0]
-
-    def get_localization_yaw(self) -> float:
-        """Returns the current yaw of the robot according to the localization. 0 is the x-axis of the field."""
-        if self.robot_pose is None:
-            return 0.0
-        return quat2euler(xyzw2wxyz(numpify(self.robot_pose.pose.pose.orientation)), axes="szxy")[0]

bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/localization_blackboard.py

@@ -13,7 +13,7 @@
         <param name="odom_frame" value="$(var tf_prefix)odom"/>
         <param name="use_sim_time" value="$(var sim)"/>
     </node>
-
+    
     <node name="odometry_fuser" pkg="bitbots_odometry" exec="odometry_fuser" launch-prefix="$(var taskset)">
         <param name="base_link_frame" value="$(var tf_prefix)base_link"/>
         <param name="r_sole_frame" value="$(var tf_prefix)r_sole"/>