Ask Your Question
0

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

Hello,

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();

   pub.publish(msg_to_publish);
}

Thank you for the help!

edit retag flag offensive close merge delete

5 Answers

Sort by ยป oldest newest most voted
2

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

Comments

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
1

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
1

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

Comments

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
0

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{
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");
    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;

    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!

edit flag offensive delete link more

Comments

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
0

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...

TurtlebotTeleop::TurtlebotTeleop():
  ph_("~"),
  linear_(1),
  angular_(0),
  deadman_axis_(4),
  l_scale_(0.3),
  a_scale_(0.9),
  lin_vel_fix(0),
  ang_vel_fix(0)

{
  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

Comments

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

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

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

Seen: 1,077 times

Last updated: Feb 08 '12