diff --git a/.vscode/settings.json b/.vscode/settings.json index b44c44a42..be0dde9f6 100644 --- a/.vscode/settings.json +++ b/.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 diff --git a/bitbots_navigation/bitbots_localization/config/config.yaml b/bitbots_navigation/bitbots_localization/config/config.yaml index fe5e63ebc..13e135e94 100644 --- a/bitbots_navigation/bitbots_localization/config/config.yaml +++ b/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: diff --git a/bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp b/bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp index 18eef6400..2519bf53c 100644 --- a/bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp +++ b/bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp @@ -214,9 +214,6 @@ class Localization { std::shared_ptr lines_; std::shared_ptr 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_; diff --git a/bitbots_navigation/bitbots_localization/src/localization.cpp b/bitbots_navigation/bitbots_localization/src/localization.cpp index a6d3c4f2f..708fdf0d4 100644 --- a/bitbots_navigation/bitbots_localization/src/localization.cpp +++ b/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 req, std::shared_ptr res) { - observe_ = !req->paused; + if (req->paused) { + publishing_timer_->cancel(); + } else { + publishing_timer_->reset(); + } res->success = true; return true; } diff --git a/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/actions/initialize.py b/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/actions/initialize.py index 94a436ca9..29b401efa 100644 --- a/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/actions/initialize.py +++ b/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() diff --git a/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/decisions/walk.py b/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/decisions/walk.py new file mode 100644 index 000000000..acc3b05bf --- /dev/null +++ b/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" diff --git a/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/localization.dsd b/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/localization.dsd index f45d73bbb..3bca21850 100644 --- a/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/localization.dsd +++ b/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 diff --git a/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/localization_blackboard.py b/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/localization_blackboard.py index 429c3c6f3..5e9341c92 100644 --- a/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/localization_blackboard.py +++ b/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] diff --git a/bitbots_navigation/bitbots_odometry/launch/odometry.launch b/bitbots_navigation/bitbots_odometry/launch/odometry.launch index f30772b74..df13a2816 100644 --- a/bitbots_navigation/bitbots_odometry/launch/odometry.launch +++ b/bitbots_navigation/bitbots_odometry/launch/odometry.launch @@ -13,7 +13,7 @@ - + diff --git a/bitbots_navigation/bitbots_odometry/src/odometry_fuser.cpp b/bitbots_navigation/bitbots_odometry/src/odometry_fuser.cpp index ac47677d8..48d85f40b 100644 --- a/bitbots_navigation/bitbots_odometry/src/odometry_fuser.cpp +++ b/bitbots_navigation/bitbots_odometry/src/odometry_fuser.cpp @@ -44,12 +44,8 @@ class OdometryFuser : public rclcpp::Node { public: OdometryFuser() : Node("OdometryFuser"), - odom_to_base_link_(tf2::Transform::getIdentity()), - previous_motion_odom_(tf2::Transform::getIdentity()), - previous_imu_orientation_in_base_link(tf2::Quaternion::getIdentity()), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_, this), - odom_pub_(this->create_publisher("fused_odometry", 1)), support_state_cache_(100), imu_sub_(this, "imu/data"), motion_odom_sub_(this, "motion_odometry"), @@ -84,6 +80,9 @@ class OdometryFuser : public rclcpp::Node { tf2::Transform motion_odometry; tf2::fromMsg(odom_data_.pose.pose, motion_odometry); + // combine orientations to new quaternion if IMU is active, use purely odom otherwise + tf2::Transform fused_odometry{motion_odometry}; + // Check if the imu is active if (imu_data_received_) { // Check if fused_time_ is recent enough @@ -97,13 +96,20 @@ class OdometryFuser : public rclcpp::Node { return; } - // We use the orientation provided by the IMU and merge it with translation from the walking - // (the double integration makes using the linear acceleration of the IMU as translation not feasible) + // get roll an pitch from imu tf2::Quaternion imu_orientation; tf2::fromMsg(imu_data_.orientation, imu_orientation); // compute the point of rotation (in base_link frame) - tf2::Transform rotation_point_in_base_link = getCurrentRotationPoint(); + tf2::Transform rotation_point_in_base = getCurrentRotationPoint(); + // get base_link in rotation point frame + tf2::Transform base_link_in_rotation_point = rotation_point_in_base.inverse(); + + // get only translation and yaw from motion odometry + tf2::Quaternion odom_orientation_yaw = getCurrentMotionOdomYaw(motion_odometry.getRotation()); + tf2::Transform motion_odometry_yaw; + motion_odometry_yaw.setRotation(odom_orientation_yaw); + motion_odometry_yaw.setOrigin(motion_odometry.getOrigin()); // Get the rotation offset between the IMU and the baselink tf2::Transform imu_mounting_offset; @@ -115,54 +121,29 @@ class OdometryFuser : public rclcpp::Node { RCLCPP_ERROR(this->get_logger(), "Not able to fuse IMU data with odometry due to a tf problem: %s", ex.what()); } - // Get imu orientation in base_link frame - tf2::Quaternion imu_orientation_in_base_link = imu_orientation * imu_mounting_offset.getRotation(); - - // Get how far we walked since the last time - tf2::Transform previous_to_current = - tf2::Transform(previous_imu_orientation_in_base_link.inverse() * imu_orientation_in_base_link, - (previous_motion_odom_.inverseTimes(motion_odometry)).getOrigin()); - - // Apply the walked amount to the current state - // Go from odom to base_link, then to the rotation point and apply the movement since the last time there - tf2::Transform odom_to_rotation_point = odom_to_base_link_ = - odom_to_base_link_ * rotation_point_in_base_link * previous_to_current; - - // While we are still in the rotation point frame, we can set the z to 0 as the rotation point is on the ground - odom_to_rotation_point.setOrigin( - {odom_to_rotation_point.getOrigin().x(), odom_to_rotation_point.getOrigin().y(), 0}); - - // Go back to the base_link frame as the odom is defined between odom and base_link - odom_to_base_link_ = odom_to_rotation_point * rotation_point_in_base_link.inverse(); - - // Just to be sure, we set the rotation to the current imu orientation - odom_to_base_link_.setRotation(imu_orientation_in_base_link); - - // Update the previous states - previous_imu_orientation_in_base_link = imu_orientation_in_base_link; - previous_motion_odom_ = motion_odometry; + // get imu transform without yaw + tf2::Quaternion imu_orientation_without_yaw_component = + getCurrentImuRotationWithoutYaw(imu_orientation * imu_mounting_offset.getRotation()); + tf2::Transform imu_without_yaw_component; + imu_without_yaw_component.setRotation(imu_orientation_without_yaw_component); + imu_without_yaw_component.setOrigin({0, 0, 0}); + + // transformation chain to get correctly rotated odom frame + // go to the rotation point in the odom frame. rotate the transform to the base link at this point + fused_odometry = + motion_odometry_yaw * rotation_point_in_base * imu_without_yaw_component * base_link_in_rotation_point; } else { - odom_to_base_link_ = motion_odometry; + fused_odometry = motion_odometry; } - // Combine it all into a tf + // combine it all into a tf tf_.header.stamp = fused_time_; tf_.header.frame_id = odom_frame_; tf_.child_frame_id = base_link_frame_; - tf_.transform = toMsg(odom_to_base_link_); + geometry_msgs::msg::Transform fused_odom_msg; + fused_odom_msg = toMsg(fused_odometry); + tf_.transform = fused_odom_msg; br_->sendTransform(tf_); - - // Publish the odometry message for debugging - nav_msgs::msg::Odometry odom_msg; - odom_msg.header.stamp = fused_time_; - odom_msg.header.frame_id = odom_frame_; - odom_msg.child_frame_id = base_link_frame_; - odom_msg.pose.pose.position.x = odom_to_base_link_.getOrigin().x(); - odom_msg.pose.pose.position.y = odom_to_base_link_.getOrigin().y(); - odom_msg.pose.pose.position.z = odom_to_base_link_.getOrigin().z(); - odom_msg.pose.pose.orientation = toMsg(odom_to_base_link_.getRotation()); - odom_msg.twist = odom_data_.twist; - odom_pub_->publish(odom_msg); } void supportCallback(const biped_interfaces::msg::Phase::SharedPtr msg) { support_state_cache_.add(msg); } @@ -178,9 +159,6 @@ class OdometryFuser : public rclcpp::Node { } private: - tf2::Transform odom_to_base_link_; - tf2::Transform previous_motion_odom_; - tf2::Quaternion previous_imu_orientation_in_base_link; sensor_msgs::msg::Imu imu_data_; nav_msgs::msg::Odometry odom_data_; tf2_ros::Buffer tf_buffer_; @@ -192,8 +170,6 @@ class OdometryFuser : public rclcpp::Node { rclcpp::Subscription::SharedPtr walk_support_state_sub_; rclcpp::Subscription::SharedPtr kick_support_state_sub_; - rclcpp::Publisher::SharedPtr odom_pub_; - message_filters::Cache support_state_cache_; message_filters::Subscriber imu_sub_; @@ -253,7 +229,7 @@ class OdometryFuser : public rclcpp::Node { tf2::Transform getCurrentRotationPoint() { geometry_msgs::msg::TransformStamped rotation_point; - tf2::Transform rotation_point_tf = tf2::Transform::getIdentity(); + tf2::Transform rotation_point_tf; char current_support_state = biped_interfaces::msg::Phase::DOUBLE_STANCE; @@ -298,10 +274,12 @@ class OdometryFuser : public rclcpp::Node { l_to_r_sole_tf.getOrigin().z() / 2}); // Set to zero rotation, because the rotation measurement is done by the imu - l_to_center_tf.setRotation(tf2::Quaternion::getIdentity()); + tf2::Quaternion zero_rotation; + zero_rotation.setRPY(0, 0, 0); + l_to_center_tf.setRotation(zero_rotation); rotation_point_tf = base_to_l_sole_tf * l_to_center_tf; - rotation_point_tf.setRotation(tf2::Quaternion::getIdentity()); + rotation_point_tf.setRotation(zero_rotation); } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); } @@ -309,7 +287,6 @@ class OdometryFuser : public rclcpp::Node { RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 2, "cop not available and unknown support state %c", current_support_state); } - // rotation_point_tf.setRotation(tf2::Quaternion::getIdentity()); return rotation_point_tf; } };