Revert "Use imu rotation to estimate orientation of odometry"

.vscode/settings.json

@@ -215,8 +215,7 @@
         "future": "cpp",
         "*.ipp": "cpp",
         "span": "cpp",
-        "ranges": "cpp",
-        "core": "cpp"
+        "ranges": "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: 1.5
-      max_motion_angular: 7.0
+      max_motion_linear: 0.5
+      max_motion_angular: 3.14
     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.05
-        rotation_to_rotation: 0.2
+        distance_to_rotation: 0.2
+        rotation_to_rotation: 1.5
         max_rotation: 0.45
         max_translation: 0.09
       weighting:

bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp

@@ -214,9 +214,6 @@ 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,9 +152,7 @@ void Localization::run_filter_one_step() {
   updateParams();
 
   // Set the measurements in the observation model
-  if (observe_) {
-    updateMeasurements();
-  }
+  updateMeasurements();
 
   // Get the odometry offset since the last cycle
   getMotion();
@@ -206,7 +204,11 @@ 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) {
-  observe_ = !req->paused;
+  if (req->paused) {
+    publishing_timer_->cancel();
+  } else {
+    publishing_timer_->reset();
+  }
   res->success = true;
   return true;
 }

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

@@ -1,9 +1,40 @@
+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 InitOpponentHalf(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):
     def perform(self, reevaluate=False):
         self.do_not_reevaluate()
         self.blackboard.node.get_logger().debug("Initializing on the opponent half")
@@ -13,7 +44,7 @@ def perform(self, reevaluate=False):
         return self.pop()
 
 
-class InitOwnHalf(AbstractLocalizationActionElement):
+class InitOwnHalf(AbstractInitialize):
     def perform(self, reevaluate=False):
         self.do_not_reevaluate()
         self.blackboard.node.get_logger().debug("Initializing on our half")
@@ -23,7 +54,7 @@ def perform(self, reevaluate=False):
         return self.pop()
 
 
-class InitPosition(AbstractLocalizationActionElement):
+class InitPosition(AbstractInitialize):
     def perform(self, reevaluate=False):
         self.do_not_reevaluate()
         self.blackboard.node.get_logger().debug("Initializing position")
@@ -48,7 +79,42 @@ def perform(self, reevaluate=False):
         return self.pop()
 
 
-class InitSide(AbstractLocalizationActionElement):
+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):
     def perform(self, reevaluate=False):
         self.do_not_reevaluate()
         self.blackboard.node.get_logger().debug("Initializing on the side lines of our side")
@@ -58,7 +124,7 @@ def perform(self, reevaluate=False):
         return self.pop()
 
 
-class InitGoal(AbstractLocalizationActionElement):
+class InitGoal(AbstractInitialize):
     def perform(self, reevaluate=False):
         self.do_not_reevaluate()
         self.blackboard.node.get_logger().debug("Initializing on the side lines of our side")
@@ -70,7 +136,7 @@ def perform(self, reevaluate=False):
         return self.pop()
 
 
-class InitPenaltyKick(AbstractLocalizationActionElement):
+class InitPenaltyKick(AbstractInitialize):
     def perform(self, reevaluate=False):
         self.do_not_reevaluate()
         self.blackboard.node.get_logger().debug("Initializing behind penalty mark")
@@ -82,3 +148,32 @@ 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

@@ -0,0 +1,49 @@
+import numpy as np
+import tf2_ros as tf2
+from rclpy.time import Time
+
+from bitbots_localization_handler.localization_dsd.decisions import AbstractLocalizationDecisionElement
+
+
+class WalkedSinceLastInit(AbstractLocalizationDecisionElement):
+    """
+    Decides if we walked significantly since our last initialization
+    """
+
+    def __init__(self, blackboard, dsd, parameters):
+        super().__init__(blackboard, dsd, parameters)
+        self.distance_threshold = parameters.get("dist", 0.5)
+
+    def perform(self, reevaluate=False):
+        if not self.blackboard.use_sim_time:
+            # in real life we always have moved and are not teleported
+            return "YES"
+
+        if self.blackboard.last_init_odom_transform is None:
+            return "YES"  # We don't know the last init state so we say that we moved away from it
+
+        try:
+            odom_transform = self.blackboard.tf_buffer.lookup_transform(
+                self.blackboard.odom_frame, self.blackboard.base_footprint_frame, Time()
+            )
+        except (tf2.LookupException, tf2.ConnectivityException, tf2.ExtrapolationException) as e:
+            self.blackboard.node.get_logger().error(
+                f"Reset localization to last init state, because we got up and have no tf: {e}"
+            )
+            # We assume that we didn't walk if the tf lookup fails
+            return "YES"
+
+        walked_distance = np.linalg.norm(
+            np.array([odom_transform.transform.translation.x, odom_transform.transform.translation.y])
+            - np.array(
+                [
+                    self.blackboard.last_init_odom_transform.transform.translation.x,
+                    self.blackboard.last_init_odom_transform.transform.translation.y,
+                ]
+            )
+        )
+
+        if walked_distance < self.distance_threshold:
+            return "NO"
+        else:
+            return "YES"

bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/localization.dsd

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

bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/localization_blackboard.py

@@ -1,12 +1,18 @@
+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
 
@@ -16,6 +22,16 @@ 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"])[
@@ -42,10 +58,17 @@ 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
 
@@ -88,3 +111,14 @@ 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_odometry/launch/odometry.launch

@@ -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"/>