Orientation also in imu msg

[felixmn91]

thank you for your repo! It worked for me plug-and-play with the MicroStrain 3DM-GX4-25.

Is it possible to include the orientation in the published imu msg?

Kind regards,
Felix

[versatran01]

I believe if you set enable_filter in the launch file to be true then it will publish the internal attitude filter orientation.

[felixmn91]

yes that's true, but only in a custom msg, not in the sensor_msgs/Imu Message

[versatran01]

You are right. I will take a look. Meanwhile, if you already have something, a PR is always welcome.

[felixmn91]

a workaround:

#include "ros/ros.h"
#include "std_msgs/String.h"

#include <imu_3dm_gx4/FilterOutput.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

class ImuWithOrientation
{
public:
ImuWithOrientation();
private:
void imuCallback(const sensor_msgs::Imu::ConstPtr& imu);
void imuFilterCallback(const imu_3dm_gx4::FilterOutput::ConstPtr& orientation);

ros::NodeHandle n;
ros::Subscriber sub_imu;
ros::Publisher pub_imu;
ros::Subscriber sub_imuFilter;

sensor_msgs::Imu current_imu_msg;

};

void ImuWithOrientation::imuCallback(const sensor_msgs::Imu::ConstPtr& imu)
{
//ROS_INFO("I heard from imu [%s]", imu->header.frame_id.c_str());
current_imu_msg = *imu;
}

void ImuWithOrientation::imuFilterCallback(const imu_3dm_gx4::FilterOutput::ConstPtr& orientation)
{
//ROS_INFO("I heard from imuFilter [%s]", orientation->header.frame_id.c_str());

sensor_msgs::Imu temp_imu = current_imu_msg;
temp_imu.orientation = orientation->orientation;

pub_imu.publish(temp_imu);
}

ImuWithOrientation::ImuWithOrientation()
{
sub_imu = n.subscribe("imu/imu", 100, &ImuWithOrientation::imuCallback, this);
sub_imuFilter = n.subscribe("imu/filter", 100, &ImuWithOrientation::imuFilterCallback, this);

pub_imu = n.advertise<sensor_msgs::Imu>("imu_with_orientation", 100);
}

int main(int argc, char **argv)
{
ros::init(argc, argv, "publish_imu_with_orientation");
ImuWithOrientation imuWithOrientation;
ros::spin();
return 0;
}