From 848ba775ea046496f8239fbc083e8a26331c0dc0 Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Fri, 15 Nov 2024 13:04:22 +0100 Subject: [PATCH 1/6] 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 2/6] 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 3/6] 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 4/6] 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 5/6] 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 6/6] 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;