Revision history [back]

So I found a solution but it doesn't make sens to me... I tried to find a function similar to q.normalize() from tf2/LinearMath/Quaternion.h for python but I didn't find one proper to ros2. So I use the library pyquaternion and it still didn't work even with the normalization from this library. Like I wanted to be sure it wasn't coming from the quaternions, I create a c++ node that subscribe to my real IMU topic (real_imu_nucleo1000), create a quaternion with the same value that the msg in input, normalize it with q.normalize() and publish a new imu msg. But actually even without q.normalize() it works ! I literally doesn't change one thing inside the quaternion, it's just a copy past...^^' :

class normalize_tf2 : public rclcpp::Node
{
  public:
    normalize_tf2()
    : Node("normalize_tf2"), count_(0)
    {
      publisher_ = this->create_publisher<sensor_msgs::msg::Imu>("real_imu_nucleo1000_tf2", 10);
      subscription_ = this->create_subscription<sensor_msgs::msg::Imu>(
      "real_imu_nucleo1000", 10, std::bind(&normalize_tf2::topic_callback, this, _1));
      imu_real.header.frame_id="real_imu_nucleo1000_tf2";
      imu_real.orientation_covariance={0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1};
      imu_real.linear_acceleration_covariance={0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1};
      imu_real.angular_velocity_covariance={0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1};
    }

  private:
     void topic_callback(const sensor_msgs::msg::Imu msg) 
    {

       tf2::Quaternion q;
       q.setX(msg.orientation.x);
       q.setY(msg.orientation.y);
       q.setZ(msg.orientation.z);
       q.setW(msg.orientation.w);
       //q.normalize();
       imu_real.header.stamp=this->now();
       imu_real.orientation.w=q.getW();
       imu_real.orientation.x=q.getX();
       imu_real.orientation.y=q.getY();
       imu_real.orientation.z=q.getZ();
       publisher_->publish(imu_real);
    }
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr subscription_;
    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr publisher_;
    size_t count_;
    sensor_msgs::msg::Imu imu_real;
};

And with this msg r_l works normally... But if I echo "real_imu_nucleo1000_tf2" and "real_imu_nucleo1000" we can see that nothing seems to have changed :

Quaternion that went through tf2 (works) :

   header:
  stamp:
    sec: 784
    nanosec: 350000000
  frame_id: real_imu_nucleo1000_tf2
orientation:
  x: -0.9935468873394891
  y: 0.10426965190121015
  z: 0.0413958013927812
  w: 0.016697603943751436
orientation_covariance:
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
angular_velocity:
  x: 0.0
  y: 0.0
  z: 0.0
angular_velocity_covariance:
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
linear_acceleration:
  x: 0.0
  y: 0.0
  z: 0.0
linear_acceleration_covariance:
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1

Original quaternion (doesn't work):

header:
  stamp:
    sec: 784
    nanosec: 350000000
  frame_id: real_imu_nucleo1000
orientation:
  x: -0.9935468873394891
  y: 0.10426965190121015
  z: 0.0413958013927812
  w: 0.016697603943751436
orientation_covariance:
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
angular_velocity:
  x: 0.0
  y: 0.0
  z: 0.0
angular_velocity_covariance:
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
linear_acceleration:
  x: 0.0
  y: 0.0
  z: 0.0
linear_acceleration_covariance:
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1
- 0.1

I just modified real_imu_nucleo1000 to real_imu_nucleo1000_tf2 here :

tf_real_imu_Nucleo = Node(package = "tf2_ros", 
                    executable = "static_transform_publisher",
                    arguments = ["0", "0", "-0.042", "0", "0", "0", "main_buddy_link", "real_imu_nucleo1000"])

and here :

imu0: /real_imu_nucleo1000

So if someone understand what is going one please tell me ^^ because yes it works, but it's very messy and not very efficient.