ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Doing maths to a subscribed message to public a new one.

asked 2012-02-06 02:55:57 -0500

apalomer gravatar image

updated 2012-02-06 02:56:47 -0500


I'm working with xsense, and I saw that it's reference coordinate is north west up (quite wierd) and that ROS works with east north up, so I have to change the message that publishes my xsense node to the new reference sistem. The thing is that I know how to subscribe a message, and how to publish, but I don't know how to acess the message that I'm subscribin outside the callback function.

What I would like is something like:

#include <ros/ros.h>
#include <sensro_msgs/Imu.h>

void message_received(sensor_msgs::Imu::ConstPtr& msg){
   ROS_INFO("Received quaternion: (%f, %f, %f, %f)", msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w);
int main(int argc, char** argv){
   ros::init(argc, argv, "nwu_to_enu");
   ros::NodeHandle n;
   ros::NodeHandle pn("~");
   std::string frame_id;

   pn.param<std::string>("frame_id", frame_id, "gyro_link");
   ros::Publisher pub = n.advertise<sensor_msgs::imu>("imu/data", 10);
   ros::Subscriber sub = n.subscribe<sensor_msgs::imu>("xsens/imu/data", 1000, message_received);

   sensor_msgs::Imu msg_to_publish;

   //from now on the code might not be correct as I don't really know how to do it, that's my question. msg is the one that I have from the callback function and that I need to be able to access outside the callback function.
   msg_to_publish.header.stamp = ros::Time::now();
   msg_to_publish.header.frame_id = frame_id.c_str();

   msg_to_publish.orientation.x = - msg->quaternion_x(); //I just change the sign as an example, I'll have to see which transform do I need.
   msg_to_publish.orientation.y = - msg->quaternion_y();
   msg_to_publish.orientation.z = - msg->quaternion_z();
   msg_to_publish.orientation.w = - msg->quaternion_w();

   msg_to_publish.angular_velocity.x = msg->gyroscope_x();
   msg_to_publish.angular_velocity.y = msg->gyroscope_y();
   msg_to_publish.angular_velocity.z = msg->gyroscope_z();

   msg_to_publish.linear_acceleration.x = msg->accelerometer_x();
   msg_to_publish.linear_acceleration.y = msg->accelerometer_y();
   msg_to_publish.linear_acceleration.z = msg->accelerometer_z();


Thank you for the help!

edit retag flag offensive close merge delete

5 Answers

Sort by ยป oldest newest most voted

answered 2012-02-07 06:44:16 -0500

Bill Smart gravatar image

updated 2012-02-08 01:35:42 -0500

You're shadowing the variables pub and sub in your constructor. You're then assigning to these local copies, which leads to them being destructed when the constructor for your class is done.

edit flag offensive delete link more


ah ok, i can see now the difference, what I'm doing wrong is ros::Publisher pub = .... instead of pub = ...., isn't it?
apalomer gravatar image apalomer  ( 2012-02-07 20:48:09 -0500 )edit
That's right.
Bill Smart gravatar image Bill Smart  ( 2012-02-08 01:36:05 -0500 )edit

answered 2012-02-07 01:05:13 -0500

arebgun gravatar image

Your callback is never called because you have stored your publisher and subscriber in local variables which go out of scope and get destroyed immediately after constructor finishes. Once you put them in the class variables that you defined you should be fine, assuming the topic name is correct.

edit flag offensive delete link more

answered 2012-02-06 03:11:22 -0500

dornhege gravatar image

I think you want to move the publishing code dealing with the message into the callback function...

BTW: North, West up leads to 0 deg being north.

edit flag offensive delete link more


The canonical ROS convention is East, North, Up (ENU) with zero radians being East. The NED aerospace convention (North, East, Down) has zero to the North.
joq gravatar image joq  ( 2012-02-06 06:44:43 -0500 )edit

answered 2012-02-06 21:04:50 -0500

apalomer gravatar image

updated 2012-02-08 21:32:15 -0500

I've done this:

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

class Transform{
    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;

    pn.param<std::string>("frame_id", frame_id, "/gyro_link");
    pub = n.advertise<sensor_msgs::imu>("imu/data", 1);
    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;


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

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?


edit flag offensive delete link more


I think the problem is that your subscriber dies after the constructor finishes. Store it as a class member. EDIT: Actually, the local variables in the constructor hide the class variables.
raahlb gravatar image raahlb  ( 2012-02-07 01:05:15 -0500 )edit

answered 2012-02-07 01:42:51 -0500

apalomer gravatar image

In turtlebot_teleop, in the joystick they do it like I did (actually I copied the structure) and it works...


  ph_.param("axis_linear", linear_, linear_);
  ph_.param("axis_angular", angular_, angular_);
  ph_.param("axis_deadman", deadman_axis_, deadman_axis_);
  ph_.param("scale_angular", a_scale_, a_scale_);
  ph_.param("scale_linear", l_scale_, l_scale_);

  vel_pub_ = nh_.advertise<geometry_msgs::twist>("cmd_vel", 1);
  joy_sub_ = nh_.subscribe<sensor_msgs::joy>("joy", 10, &TurtlebotTeleop::joyCallback,this);
  timer_ = nh_.createTimer(ros::Duration(0.1), boost::bind(&TurtlebotTeleop::publish, this));
edit flag offensive delete link more


Yes, that is absolutely correct and not shadowing other variables.
dornhege gravatar image dornhege  ( 2012-02-07 23:47:37 -0500 )edit

Question Tools


Asked: 2012-02-06 02:55:57 -0500

Seen: 1,192 times

Last updated: Feb 08 '12