Revision history [back]

I've done this:

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <cmath>
#define PI 3.14159265

class Transform{
public:
    Transform();
private:
    ros::NodeHandle n, pn;
    void subMessage(const sensor_msgs::Imu::ConstPtr& msg);
    double roll, pitch, yaw;
    ros::Publisher pub;
    ros::Subscriber sub;
    sensor_msgs::Imu msg_pub;
    std::string frame_id;
};

Transform::Transform():
    pn("~"),
    roll(0),
    pitch(0),
    yaw(0)
{
    pn.param<std::string>("frame_id", frame_id, "/gyro_link");
    ros::Publisher pub = n.advertise<sensor_msgs::imu>("imu/data", 1);
    ros::Subscriber sub = n.subscribe<sensor_msgs::imu>("xsens/imu/data", 10, &Transform::subMessage, this);
};

void Transform::subMessage(const sensor_msgs::Imu::ConstPtr& msg){
        roll = atan2(2*(msg->orientation.x*msg->orientation.y + msg->orientation.z*msg->orientation.w),1-2*(msg->orientation.y*msg->orientation.y+msg->orientation.z*msg->orientation.z)) * 180/PI;
        pitch = asin(2*(msg->orientation.x* msg->orientation.z - msg->orientation.y*msg->orientation.w)) * 180/PI;
        yaw = atan2(2*(msg->orientation.x*msg->orientation.w+msg->orientation.y*msg->orientation.z),1-2*(msg->orientation.z*msg->orientation.z+msg->orientation.w*msg->orientation.w)) * 180/PI;
        ROS_INFO("Received RPY: (%f,%f,%f)",roll,pitch,yaw);

    yaw -= 90;

    ROS_INFO("Processed RPY: (%f,%f,%f)",roll,pitch,yaw);

    roll *= PI/180;
    pitch *= PI/180;
    yaw *= PI/180;

    msg_pub.header.stamp = ros::Time::now();
    msg_pub.header.frame_id = frame_id.c_str();

        msg_pub.orientation.x = cos(roll/2)*cos(pitch/2)*cos(yaw/2) + sin(roll/2)*sin(pitch/2)*sin(yaw/2);
        msg_pub.orientation.y = sin(roll/2)*cos(pitch/2)*cos(yaw/2) - cos(roll/2)*sin(pitch/2)*sin(yaw/2); 
        msg_pub.orientation.z = cos(roll/2)*sin(pitch/2)*cos(yaw/2) + sin(roll/2)*cos(pitch/2)*sin(yaw/2);
        msg_pub.orientation.w = cos(roll/2)*cos(pitch/2)*sin(yaw/2) - sin(roll/2)*sin(pitch/2)*cos(yaw/2);

    roll = atan2(2*(msg_pub.orientation.x*msg_pub.orientation.y + msg_pub.orientation.z*msg_pub.orientation.w),1-2*(msg_pub.orientation.y*msg_pub.orientation.y+msg_pub.orientation.z*msg_pub.orientation.z)) * 180/PI;
        pitch = asin(2*(msg_pub.orientation.x* msg_pub.orientation.z - msg_pub.orientation.y*msg_pub.orientation.w)) * 180/PI;
        yaw = atan2(2*(msg_pub.orientation.x*msg_pub.orientation.w+msg_pub.orientation.y*msg_pub.orientation.z),1-2*(msg_pub.orientation.z*msg_pub.orientation.z+msg_pub.orientation.w*msg_pub.orientation.w)) * 180/PI;
        ROS_INFO("Published RPY: (%f,%f,%f)",roll,pitch,yaw);

    msg_pub.angular_velocity.x = msg->angular_velocity.x;
    msg_pub.angular_velocity.y = msg->angular_velocity.y;
    msg_pub.angular_velocity.z = msg->angular_velocity.z;

    msg_pub.linear_acceleration.x = msg->linear_acceleration.x;
    msg_pub.linear_acceleration.y = msg->linear_acceleration.y;
    msg_pub.linear_acceleration.z = msg->linear_acceleration.z;

    pub.publish(msg_pub);
}


int main(int argc, char** argv)
{
        ros::init(argc, argv, "nwu_to_enu");
    Transform transform;
    ros::spin();
}

to do what I want some maths on a topic and then publish it again, but the callback function, subMessage, is never called. Any idea what can be happening?

