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

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

apalomer
318 ●51 ●54 ●66

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

add a comment

2

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

Bill Smart
1263 ●6 ●15 ●34

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

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 ( 2012-02-07 20:48:09 -0500 )edit

That's right.

Bill Smart ( 2012-02-08 01:36:05 -0500 )edit

add a comment

1

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

arebgun

2121 ●7 ●22 ●43 http://www.cs.arizona....

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

add a comment

1

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

dornhege

31395 ●134 ●284 ●498

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

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 ( 2012-02-06 06:44:43 -0500 )edit

add a comment

0

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

apalomer
318 ●51 ●54 ●66

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

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 ( 2012-02-07 01:05:15 -0500 )edit

add a comment

0

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

apalomer
318 ●51 ●54 ●66

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

Comments

Yes, that is absolutely correct and not shadowing other variables.

dornhege ( 2012-02-07 23:47:37 -0500 )edit

add a comment

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

5 Answers

Comments

Comments

Comments

Comments

Question Tools

Stats

Related questions

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

5 Answers

Comments

Comments

Comments

Comments

Question Tools

Stats

Related questions

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