diff --git a/.vscode/settings.json b/.vscode/settings.json index be0dde9f6..b44c44a42 100644 --- a/.vscode/settings.json +++ b/.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 diff --git a/bitbots_navigation/bitbots_localization/config/config.yaml b/bitbots_navigation/bitbots_localization/config/config.yaml index 13e135e94..fe5e63ebc 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: 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: diff --git a/bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp b/bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp index 2519bf53c..18eef6400 100644 --- a/bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp +++ b/bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp @@ -214,6 +214,9 @@ 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 708fdf0d4..a6d3c4f2f 100644 --- a/bitbots_navigation/bitbots_localization/src/localization.cpp +++ b/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 req, std::shared_ptr res) { - if (req->paused) { - publishing_timer_->cancel(); - } else { - publishing_timer_->reset(); - } + observe_ = !req->paused; 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 29b401efa..94a436ca9 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,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() 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 deleted file mode 100644 index acc3b05bf..000000000 --- a/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/decisions/walk.py +++ /dev/null @@ -1,49 +0,0 @@ -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 3bca21850..f45d73bbb 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,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 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 5e9341c92..429c3c6f3 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,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] diff --git a/bitbots_navigation/bitbots_odometry/launch/odometry.launch b/bitbots_navigation/bitbots_odometry/launch/odometry.launch index df13a2816..f30772b74 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 48d85f40b..ac47677d8 100644 --- a/bitbots_navigation/bitbots_odometry/src/odometry_fuser.cpp +++ b/bitbots_navigation/bitbots_odometry/src/odometry_fuser.cpp @@ -44,8 +44,12 @@ 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"), @@ -80,9 +84,6 @@ 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 @@ -96,20 +97,13 @@ class OdometryFuser : public rclcpp::Node { return; } - // get roll an pitch from imu + // 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) 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 = 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()); + tf2::Transform rotation_point_in_base_link = getCurrentRotationPoint(); // Get the rotation offset between the IMU and the baselink tf2::Transform imu_mounting_offset; @@ -121,29 +115,54 @@ 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 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; + // 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; } else { - fused_odometry = motion_odometry; + odom_to_base_link_ = 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_; - geometry_msgs::msg::Transform fused_odom_msg; - fused_odom_msg = toMsg(fused_odometry); - tf_.transform = fused_odom_msg; + tf_.transform = toMsg(odom_to_base_link_); 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); } @@ -159,6 +178,9 @@ 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_; @@ -170,6 +192,8 @@ 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_; @@ -229,7 +253,7 @@ class OdometryFuser : public rclcpp::Node { tf2::Transform getCurrentRotationPoint() { geometry_msgs::msg::TransformStamped rotation_point; - tf2::Transform rotation_point_tf; + tf2::Transform rotation_point_tf = tf2::Transform::getIdentity(); char current_support_state = biped_interfaces::msg::Phase::DOUBLE_STANCE; @@ -274,12 +298,10 @@ 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 - tf2::Quaternion zero_rotation; - zero_rotation.setRPY(0, 0, 0); - l_to_center_tf.setRotation(zero_rotation); + l_to_center_tf.setRotation(tf2::Quaternion::getIdentity()); rotation_point_tf = base_to_l_sole_tf * l_to_center_tf; - rotation_point_tf.setRotation(zero_rotation); + rotation_point_tf.setRotation(tf2::Quaternion::getIdentity()); } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); } @@ -287,6 +309,7 @@ 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; } };