Thanks!

I've done this:

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <cmath>
#define PI 3.14159265

class Transform{
public:
    Transform();
private:
    ros::NodeHandle n, pn;
    void subMessage(const sensor_msgs::Imu::ConstPtr& msg);
    double roll, pitch, yaw;
    ros::Publisher pub;
    ros::Subscriber sub;
    sensor_msgs::Imu msg_pub;
    std::string frame_id;
};

Transform::Transform():
    pn("~"),
    roll(0),
    pitch(0),
    yaw(0)
{
    pn.param<std::string>("frame_id", frame_id, "/gyro_link");
    ros::Publisher pub = n.advertise<sensor_msgs::imu>("imu/data", 1);
    ros::Subscriber sub = n.subscribe<sensor_msgs::imu>("xsens/imu/data", 10, &Transform::subMessage, this);
};

void Transform::subMessage(const sensor_msgs::Imu::ConstPtr& msg){
        roll = atan2(2*(msg->orientation.x*msg->orientation.y + msg->orientation.z*msg->orientation.w),1-2*(msg->orientation.y*msg->orientation.y+msg->orientation.z*msg->orientation.z)) * 180/PI;
        pitch = asin(2*(msg->orientation.x* msg->orientation.z - msg->orientation.y*msg->orientation.w)) * 180/PI;
        yaw = atan2(2*(msg->orientation.x*msg->orientation.w+msg->orientation.y*msg->orientation.z),1-2*(msg->orientation.z*msg->orientation.z+msg->orientation.w*msg->orientation.w)) * 180/PI;
        ROS_INFO("Received RPY: (%f,%f,%f)",roll,pitch,yaw);

    yaw -= 90;

    ROS_INFO("Processed RPY: (%f,%f,%f)",roll,pitch,yaw);

    roll *= PI/180;
    pitch *= PI/180;
    yaw *= PI/180;

    msg_pub.header.stamp = ros::Time::now();
    msg_pub.header.frame_id = frame_id.c_str();

        msg_pub.orientation.x = cos(roll/2)*cos(pitch/2)*cos(yaw/2) + sin(roll/2)*sin(pitch/2)*sin(yaw/2);
        msg_pub.orientation.y = sin(roll/2)*cos(pitch/2)*cos(yaw/2) - cos(roll/2)*sin(pitch/2)*sin(yaw/2); 
        msg_pub.orientation.z = cos(roll/2)*sin(pitch/2)*cos(yaw/2) + sin(roll/2)*cos(pitch/2)*sin(yaw/2);
        msg_pub.orientation.w = cos(roll/2)*cos(pitch/2)*sin(yaw/2) - sin(roll/2)*sin(pitch/2)*cos(yaw/2);

    roll = atan2(2*(msg_pub.orientation.x*msg_pub.orientation.y + msg_pub.orientation.z*msg_pub.orientation.w),1-2*(msg_pub.orientation.y*msg_pub.orientation.y+msg_pub.orientation.z*msg_pub.orientation.z)) * 180/PI;
        pitch = asin(2*(msg_pub.orientation.x* msg_pub.orientation.z - msg_pub.orientation.y*msg_pub.orientation.w)) * 180/PI;
        yaw = atan2(2*(msg_pub.orientation.x*msg_pub.orientation.w+msg_pub.orientation.y*msg_pub.orientation.z),1-2*(msg_pub.orientation.z*msg_pub.orientation.z+msg_pub.orientation.w*msg_pub.orientation.w)) * 180/PI;
        ROS_INFO("Published RPY: (%f,%f,%f)",roll,pitch,yaw);

    msg_pub.angular_velocity.x = msg->angular_velocity.x;
    msg_pub.angular_velocity.y = msg->angular_velocity.y;
    msg_pub.angular_velocity.z = msg->angular_velocity.z;

    msg_pub.linear_acceleration.x = msg->linear_acceleration.x;
    msg_pub.linear_acceleration.y = msg->linear_acceleration.y;
    msg_pub.linear_acceleration.z = msg->linear_acceleration.z;

    pub.publish(msg_pub);
}


int main(int argc, char** argv)
{
        ros::init(argc, argv, "nwu_to_enu");
    Transform transform;
    ros::spin();
}

to do what I want some maths on a topic and then publish it again, but the callback function, subMessage, is never called. Any idea what can be happening?

Thanks!