From 848ba775ea046496f8239fbc083e8a26331c0dc0 Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Fri, 15 Nov 2024 13:04:22 +0100 Subject: [PATCH 01/13] add imu subcriber and imu rotation transform computation --- .../bitbots_odometry/motion_odometry.hpp | 7 +++++ .../bitbots_odometry/src/motion_odometry.cpp | 27 ++++++++++++++++++- 2 files changed, 33 insertions(+), 1 deletion(-) diff --git a/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/motion_odometry.hpp b/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/motion_odometry.hpp index b734adb8b..8972df947 100644 --- a/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/motion_odometry.hpp +++ b/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/motion_odometry.hpp @@ -6,9 +6,11 @@ #include #include +#include #include #include #include +#include #include #include #include @@ -36,6 +38,7 @@ class MotionOdometry : public rclcpp::Node { rclcpp::Subscription::SharedPtr walk_support_state_sub_; rclcpp::Subscription::SharedPtr kick_support_state_sub_; rclcpp::Subscription::SharedPtr odom_subscriber_; + rclcpp::Subscription::SharedPtr imu_subscriber_; // Declare parameter listener and struct from the generate_parameter_library motion_odometry::ParamListener param_listener_; @@ -44,6 +47,7 @@ class MotionOdometry : public rclcpp::Node { void supportCallback(const biped_interfaces::msg::Phase::SharedPtr msg); void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg); + void IMUCallback(const sensor_msgs::msg::Imu::SharedPtr msg); tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -52,6 +56,9 @@ class MotionOdometry : public rclcpp::Node { std::string previous_support_link_; std::string current_support_link_; rclcpp::Time start_time_; + + geometry_msgs::msg::QuaternionStamped current_imu_orientation_; + geometry_msgs::msg::QuaternionStamped previous_imu_orientation_inverse_; }; } // namespace bitbots_odometry diff --git a/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp b/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp index d6c04f288..5510f0207 100644 --- a/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp +++ b/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp @@ -25,6 +25,8 @@ MotionOdometry::MotionOdometry() "dynamic_kick_support_state", 1, std::bind(&MotionOdometry::supportCallback, this, _1)); odom_subscriber_ = this->create_subscription( "walk_engine_odometry", 1, std::bind(&MotionOdometry::odomCallback, this, _1)); + imu_subscriber_ = this->create_subscription("imu/data", 1, + std::bind(&MotionOdometry::IMUCallback, this, _1)); pub_odometry_ = this->create_publisher("motion_odometry", 1); @@ -58,6 +60,8 @@ void MotionOdometry::loop() { current_support_link_ = l_sole_frame_; } + geometry_msgs::msg::QuaternionStamped current_imu_orientation = + current_imu_orientation_; // TODO: time stamp fitting to step taken try { // add the transform between previous and current support link to the odometry transform. // we wait a bit for the transform as the joint messages are maybe a bit behind @@ -78,9 +82,20 @@ void MotionOdometry::loop() { double yaw = tf2::getYaw(previous_to_current_support.getRotation()) * config_.yaw_scaling; previous_to_current_support.setOrigin({x, y, 0}); tf2::Quaternion q; - q.setRPY(0, 0, yaw); + // rotation = current * inverse(previous) + tf2::Quaternion curr_imu_rot_quat; + tf2::Quaternion prev_imu_rot_quat_inv; + tf2::fromMsg(current_imu_orientation.quaternion, curr_imu_rot_quat); + tf2::fromMsg(previous_imu_orientation_inverse_.quaternion, prev_imu_rot_quat_inv); + tf2::Transform imu_to_support_foot; + geometry_msgs::msg::TransformStamped imu_to_support_foot_msg = + tf_buffer_.lookupTransform("imu_frame", current_support_link_, foot_change_time_); + fromMsg(imu_to_support_foot_msg.transform, imu_to_support_foot); + q = curr_imu_rot_quat * prev_imu_rot_quat_inv * imu_to_support_foot.getRotation(); + previous_to_current_support.setRotation(q); odometry_to_support_foot_ = odometry_to_support_foot_ * previous_to_current_support; + } catch (tf2::TransformException &ex) { RCLCPP_WARN(this->get_logger(), "%s", ex.what()); rclcpp::sleep_for(std::chrono::milliseconds(1000)); @@ -89,6 +104,11 @@ void MotionOdometry::loop() { // update current support link for transform from foot to base link previous_support_link_ = current_support_link_; + tf2::Quaternion rotation; + tf2::fromMsg(current_imu_orientation.quaternion, rotation); + rotation.setW(-1.0); + rotation.normalize(); + previous_imu_orientation_inverse_.quaternion = tf2::toMsg(rotation); // remember the support state change but skip the double support phase if (current_support_state_ != biped_interfaces::msg::Phase::DOUBLE_STANCE) { @@ -177,6 +197,11 @@ void MotionOdometry::supportCallback(const biped_interfaces::msg::Phase::SharedP void MotionOdometry::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { current_odom_msg_ = *msg; } +void MotionOdometry::IMUCallback(const sensor_msgs::msg::Imu::SharedPtr msg) { + current_imu_orientation_.quaternion = msg->orientation; + current_imu_orientation_.header = msg->header; +} + } // namespace bitbots_odometry int main(int argc, char **argv) { From 1aab5353b6156568539917037fc7ec7d264fc77b Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Fri, 15 Nov 2024 13:35:02 +0100 Subject: [PATCH 02/13] use imu orientation in base to sole transform instead --- .../bitbots_odometry/motion_odometry.hpp | 3 ++ .../bitbots_odometry/src/motion_odometry.cpp | 47 +++++++++---------- 2 files changed, 26 insertions(+), 24 deletions(-) diff --git a/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/motion_odometry.hpp b/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/motion_odometry.hpp index 8972df947..2e64d4729 100644 --- a/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/motion_odometry.hpp +++ b/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/motion_odometry.hpp @@ -59,6 +59,9 @@ class MotionOdometry : public rclcpp::Node { geometry_msgs::msg::QuaternionStamped current_imu_orientation_; geometry_msgs::msg::QuaternionStamped previous_imu_orientation_inverse_; + geometry_msgs::msg::QuaternionStamped initial_imu_transform_; + + bool initial_transform_set_ = false; }; } // namespace bitbots_odometry diff --git a/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp b/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp index 5510f0207..f496074e1 100644 --- a/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp +++ b/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp @@ -60,8 +60,6 @@ void MotionOdometry::loop() { current_support_link_ = l_sole_frame_; } - geometry_msgs::msg::QuaternionStamped current_imu_orientation = - current_imu_orientation_; // TODO: time stamp fitting to step taken try { // add the transform between previous and current support link to the odometry transform. // we wait a bit for the transform as the joint messages are maybe a bit behind @@ -79,21 +77,8 @@ void MotionOdometry::loop() { x = x * config_.x_backward_scaling; } double y = previous_to_current_support.getOrigin().y() * config_.y_scaling; - double yaw = tf2::getYaw(previous_to_current_support.getRotation()) * config_.yaw_scaling; previous_to_current_support.setOrigin({x, y, 0}); - tf2::Quaternion q; - // rotation = current * inverse(previous) - tf2::Quaternion curr_imu_rot_quat; - tf2::Quaternion prev_imu_rot_quat_inv; - tf2::fromMsg(current_imu_orientation.quaternion, curr_imu_rot_quat); - tf2::fromMsg(previous_imu_orientation_inverse_.quaternion, prev_imu_rot_quat_inv); - tf2::Transform imu_to_support_foot; - geometry_msgs::msg::TransformStamped imu_to_support_foot_msg = - tf_buffer_.lookupTransform("imu_frame", current_support_link_, foot_change_time_); - fromMsg(imu_to_support_foot_msg.transform, imu_to_support_foot); - q = curr_imu_rot_quat * prev_imu_rot_quat_inv * imu_to_support_foot.getRotation(); - - previous_to_current_support.setRotation(q); + odometry_to_support_foot_ = odometry_to_support_foot_ * previous_to_current_support; } catch (tf2::TransformException &ex) { @@ -104,11 +89,6 @@ void MotionOdometry::loop() { // update current support link for transform from foot to base link previous_support_link_ = current_support_link_; - tf2::Quaternion rotation; - tf2::fromMsg(current_imu_orientation.quaternion, rotation); - rotation.setW(-1.0); - rotation.normalize(); - previous_imu_orientation_inverse_.quaternion = tf2::toMsg(rotation); // remember the support state change but skip the double support phase if (current_support_state_ != biped_interfaces::msg::Phase::DOUBLE_STANCE) { @@ -129,10 +109,20 @@ void MotionOdometry::loop() { x = x * config_.x_backward_scaling; } double y = current_support_to_base.getOrigin().y() * config_.y_scaling; - double yaw = tf2::getYaw(current_support_to_base.getRotation()) * config_.yaw_scaling; - current_support_to_base.setOrigin({x, y, current_support_to_base.getOrigin().z()}); + tf2::Quaternion q; - q.setRPY(0, 0, yaw); + + tf2::Quaternion curr_imu_rot_quat; + tf2::Quaternion initial_imu_transform_quat; + tf2::fromMsg(current_imu_orientation_.quaternion, curr_imu_rot_quat); + tf2::fromMsg(initial_imu_transform_.quaternion, initial_imu_transform_quat); + tf2::Transform imu_to_base; + geometry_msgs::msg::TransformStamped imu_to_base_msg = + tf_buffer_.lookupTransform("imu_frame", base_link_frame_, rclcpp::Time(0, 0, RCL_ROS_TIME)); + fromMsg(imu_to_base_msg.transform, imu_to_base); + q = curr_imu_rot_quat * initial_imu_transform_quat * imu_to_base.getRotation(); + + current_support_to_base.setOrigin({x, y, current_support_to_base.getOrigin().z()}); current_support_to_base.setRotation(q); tf2::Transform odom_to_base_link = odometry_to_support_foot_ * current_support_to_base; @@ -200,6 +190,15 @@ void MotionOdometry::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) void MotionOdometry::IMUCallback(const sensor_msgs::msg::Imu::SharedPtr msg) { current_imu_orientation_.quaternion = msg->orientation; current_imu_orientation_.header = msg->header; + if (initial_transform_set_ == false) { + initial_transform_set_ = true; + initial_imu_transform_ = current_imu_orientation_; + tf2::Quaternion rotation; + tf2::fromMsg(initial_imu_transform_.quaternion, rotation); + rotation.setW(-1.0); + rotation.normalize(); + initial_imu_transform_.quaternion = tf2::toMsg(rotation); + } } } // namespace bitbots_odometry From 53fa166ea77b353f1dd2626da213ab4ec4aafbd3 Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Fri, 15 Nov 2024 13:52:50 +0100 Subject: [PATCH 03/13] change type of rotations --- .../bitbots_odometry/motion_odometry.hpp | 9 +++--- .../bitbots_odometry/src/motion_odometry.cpp | 28 ++++++++----------- 2 files changed, 15 insertions(+), 22 deletions(-) diff --git a/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/motion_odometry.hpp b/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/motion_odometry.hpp index 2e64d4729..122a2550c 100644 --- a/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/motion_odometry.hpp +++ b/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/motion_odometry.hpp @@ -33,7 +33,7 @@ class MotionOdometry : public rclcpp::Node { rclcpp::Time current_support_state_time_{rclcpp::Time(0, 0, RCL_ROS_TIME)}; nav_msgs::msg::Odometry current_odom_msg_; tf2::Transform odometry_to_support_foot_; - std::string base_link_frame_, r_sole_frame_, l_sole_frame_, odom_frame_; + std::string base_link_frame_, r_sole_frame_, l_sole_frame_, odom_frame_, imu_frame_; rclcpp::Publisher::SharedPtr pub_odometry_; rclcpp::Subscription::SharedPtr walk_support_state_sub_; rclcpp::Subscription::SharedPtr kick_support_state_sub_; @@ -57,11 +57,10 @@ class MotionOdometry : public rclcpp::Node { std::string current_support_link_; rclcpp::Time start_time_; - geometry_msgs::msg::QuaternionStamped current_imu_orientation_; - geometry_msgs::msg::QuaternionStamped previous_imu_orientation_inverse_; - geometry_msgs::msg::QuaternionStamped initial_imu_transform_; + tf2::Quaternion current_imu_orientation_; + tf2::Quaternion initial_imu_transform_; - bool initial_transform_set_ = false; + bool is_initial_transform_set_ = false; }; } // namespace bitbots_odometry diff --git a/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp b/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp index f496074e1..feac15d3e 100644 --- a/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp +++ b/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp @@ -18,6 +18,8 @@ MotionOdometry::MotionOdometry() this->get_parameter("l_sole_frame", l_sole_frame_); this->declare_parameter("odom_frame", "odom"); this->get_parameter("odom_frame", odom_frame_); + this->declare_parameter("imu_frame", "imu_frame"); + this->get_parameter("imu_frame", imu_frame_); walk_support_state_sub_ = this->create_subscription( "walk_support_state", 1, std::bind(&MotionOdometry::supportCallback, this, _1)); @@ -100,6 +102,9 @@ void MotionOdometry::loop() { try { geometry_msgs::msg::TransformStamped current_support_to_base_msg = tf_buffer_.lookupTransform(previous_support_link_, base_link_frame_, rclcpp::Time(0, 0, RCL_ROS_TIME)); + geometry_msgs::msg::TransformStamped imu_to_base_msg = + tf_buffer_.lookupTransform(imu_frame_, base_link_frame_, rclcpp::Time(0, 0, RCL_ROS_TIME)); + tf2::Transform current_support_to_base; tf2::fromMsg(current_support_to_base_msg.transform, current_support_to_base); double x = current_support_to_base.getOrigin().x(); @@ -111,16 +116,9 @@ void MotionOdometry::loop() { double y = current_support_to_base.getOrigin().y() * config_.y_scaling; tf2::Quaternion q; - - tf2::Quaternion curr_imu_rot_quat; - tf2::Quaternion initial_imu_transform_quat; - tf2::fromMsg(current_imu_orientation_.quaternion, curr_imu_rot_quat); - tf2::fromMsg(initial_imu_transform_.quaternion, initial_imu_transform_quat); tf2::Transform imu_to_base; - geometry_msgs::msg::TransformStamped imu_to_base_msg = - tf_buffer_.lookupTransform("imu_frame", base_link_frame_, rclcpp::Time(0, 0, RCL_ROS_TIME)); fromMsg(imu_to_base_msg.transform, imu_to_base); - q = curr_imu_rot_quat * initial_imu_transform_quat * imu_to_base.getRotation(); + q = current_imu_orientation_ * initial_imu_transform_ * imu_to_base.getRotation(); current_support_to_base.setOrigin({x, y, current_support_to_base.getOrigin().z()}); current_support_to_base.setRotation(q); @@ -188,16 +186,12 @@ void MotionOdometry::supportCallback(const biped_interfaces::msg::Phase::SharedP void MotionOdometry::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { current_odom_msg_ = *msg; } void MotionOdometry::IMUCallback(const sensor_msgs::msg::Imu::SharedPtr msg) { - current_imu_orientation_.quaternion = msg->orientation; - current_imu_orientation_.header = msg->header; - if (initial_transform_set_ == false) { - initial_transform_set_ = true; + tf2::fromMsg(msg->orientation, current_imu_orientation_); + if (is_initial_transform_set_ == false) { + is_initial_transform_set_ = true; initial_imu_transform_ = current_imu_orientation_; - tf2::Quaternion rotation; - tf2::fromMsg(initial_imu_transform_.quaternion, rotation); - rotation.setW(-1.0); - rotation.normalize(); - initial_imu_transform_.quaternion = tf2::toMsg(rotation); + initial_imu_transform_.setW(-1.0); + initial_imu_transform_.normalize(); } } From 960b1eba45be90a795dadcc35ae71588767169f3 Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Fri, 13 Dec 2024 13:33:44 +0100 Subject: [PATCH 04/13] set yaw to zero for step to step odom --- bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp b/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp index feac15d3e..e6365b85a 100644 --- a/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp +++ b/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp @@ -80,6 +80,7 @@ void MotionOdometry::loop() { } double y = previous_to_current_support.getOrigin().y() * config_.y_scaling; previous_to_current_support.setOrigin({x, y, 0}); + previous_to_current_support.setRotation(tf2::Quaternion(0, 0, 0, 1)); odometry_to_support_foot_ = odometry_to_support_foot_ * previous_to_current_support; From 085e191a10a888ebb6c81bfd1c38d409a3a0a5b6 Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Fri, 20 Dec 2024 13:01:29 +0100 Subject: [PATCH 05/13] prepare for change --- .../bitbots_odometry/src/motion_odometry.cpp | 30 ++++++++++++------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp b/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp index e6365b85a..36f5a8d84 100644 --- a/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp +++ b/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp @@ -79,8 +79,12 @@ void MotionOdometry::loop() { x = x * config_.x_backward_scaling; } double y = previous_to_current_support.getOrigin().y() * config_.y_scaling; + double yaw = tf2::getYaw(previous_to_current_support.getRotation()) * config_.yaw_scaling; previous_to_current_support.setOrigin({x, y, 0}); - previous_to_current_support.setRotation(tf2::Quaternion(0, 0, 0, 1)); + + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + previous_to_current_support.setRotation(q); odometry_to_support_foot_ = odometry_to_support_foot_ * previous_to_current_support; @@ -103,8 +107,8 @@ void MotionOdometry::loop() { try { geometry_msgs::msg::TransformStamped current_support_to_base_msg = tf_buffer_.lookupTransform(previous_support_link_, base_link_frame_, rclcpp::Time(0, 0, RCL_ROS_TIME)); - geometry_msgs::msg::TransformStamped imu_to_base_msg = - tf_buffer_.lookupTransform(imu_frame_, base_link_frame_, rclcpp::Time(0, 0, RCL_ROS_TIME)); + geometry_msgs::msg::TransformStamped imu_to_previous_support_msg = + tf_buffer_.lookupTransform(previous_support_link_, imu_frame_, rclcpp::Time(0, 0, RCL_ROS_TIME)); tf2::Transform current_support_to_base; tf2::fromMsg(current_support_to_base_msg.transform, current_support_to_base); @@ -115,16 +119,22 @@ void MotionOdometry::loop() { x = x * config_.x_backward_scaling; } double y = current_support_to_base.getOrigin().y() * config_.y_scaling; - - tf2::Quaternion q; - tf2::Transform imu_to_base; - fromMsg(imu_to_base_msg.transform, imu_to_base); - q = current_imu_orientation_ * initial_imu_transform_ * imu_to_base.getRotation(); - + double yaw = tf2::getYaw(current_support_to_base.getRotation()) * config_.yaw_scaling; current_support_to_base.setOrigin({x, y, current_support_to_base.getOrigin().z()}); + tf2::Quaternion q; + q.setRPY(0, 0, yaw); current_support_to_base.setRotation(q); - tf2::Transform odom_to_base_link = odometry_to_support_foot_ * current_support_to_base; + tf2::Quaternion imu_rotation; + tf2::Transform imu_to_previous_support; + fromMsg(imu_to_previous_support_msg.transform, imu_to_previous_support); + + imu_rotation = current_imu_orientation_ * imu_to_previous_support.getRotation(); // * initial_imu_transform_ * + + tf2::Transform odometry_to_support_foot_with_imu_yaw = odometry_to_support_foot_; + odometry_to_support_foot_with_imu_yaw.setRotation(imu_rotation); + + tf2::Transform odom_to_base_link = odometry_to_support_foot_with_imu_yaw * current_support_to_base; geometry_msgs::msg::TransformStamped odom_to_base_link_msg = geometry_msgs::msg::TransformStamped(); odom_to_base_link_msg.transform = tf2::toMsg(odom_to_base_link); odom_to_base_link_msg.header.stamp = current_support_to_base_msg.header.stamp; From d74472e4a9990e6d09b3c93742cd3311296b97ac Mon Sep 17 00:00:00 2001 From: Eamad Date: Fri, 20 Dec 2024 13:40:48 +0100 Subject: [PATCH 06/13] add rotation from imu --- .../bitbots_odometry/src/motion_odometry.cpp | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp b/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp index 36f5a8d84..3a3e5f0e3 100644 --- a/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp +++ b/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp @@ -107,7 +107,7 @@ void MotionOdometry::loop() { try { geometry_msgs::msg::TransformStamped current_support_to_base_msg = tf_buffer_.lookupTransform(previous_support_link_, base_link_frame_, rclcpp::Time(0, 0, RCL_ROS_TIME)); - geometry_msgs::msg::TransformStamped imu_to_previous_support_msg = + geometry_msgs::msg::TransformStamped imu_to_previous_support_msg = // TODO: rename to previous_support_to_imu_msg tf_buffer_.lookupTransform(previous_support_link_, imu_frame_, rclcpp::Time(0, 0, RCL_ROS_TIME)); tf2::Transform current_support_to_base; @@ -125,16 +125,21 @@ void MotionOdometry::loop() { q.setRPY(0, 0, yaw); current_support_to_base.setRotation(q); - tf2::Quaternion imu_rotation; - tf2::Transform imu_to_previous_support; + tf2::Transform imu_rotation; + tf2::Transform imu_to_previous_support; // TODO: rename to previous_support_to_imu fromMsg(imu_to_previous_support_msg.transform, imu_to_previous_support); - imu_rotation = current_imu_orientation_ * imu_to_previous_support.getRotation(); // * initial_imu_transform_ * + - tf2::Transform odometry_to_support_foot_with_imu_yaw = odometry_to_support_foot_; - odometry_to_support_foot_with_imu_yaw.setRotation(imu_rotation); + double imu_yaw = tf2::getYaw(imu_to_previous_support.getRotation()); + tf2::Quaternion imu_q; //TODO: anders benennen + imu_q.setRPY(0,0,imu_yaw); + imu_rotation.setRotation(imu_q); + tf2::Transform odometry_to_support_foot_with_imu_yaw = odometry_to_support_foot_; // todo: rename to without yaw + tf2::Quaternion no_rot; + odometry_to_support_foot_with_imu_yaw.setRotation(no_rot); - tf2::Transform odom_to_base_link = odometry_to_support_foot_with_imu_yaw * current_support_to_base; + tf2::Transform odom_to_base_link = odometry_to_support_foot_with_imu_yaw * imu_rotation * current_support_to_base; geometry_msgs::msg::TransformStamped odom_to_base_link_msg = geometry_msgs::msg::TransformStamped(); odom_to_base_link_msg.transform = tf2::toMsg(odom_to_base_link); odom_to_base_link_msg.header.stamp = current_support_to_base_msg.header.stamp; From 91f06b24523df17b86e6e9725c3277ef12dc78fc Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Fri, 24 Jan 2025 10:14:24 +0100 Subject: [PATCH 07/13] revert changes of motion odometry --- .../bitbots_odometry/motion_odometry.hpp | 11 +----- .../bitbots_odometry/src/motion_odometry.cpp | 36 +------------------ 2 files changed, 2 insertions(+), 45 deletions(-) diff --git a/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/motion_odometry.hpp b/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/motion_odometry.hpp index 122a2550c..b734adb8b 100644 --- a/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/motion_odometry.hpp +++ b/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/motion_odometry.hpp @@ -6,11 +6,9 @@ #include #include -#include #include #include #include -#include #include #include #include @@ -33,12 +31,11 @@ class MotionOdometry : public rclcpp::Node { rclcpp::Time current_support_state_time_{rclcpp::Time(0, 0, RCL_ROS_TIME)}; nav_msgs::msg::Odometry current_odom_msg_; tf2::Transform odometry_to_support_foot_; - std::string base_link_frame_, r_sole_frame_, l_sole_frame_, odom_frame_, imu_frame_; + std::string base_link_frame_, r_sole_frame_, l_sole_frame_, odom_frame_; rclcpp::Publisher::SharedPtr pub_odometry_; rclcpp::Subscription::SharedPtr walk_support_state_sub_; rclcpp::Subscription::SharedPtr kick_support_state_sub_; rclcpp::Subscription::SharedPtr odom_subscriber_; - rclcpp::Subscription::SharedPtr imu_subscriber_; // Declare parameter listener and struct from the generate_parameter_library motion_odometry::ParamListener param_listener_; @@ -47,7 +44,6 @@ class MotionOdometry : public rclcpp::Node { void supportCallback(const biped_interfaces::msg::Phase::SharedPtr msg); void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg); - void IMUCallback(const sensor_msgs::msg::Imu::SharedPtr msg); tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -56,11 +52,6 @@ class MotionOdometry : public rclcpp::Node { std::string previous_support_link_; std::string current_support_link_; rclcpp::Time start_time_; - - tf2::Quaternion current_imu_orientation_; - tf2::Quaternion initial_imu_transform_; - - bool is_initial_transform_set_ = false; }; } // namespace bitbots_odometry diff --git a/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp b/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp index 3a3e5f0e3..d6c04f288 100644 --- a/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp +++ b/bitbots_navigation/bitbots_odometry/src/motion_odometry.cpp @@ -18,8 +18,6 @@ MotionOdometry::MotionOdometry() this->get_parameter("l_sole_frame", l_sole_frame_); this->declare_parameter("odom_frame", "odom"); this->get_parameter("odom_frame", odom_frame_); - this->declare_parameter("imu_frame", "imu_frame"); - this->get_parameter("imu_frame", imu_frame_); walk_support_state_sub_ = this->create_subscription( "walk_support_state", 1, std::bind(&MotionOdometry::supportCallback, this, _1)); @@ -27,8 +25,6 @@ MotionOdometry::MotionOdometry() "dynamic_kick_support_state", 1, std::bind(&MotionOdometry::supportCallback, this, _1)); odom_subscriber_ = this->create_subscription( "walk_engine_odometry", 1, std::bind(&MotionOdometry::odomCallback, this, _1)); - imu_subscriber_ = this->create_subscription("imu/data", 1, - std::bind(&MotionOdometry::IMUCallback, this, _1)); pub_odometry_ = this->create_publisher("motion_odometry", 1); @@ -81,13 +77,10 @@ void MotionOdometry::loop() { double y = previous_to_current_support.getOrigin().y() * config_.y_scaling; double yaw = tf2::getYaw(previous_to_current_support.getRotation()) * config_.yaw_scaling; previous_to_current_support.setOrigin({x, y, 0}); - tf2::Quaternion q; q.setRPY(0, 0, yaw); previous_to_current_support.setRotation(q); - odometry_to_support_foot_ = odometry_to_support_foot_ * previous_to_current_support; - } catch (tf2::TransformException &ex) { RCLCPP_WARN(this->get_logger(), "%s", ex.what()); rclcpp::sleep_for(std::chrono::milliseconds(1000)); @@ -107,9 +100,6 @@ void MotionOdometry::loop() { try { geometry_msgs::msg::TransformStamped current_support_to_base_msg = tf_buffer_.lookupTransform(previous_support_link_, base_link_frame_, rclcpp::Time(0, 0, RCL_ROS_TIME)); - geometry_msgs::msg::TransformStamped imu_to_previous_support_msg = // TODO: rename to previous_support_to_imu_msg - tf_buffer_.lookupTransform(previous_support_link_, imu_frame_, rclcpp::Time(0, 0, RCL_ROS_TIME)); - tf2::Transform current_support_to_base; tf2::fromMsg(current_support_to_base_msg.transform, current_support_to_base); double x = current_support_to_base.getOrigin().x(); @@ -125,21 +115,7 @@ void MotionOdometry::loop() { q.setRPY(0, 0, yaw); current_support_to_base.setRotation(q); - tf2::Transform imu_rotation; - tf2::Transform imu_to_previous_support; // TODO: rename to previous_support_to_imu - fromMsg(imu_to_previous_support_msg.transform, imu_to_previous_support); - - - - double imu_yaw = tf2::getYaw(imu_to_previous_support.getRotation()); - tf2::Quaternion imu_q; //TODO: anders benennen - imu_q.setRPY(0,0,imu_yaw); - imu_rotation.setRotation(imu_q); - tf2::Transform odometry_to_support_foot_with_imu_yaw = odometry_to_support_foot_; // todo: rename to without yaw - tf2::Quaternion no_rot; - odometry_to_support_foot_with_imu_yaw.setRotation(no_rot); - - tf2::Transform odom_to_base_link = odometry_to_support_foot_with_imu_yaw * imu_rotation * current_support_to_base; + tf2::Transform odom_to_base_link = odometry_to_support_foot_ * current_support_to_base; geometry_msgs::msg::TransformStamped odom_to_base_link_msg = geometry_msgs::msg::TransformStamped(); odom_to_base_link_msg.transform = tf2::toMsg(odom_to_base_link); odom_to_base_link_msg.header.stamp = current_support_to_base_msg.header.stamp; @@ -201,16 +177,6 @@ void MotionOdometry::supportCallback(const biped_interfaces::msg::Phase::SharedP void MotionOdometry::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { current_odom_msg_ = *msg; } -void MotionOdometry::IMUCallback(const sensor_msgs::msg::Imu::SharedPtr msg) { - tf2::fromMsg(msg->orientation, current_imu_orientation_); - if (is_initial_transform_set_ == false) { - is_initial_transform_set_ = true; - initial_imu_transform_ = current_imu_orientation_; - initial_imu_transform_.setW(-1.0); - initial_imu_transform_.normalize(); - } -} - } // namespace bitbots_odometry int main(int argc, char **argv) { From ed0dc0e9b2a9a093eca79676456f9eb6377bd7f1 Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Fri, 24 Jan 2025 11:21:44 +0100 Subject: [PATCH 08/13] add a fused odometry node --- .../bitbots_odometry/CMakeLists.txt | 21 +++ .../bitbots_odometry/fused_odometry.hpp | 49 ++++++ .../bitbots_odometry/src/fused_odometry.cpp | 158 ++++++++++++++++++ 3 files changed, 228 insertions(+) create mode 100644 bitbots_navigation/bitbots_odometry/include/bitbots_odometry/fused_odometry.hpp create mode 100644 bitbots_navigation/bitbots_odometry/src/fused_odometry.cpp diff --git a/bitbots_navigation/bitbots_odometry/CMakeLists.txt b/bitbots_navigation/bitbots_odometry/CMakeLists.txt index 9ff8c552c..c5706ef03 100644 --- a/bitbots_navigation/bitbots_odometry/CMakeLists.txt +++ b/bitbots_navigation/bitbots_odometry/CMakeLists.txt @@ -32,6 +32,7 @@ add_compile_options(-Wall -Werror -Wno-unused) add_executable(odometry_fuser src/odometry_fuser.cpp) add_executable(motion_odometry src/motion_odometry.cpp) +add_executable(fused_odometry src/fused_odometry.cpp) target_link_libraries(motion_odometry rclcpp::rclcpp odometry_parameters) @@ -72,12 +73,32 @@ ament_target_dependencies( tf2_geometry_msgs tf2_ros) +ament_target_dependencies( + fused_odometry + ament_cmake + biped_interfaces + bitbots_docs + bitbots_utils + Eigen3 + geometry_msgs + message_filters + nav_msgs + rclcpp + rot_conv + sensor_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros) + enable_bitbots_docs() install(TARGETS motion_odometry DESTINATION lib/${PROJECT_NAME}) install(TARGETS odometry_fuser DESTINATION lib/${PROJECT_NAME}) +install(TARGETS fused_odometry DESTINATION lib/${PROJECT_NAME}) + install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) diff --git a/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/fused_odometry.hpp b/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/fused_odometry.hpp new file mode 100644 index 000000000..e34ea26a5 --- /dev/null +++ b/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/fused_odometry.hpp @@ -0,0 +1,49 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using std::placeholders::_1; + +namespace bitbots_odometry { + +class FusedOdometry : public rclcpp::Node { + public: + FusedOdometry(); + void loop(); + + private: + nav_msgs::msg::Odometry prev_odom_msg_; + nav_msgs::msg::Odometry current_odom_msg_; + sensor_msgs::msg::Imu prev_imu_msg_; + sensor_msgs::msg::Imu current_imu_msg_; + tf2::Transform odom_to_base_; + std::string base_link_frame_, odom_frame_, imu_frame_; + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr imu_sub_; + + void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg); + void IMUCallback(const sensor_msgs::msg::Imu::SharedPtr msg); + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + std::unique_ptr br_; + + bool first_imu_msg_received_ = false; + bool first_odom_msg_received_ = false; +}; + +} // namespace bitbots_odometry diff --git a/bitbots_navigation/bitbots_odometry/src/fused_odometry.cpp b/bitbots_navigation/bitbots_odometry/src/fused_odometry.cpp new file mode 100644 index 000000000..8c7261a2b --- /dev/null +++ b/bitbots_navigation/bitbots_odometry/src/fused_odometry.cpp @@ -0,0 +1,158 @@ +#include + +namespace bitbots_odometry { + +FusedOdometry::FusedOdometry() + : Node("FusedOdometry"), + odom_to_base_( + tf2::Transform::getIdentity()), // TODO maybe identity is not completly correct here, because z-axis + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_, this), + br_(std::make_unique(this)) { + this->declare_parameter("base_link_frame", "base_link"); + this->get_parameter("base_link_frame", base_link_frame_); + this->declare_parameter("odom_frame", "odom"); + this->get_parameter("odom_frame", odom_frame_); + this->declare_parameter("imu_frame", "imu_frame"); + this->get_parameter("imu_frame", imu_frame_); + + odom_sub_ = this->create_subscription("motion_odometry", 1, + std::bind(&FusedOdometry::odomCallback, this, _1)); + imu_sub_ = + this->create_subscription("imu/data", 1, std::bind(&FusedOdometry::IMUCallback, this, _1)); + odom_pub_ = this->create_publisher("fused_odometry", 1); +} + +void FusedOdometry::loop() { + // Wait for tf to be available + bitbots_utils::wait_for_tf(this->get_logger(), this->get_clock(), &tf_buffer_, {imu_frame_}, base_link_frame_); + + try { + if (!first_odom_msg_received_ || !first_imu_msg_received_) { + return; + } + + nav_msgs::msg::Odometry current_odom_msg = current_odom_msg_; + + tf2::Transform previous_motion_odom = tf2::Transform(); + tf2::Transform current_motion_odom = tf2::Transform(); + + previous_motion_odom.setOrigin(tf2::Vector3( + prev_odom_msg_.pose.pose.position.x, prev_odom_msg_.pose.pose.position.y, prev_odom_msg_.pose.pose.position.z)); + previous_motion_odom.setRotation( + tf2::Quaternion(prev_odom_msg_.pose.pose.orientation.x, prev_odom_msg_.pose.pose.orientation.y, + prev_odom_msg_.pose.pose.orientation.z, prev_odom_msg_.pose.pose.orientation.w)); + current_motion_odom.setOrigin(tf2::Vector3(current_odom_msg.pose.pose.position.x, + current_odom_msg.pose.pose.position.y, + current_odom_msg.pose.pose.position.z)); + current_motion_odom.setRotation( + tf2::Quaternion(current_odom_msg.pose.pose.orientation.x, current_odom_msg.pose.pose.orientation.y, + current_odom_msg.pose.pose.orientation.z, current_odom_msg.pose.pose.orientation.w)); + + tf2::Transform previous_to_current_motion_odom = tf2::Transform(); + previous_to_current_motion_odom = previous_motion_odom.inverseTimes(current_motion_odom); + + sensor_msgs::msg::Imu current_imu_msg = current_imu_msg_; + + tf2::Transform previous_imu = tf2::Transform(); + tf2::Transform current_imu = tf2::Transform(); + previous_imu.setOrigin( + tf2::Vector3(prev_imu_msg_.orientation.x, prev_imu_msg_.orientation.y, prev_imu_msg_.orientation.z)); + previous_imu.setRotation(tf2::Quaternion(prev_imu_msg_.orientation.x, prev_imu_msg_.orientation.y, + prev_imu_msg_.orientation.z, prev_imu_msg_.orientation.w)); + current_imu.setOrigin( + tf2::Vector3(current_imu_msg.orientation.x, current_imu_msg.orientation.y, current_imu_msg.orientation.z)); + current_imu.setRotation(tf2::Quaternion(current_imu_msg.orientation.x, current_imu_msg.orientation.y, + current_imu_msg.orientation.z, current_imu_msg.orientation.w)); + + tf2::Transform imu_mounting_offset; + try { + geometry_msgs::msg::TransformStamped imu_mounting_transform = + tf_buffer_.lookupTransform(current_imu_msg.header.frame_id, base_link_frame_, current_imu_msg.header.stamp); + fromMsg(imu_mounting_transform.transform, imu_mounting_offset); + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "Not able to fuse IMU data with odometry due to a tf problem: %s", ex.what()); + } + + tf2::Transform prev_imu_mounting_offset; + try { + geometry_msgs::msg::TransformStamped prev_imu_mounting_transform = + tf_buffer_.lookupTransform(prev_imu_msg_.header.frame_id, base_link_frame_, current_imu_msg_.header.stamp); + fromMsg(prev_imu_mounting_transform.transform, prev_imu_mounting_offset); + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "Not able to fuse IMU data with odometry due to a tf problem: %s", ex.what()); + } + + tf2::Transform previous_to_current_imu = tf2::Transform(); + previous_to_current_imu = (previous_imu * prev_imu_mounting_offset).inverseTimes(current_imu * imu_mounting_offset); + + tf2::Transform prev_to_curr_odom = tf2::Transform(); + double yaw = tf2::getYaw(previous_to_current_imu.getRotation()); + prev_to_curr_odom.setOrigin( + {previous_to_current_motion_odom.getOrigin().x(), previous_to_current_motion_odom.getOrigin().y(), 0}); + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + prev_to_curr_odom.setRotation(q); + + odom_to_base_ = odom_to_base_ * prev_to_curr_odom; + + prev_odom_msg_ = current_odom_msg; + prev_imu_msg_ = current_imu_msg; + + geometry_msgs::msg::TransformStamped odom_to_base_link_msg = geometry_msgs::msg::TransformStamped(); + odom_to_base_link_msg.transform = tf2::toMsg(odom_to_base_); + odom_to_base_link_msg.header.stamp = current_odom_msg.header.stamp; // TODO: maybe use right now as stamp + odom_to_base_link_msg.header.frame_id = odom_frame_; + odom_to_base_link_msg.child_frame_id = base_link_frame_; + + // odometry as message + nav_msgs::msg::Odometry odom_msg; + odom_msg.header.stamp = current_odom_msg.header.stamp; // TODO: maybe use right now as stamp + 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_msg.transform.translation.x; + odom_msg.pose.pose.position.y = odom_to_base_link_msg.transform.translation.y; + odom_msg.pose.pose.position.z = odom_to_base_link_msg.transform.translation.z; + odom_msg.pose.pose.orientation = odom_to_base_link_msg.transform.rotation; + odom_msg.twist = current_odom_msg_.twist; + odom_pub_->publish(odom_msg); + + } catch (tf2::TransformException &ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + rclcpp::sleep_for(std::chrono::milliseconds(1000)); + return; + } +} + +void FusedOdometry::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { + if (!first_odom_msg_received_) { + prev_odom_msg_ = *msg; + first_odom_msg_received_ = true; + } + current_odom_msg_ = *msg; +} + +void FusedOdometry::IMUCallback(const sensor_msgs::msg::Imu::SharedPtr msg) { + if (!first_imu_msg_received_) { + prev_imu_msg_ = *msg; + first_imu_msg_received_ = true; + } + current_imu_msg_ = *msg; +} + +} // namespace bitbots_odometry + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + rclcpp::Duration timer_duration = rclcpp::Duration::from_seconds(1.0 / 200.0); + rclcpp::experimental::executors::EventsExecutor exec; + exec.add_node(node); + + rclcpp::TimerBase::SharedPtr timer = + rclcpp::create_timer(node, node->get_clock(), timer_duration, [node]() -> void { node->loop(); }); + + exec.spin(); + rclcpp::shutdown(); +} From a692a82af75c2b8cd68d6f67f5f6c8368e331c0f Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Fri, 24 Jan 2025 12:02:10 +0100 Subject: [PATCH 09/13] use not only yaw for rotation --- bitbots_navigation/bitbots_odometry/src/fused_odometry.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/bitbots_navigation/bitbots_odometry/src/fused_odometry.cpp b/bitbots_navigation/bitbots_odometry/src/fused_odometry.cpp index 8c7261a2b..3f2ea39f5 100644 --- a/bitbots_navigation/bitbots_odometry/src/fused_odometry.cpp +++ b/bitbots_navigation/bitbots_odometry/src/fused_odometry.cpp @@ -87,12 +87,10 @@ void FusedOdometry::loop() { previous_to_current_imu = (previous_imu * prev_imu_mounting_offset).inverseTimes(current_imu * imu_mounting_offset); tf2::Transform prev_to_curr_odom = tf2::Transform(); - double yaw = tf2::getYaw(previous_to_current_imu.getRotation()); prev_to_curr_odom.setOrigin( {previous_to_current_motion_odom.getOrigin().x(), previous_to_current_motion_odom.getOrigin().y(), 0}); - tf2::Quaternion q; - q.setRPY(0, 0, yaw); - prev_to_curr_odom.setRotation(q); + + prev_to_curr_odom.setRotation(previous_to_current_imu.getRotation()); odom_to_base_ = odom_to_base_ * prev_to_curr_odom; From bc8b567cc12ff2224685ca1c7b478ce8f39aed2d Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Fri, 24 Jan 2025 12:06:34 +0100 Subject: [PATCH 10/13] add fused_odometry to odometry.launch --- bitbots_navigation/bitbots_odometry/launch/odometry.launch | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/bitbots_navigation/bitbots_odometry/launch/odometry.launch b/bitbots_navigation/bitbots_odometry/launch/odometry.launch index df13a2816..f973176e4 100644 --- a/bitbots_navigation/bitbots_odometry/launch/odometry.launch +++ b/bitbots_navigation/bitbots_odometry/launch/odometry.launch @@ -14,6 +14,12 @@ + + + + + + From db004ad679c406f82545371497232ce9781e6695 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Sun, 9 Mar 2025 21:42:55 +0100 Subject: [PATCH 11/13] Current state of combining odom fusers --- .../bitbots_odometry/src/odometry_fuser.cpp | 34 +++++++++++-------- 1 file changed, 19 insertions(+), 15 deletions(-) diff --git a/bitbots_navigation/bitbots_odometry/src/odometry_fuser.cpp b/bitbots_navigation/bitbots_odometry/src/odometry_fuser.cpp index 48d85f40b..e6cdfd793 100644 --- a/bitbots_navigation/bitbots_odometry/src/odometry_fuser.cpp +++ b/bitbots_navigation/bitbots_odometry/src/odometry_fuser.cpp @@ -44,6 +44,9 @@ class OdometryFuser : public rclcpp::Node { public: OdometryFuser() : Node("OdometryFuser"), + odom_to_base_(tf2::Transform::getIdentity()), + previous_motion_odom_(tf2::Transform::getIdentity()), + previous_imu_(tf2::Quaternion::getIdentity()), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_, this), support_state_cache_(100), @@ -80,7 +83,7 @@ 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 + // Our output data structure tf2::Transform fused_odometry{motion_odometry}; // Check if the imu is active @@ -96,7 +99,8 @@ 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); @@ -105,11 +109,6 @@ class OdometryFuser : public rclcpp::Node { // 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; @@ -121,17 +120,19 @@ 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}); + // Get imu orientation in base_link frame + tf2::Quaternion imu_orientation_in_base = imu_orientation * imu_mounting_offset.getRotation(); + // Get how far we walked since the last time + tf2::Transform previous_to_current = tf2::Transform( + previous_imu_.inverse() * imu_orientation_in_base, + previous_motion_odom_.inverseTimes(motion_odometry).getOrigin() + ); + + // Apply the walked amount to the current state // 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; + fused_odometry = odom_to_base_ * rotation_point_in_base * previous_to_current * base_link_in_rotation_point; } else { fused_odometry = motion_odometry; } @@ -159,6 +160,9 @@ class OdometryFuser : public rclcpp::Node { } private: + tf2::Transform odom_to_base_; + tf2::Transform previous_motion_odom_; + tf2::Quaternion previous_imu_; sensor_msgs::msg::Imu imu_data_; nav_msgs::msg::Odometry odom_data_; tf2_ros::Buffer tf_buffer_; From cfcd4edd5f2010a49d378570ecea199b0433dac3 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Mon, 10 Mar 2025 17:54:30 +0100 Subject: [PATCH 12/13] Working version of odometry fuser integration --- .vscode/settings.json | 3 +- .../bitbots_odometry/CMakeLists.txt | 21 --- .../bitbots_odometry/fused_odometry.hpp | 49 ------ .../bitbots_odometry/launch/odometry.launch | 8 +- .../bitbots_odometry/src/fused_odometry.cpp | 156 ------------------ .../bitbots_odometry/src/odometry_fuser.cpp | 74 +++++---- 6 files changed, 49 insertions(+), 262 deletions(-) delete mode 100644 bitbots_navigation/bitbots_odometry/include/bitbots_odometry/fused_odometry.hpp delete mode 100644 bitbots_navigation/bitbots_odometry/src/fused_odometry.cpp diff --git a/.vscode/settings.json b/.vscode/settings.json index 7168aec42..378337c33 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_odometry/CMakeLists.txt b/bitbots_navigation/bitbots_odometry/CMakeLists.txt index fc71d9d53..90e8feefe 100644 --- a/bitbots_navigation/bitbots_odometry/CMakeLists.txt +++ b/bitbots_navigation/bitbots_odometry/CMakeLists.txt @@ -38,7 +38,6 @@ add_compile_options(-Wall -Werror -Wno-unused) add_executable(odometry_fuser src/odometry_fuser.cpp) add_executable(motion_odometry src/motion_odometry.cpp) -add_executable(fused_odometry src/fused_odometry.cpp) target_link_libraries(motion_odometry rclcpp::rclcpp odometry_parameters) @@ -81,32 +80,12 @@ ament_target_dependencies( tf2_geometry_msgs tf2_ros) -ament_target_dependencies( - fused_odometry - ament_cmake - biped_interfaces - bitbots_docs - bitbots_utils - Eigen3 - geometry_msgs - message_filters - nav_msgs - rclcpp - rot_conv - sensor_msgs - tf2 - tf2_eigen - tf2_geometry_msgs - tf2_ros) - enable_bitbots_docs() install(TARGETS motion_odometry DESTINATION lib/${PROJECT_NAME}) install(TARGETS odometry_fuser DESTINATION lib/${PROJECT_NAME}) -install(TARGETS fused_odometry DESTINATION lib/${PROJECT_NAME}) - install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) diff --git a/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/fused_odometry.hpp b/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/fused_odometry.hpp deleted file mode 100644 index e34ea26a5..000000000 --- a/bitbots_navigation/bitbots_odometry/include/bitbots_odometry/fused_odometry.hpp +++ /dev/null @@ -1,49 +0,0 @@ -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using std::placeholders::_1; - -namespace bitbots_odometry { - -class FusedOdometry : public rclcpp::Node { - public: - FusedOdometry(); - void loop(); - - private: - nav_msgs::msg::Odometry prev_odom_msg_; - nav_msgs::msg::Odometry current_odom_msg_; - sensor_msgs::msg::Imu prev_imu_msg_; - sensor_msgs::msg::Imu current_imu_msg_; - tf2::Transform odom_to_base_; - std::string base_link_frame_, odom_frame_, imu_frame_; - rclcpp::Publisher::SharedPtr odom_pub_; - rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr imu_sub_; - - void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg); - void IMUCallback(const sensor_msgs::msg::Imu::SharedPtr msg); - - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; - std::unique_ptr br_; - - bool first_imu_msg_received_ = false; - bool first_odom_msg_received_ = false; -}; - -} // namespace bitbots_odometry diff --git a/bitbots_navigation/bitbots_odometry/launch/odometry.launch b/bitbots_navigation/bitbots_odometry/launch/odometry.launch index f973176e4..f30772b74 100644 --- a/bitbots_navigation/bitbots_odometry/launch/odometry.launch +++ b/bitbots_navigation/bitbots_odometry/launch/odometry.launch @@ -13,13 +13,7 @@ - - - - - - - + diff --git a/bitbots_navigation/bitbots_odometry/src/fused_odometry.cpp b/bitbots_navigation/bitbots_odometry/src/fused_odometry.cpp deleted file mode 100644 index 3f2ea39f5..000000000 --- a/bitbots_navigation/bitbots_odometry/src/fused_odometry.cpp +++ /dev/null @@ -1,156 +0,0 @@ -#include - -namespace bitbots_odometry { - -FusedOdometry::FusedOdometry() - : Node("FusedOdometry"), - odom_to_base_( - tf2::Transform::getIdentity()), // TODO maybe identity is not completly correct here, because z-axis - tf_buffer_(this->get_clock()), - tf_listener_(tf_buffer_, this), - br_(std::make_unique(this)) { - this->declare_parameter("base_link_frame", "base_link"); - this->get_parameter("base_link_frame", base_link_frame_); - this->declare_parameter("odom_frame", "odom"); - this->get_parameter("odom_frame", odom_frame_); - this->declare_parameter("imu_frame", "imu_frame"); - this->get_parameter("imu_frame", imu_frame_); - - odom_sub_ = this->create_subscription("motion_odometry", 1, - std::bind(&FusedOdometry::odomCallback, this, _1)); - imu_sub_ = - this->create_subscription("imu/data", 1, std::bind(&FusedOdometry::IMUCallback, this, _1)); - odom_pub_ = this->create_publisher("fused_odometry", 1); -} - -void FusedOdometry::loop() { - // Wait for tf to be available - bitbots_utils::wait_for_tf(this->get_logger(), this->get_clock(), &tf_buffer_, {imu_frame_}, base_link_frame_); - - try { - if (!first_odom_msg_received_ || !first_imu_msg_received_) { - return; - } - - nav_msgs::msg::Odometry current_odom_msg = current_odom_msg_; - - tf2::Transform previous_motion_odom = tf2::Transform(); - tf2::Transform current_motion_odom = tf2::Transform(); - - previous_motion_odom.setOrigin(tf2::Vector3( - prev_odom_msg_.pose.pose.position.x, prev_odom_msg_.pose.pose.position.y, prev_odom_msg_.pose.pose.position.z)); - previous_motion_odom.setRotation( - tf2::Quaternion(prev_odom_msg_.pose.pose.orientation.x, prev_odom_msg_.pose.pose.orientation.y, - prev_odom_msg_.pose.pose.orientation.z, prev_odom_msg_.pose.pose.orientation.w)); - current_motion_odom.setOrigin(tf2::Vector3(current_odom_msg.pose.pose.position.x, - current_odom_msg.pose.pose.position.y, - current_odom_msg.pose.pose.position.z)); - current_motion_odom.setRotation( - tf2::Quaternion(current_odom_msg.pose.pose.orientation.x, current_odom_msg.pose.pose.orientation.y, - current_odom_msg.pose.pose.orientation.z, current_odom_msg.pose.pose.orientation.w)); - - tf2::Transform previous_to_current_motion_odom = tf2::Transform(); - previous_to_current_motion_odom = previous_motion_odom.inverseTimes(current_motion_odom); - - sensor_msgs::msg::Imu current_imu_msg = current_imu_msg_; - - tf2::Transform previous_imu = tf2::Transform(); - tf2::Transform current_imu = tf2::Transform(); - previous_imu.setOrigin( - tf2::Vector3(prev_imu_msg_.orientation.x, prev_imu_msg_.orientation.y, prev_imu_msg_.orientation.z)); - previous_imu.setRotation(tf2::Quaternion(prev_imu_msg_.orientation.x, prev_imu_msg_.orientation.y, - prev_imu_msg_.orientation.z, prev_imu_msg_.orientation.w)); - current_imu.setOrigin( - tf2::Vector3(current_imu_msg.orientation.x, current_imu_msg.orientation.y, current_imu_msg.orientation.z)); - current_imu.setRotation(tf2::Quaternion(current_imu_msg.orientation.x, current_imu_msg.orientation.y, - current_imu_msg.orientation.z, current_imu_msg.orientation.w)); - - tf2::Transform imu_mounting_offset; - try { - geometry_msgs::msg::TransformStamped imu_mounting_transform = - tf_buffer_.lookupTransform(current_imu_msg.header.frame_id, base_link_frame_, current_imu_msg.header.stamp); - fromMsg(imu_mounting_transform.transform, imu_mounting_offset); - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "Not able to fuse IMU data with odometry due to a tf problem: %s", ex.what()); - } - - tf2::Transform prev_imu_mounting_offset; - try { - geometry_msgs::msg::TransformStamped prev_imu_mounting_transform = - tf_buffer_.lookupTransform(prev_imu_msg_.header.frame_id, base_link_frame_, current_imu_msg_.header.stamp); - fromMsg(prev_imu_mounting_transform.transform, prev_imu_mounting_offset); - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "Not able to fuse IMU data with odometry due to a tf problem: %s", ex.what()); - } - - tf2::Transform previous_to_current_imu = tf2::Transform(); - previous_to_current_imu = (previous_imu * prev_imu_mounting_offset).inverseTimes(current_imu * imu_mounting_offset); - - tf2::Transform prev_to_curr_odom = tf2::Transform(); - prev_to_curr_odom.setOrigin( - {previous_to_current_motion_odom.getOrigin().x(), previous_to_current_motion_odom.getOrigin().y(), 0}); - - prev_to_curr_odom.setRotation(previous_to_current_imu.getRotation()); - - odom_to_base_ = odom_to_base_ * prev_to_curr_odom; - - prev_odom_msg_ = current_odom_msg; - prev_imu_msg_ = current_imu_msg; - - geometry_msgs::msg::TransformStamped odom_to_base_link_msg = geometry_msgs::msg::TransformStamped(); - odom_to_base_link_msg.transform = tf2::toMsg(odom_to_base_); - odom_to_base_link_msg.header.stamp = current_odom_msg.header.stamp; // TODO: maybe use right now as stamp - odom_to_base_link_msg.header.frame_id = odom_frame_; - odom_to_base_link_msg.child_frame_id = base_link_frame_; - - // odometry as message - nav_msgs::msg::Odometry odom_msg; - odom_msg.header.stamp = current_odom_msg.header.stamp; // TODO: maybe use right now as stamp - 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_msg.transform.translation.x; - odom_msg.pose.pose.position.y = odom_to_base_link_msg.transform.translation.y; - odom_msg.pose.pose.position.z = odom_to_base_link_msg.transform.translation.z; - odom_msg.pose.pose.orientation = odom_to_base_link_msg.transform.rotation; - odom_msg.twist = current_odom_msg_.twist; - odom_pub_->publish(odom_msg); - - } catch (tf2::TransformException &ex) { - RCLCPP_WARN(this->get_logger(), "%s", ex.what()); - rclcpp::sleep_for(std::chrono::milliseconds(1000)); - return; - } -} - -void FusedOdometry::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { - if (!first_odom_msg_received_) { - prev_odom_msg_ = *msg; - first_odom_msg_received_ = true; - } - current_odom_msg_ = *msg; -} - -void FusedOdometry::IMUCallback(const sensor_msgs::msg::Imu::SharedPtr msg) { - if (!first_imu_msg_received_) { - prev_imu_msg_ = *msg; - first_imu_msg_received_ = true; - } - current_imu_msg_ = *msg; -} - -} // namespace bitbots_odometry - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - auto node = std::make_shared(); - - rclcpp::Duration timer_duration = rclcpp::Duration::from_seconds(1.0 / 200.0); - rclcpp::experimental::executors::EventsExecutor exec; - exec.add_node(node); - - rclcpp::TimerBase::SharedPtr timer = - rclcpp::create_timer(node, node->get_clock(), timer_duration, [node]() -> void { node->loop(); }); - - exec.spin(); - rclcpp::shutdown(); -} diff --git a/bitbots_navigation/bitbots_odometry/src/odometry_fuser.cpp b/bitbots_navigation/bitbots_odometry/src/odometry_fuser.cpp index e6cdfd793..d02c6a4c1 100644 --- a/bitbots_navigation/bitbots_odometry/src/odometry_fuser.cpp +++ b/bitbots_navigation/bitbots_odometry/src/odometry_fuser.cpp @@ -44,11 +44,12 @@ class OdometryFuser : public rclcpp::Node { public: OdometryFuser() : Node("OdometryFuser"), - odom_to_base_(tf2::Transform::getIdentity()), + odom_to_base_link_(tf2::Transform::getIdentity()), previous_motion_odom_(tf2::Transform::getIdentity()), - previous_imu_(tf2::Quaternion::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"), @@ -83,9 +84,6 @@ class OdometryFuser : public rclcpp::Node { tf2::Transform motion_odometry; tf2::fromMsg(odom_data_.pose.pose, motion_odometry); - // Our output data structure - tf2::Transform fused_odometry{motion_odometry}; - // Check if the imu is active if (imu_data_received_) { // Check if fused_time_ is recent enough @@ -105,10 +103,7 @@ class OdometryFuser : public rclcpp::Node { 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(); - + tf2::Transform rotation_point_in_base_link = getCurrentRotationPoint(); // Get the rotation offset between the IMU and the baselink tf2::Transform imu_mounting_offset; @@ -121,30 +116,52 @@ class OdometryFuser : public rclcpp::Node { } // Get imu orientation in base_link frame - tf2::Quaternion imu_orientation_in_base = imu_orientation * imu_mounting_offset.getRotation(); + 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_.inverse() * imu_orientation_in_base, - previous_motion_odom_.inverseTimes(motion_odometry).getOrigin() + 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 - // 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 = odom_to_base_ * rotation_point_in_base * previous_to_current * base_link_in_rotation_point; + // 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); } @@ -160,9 +177,9 @@ class OdometryFuser : public rclcpp::Node { } private: - tf2::Transform odom_to_base_; + tf2::Transform odom_to_base_link_; tf2::Transform previous_motion_odom_; - tf2::Quaternion previous_imu_; + 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_; @@ -174,6 +191,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_; @@ -233,7 +252,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; @@ -278,12 +297,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()); } @@ -291,6 +308,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; } }; From 6c89be4bd764d7e90808fa40a33172be745b32a6 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Mon, 10 Mar 2025 21:24:53 +0100 Subject: [PATCH 13/13] Adapt localization to new odometry --- .../bitbots_localization/config/config.yaml | 8 +- .../bitbots_localization/localization.hpp | 3 + .../bitbots_localization/src/localization.cpp | 10 +- .../localization_dsd/actions/initialize.py | 107 +----------------- .../localization_dsd/decisions/walk.py | 49 -------- .../localization_dsd/localization.dsd | 6 +- .../localization_blackboard.py | 34 ------ .../bitbots_odometry/src/odometry_fuser.cpp | 21 ++-- 8 files changed, 30 insertions(+), 208 deletions(-) delete mode 100644 bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/decisions/walk.py 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/src/odometry_fuser.cpp b/bitbots_navigation/bitbots_odometry/src/odometry_fuser.cpp index d02c6a4c1..ac47677d8 100644 --- a/bitbots_navigation/bitbots_odometry/src/odometry_fuser.cpp +++ b/bitbots_navigation/bitbots_odometry/src/odometry_fuser.cpp @@ -119,18 +119,19 @@ class OdometryFuser : public rclcpp::Node { 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() - ); - + 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; - + 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}); - + 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(); @@ -308,7 +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()); + // rotation_point_tf.setRotation(tf2::Quaternion::getIdentity()); return rotation_point_tf; } };