Callback function not executed

asked 2016-08-25 16:20:05 -0600

silverbullet gravatar image

updated 2016-08-28 14:13:56 -0600

Hello everyone, I've been trying to get this to work since many days but no avail. I am using an IMU sensor. (separate usb port). I'm trying to get the values from the sensor on my arduino via rosserial. Here is my arduino code for reference.

#include <ros.h>
#include <geometry_msgs/Pose2D.h>
#include <sensor_msgs/Imu.h>


double ox,oy,oz; //orientation in x,y,z. These are made global so that they will be automatically be updated via ROS 
callback functions

double lax,lay,laz; //Linear Acceleration in x, and z similarly.

double avx,avy,avz; //Angular velocity x,y,z

double x,y,theta; //Heading Parameters

double xt,yt,thetat; //TrueHeading Parameters


ros::NodeHandle n;


void updateData(const sensor_msgs::Imu& msg)
{  
n.loginfo("woot"); //This does not get printed.
lax = msg.linear_acceleration.x;
lay = msg.linear_acceleration.y;
laz = msg.linear_acceleration.z;

avx = msg.angular_velocity.x;
avy = msg.angular_velocity.y;
avz = msg.angular_velocity.z;

}


void updateHeading(const geometry_msgs::Pose2D& msg)

{
x = msg.x;
y = msg.y;
theta = msg.theta;

}


void updateHeadingTrueDegree(const geometry_msgs::Pose2D& msg)


{
xt = msg.x;  
yt = msg.y;
thetat = msg.theta;
}

ros::Subscriber<sensor_msgs::Imu> s1("imu/data", &updateData );

ros::Subscriber<geometry_msgs::Pose2D> s2("imu/HeadingTrue", &updateHeading );

ros::Subscriber<geometry_msgs::Pose2D> s3("imu/HeadingTrue_degree", &updateHeadingTrueDegree );


void setup()
{
n.getHardware()->setBaud(115200);
n.initNode();
n.subscribe(s1);
n.subscribe(s2);
n.subscribe(s3);
}

void loop()
{
  //n.loginfo("woot"); //This works

 n.spinOnce();

//delay(15);

}

The problem is that the callback functions aren't working(because loginfo line isn't printed). I've tried to introduce delays and increase the frequency of spinOnce function but it didn't work. Also the baud rate on both my arduino and sensor is fixed and equal to 1152000. The sensor doesn't work below this value. And the messages are gettting published from the sensor because rostopic echo imu/data works.

Thanks in advance :)

EDIT :

Hey people, I have an update! I tried to just subscribe to s2 and s3 and it works, I am guessing that's because it just needs to update 3 variables, but s1 is updating 6 things with huge precision(orientation.x,y,z,w & angular velocity..etc). So I rounded off stuff to 3 places but still no avail. I'm not sure what's the reason now.

edit retag flag offensive close merge delete

Comments

Can try to comment the s2 and s3 part first? (Just subscribe to s1) To see whether that works or not.

alienmon gravatar imagealienmon ( 2016-08-25 20:07:57 -0600 )edit

Also , did you specify the queue size? Maybe the queue size is too small that before spinOnce is called, the message from s2 and s3 overwrite it already. So for this you can try to print inside both updateHeading and updateheadingtruedegree function, and see the outcome ?

alienmon gravatar imagealienmon ( 2016-08-25 20:21:53 -0600 )edit

Hey! Thanks for your response firstly. Yes I've already tried just subscribing to s1 and it didn't work. As per the queue size thing, i haven't tried it just but i think adding a delay could compensate for that? Even a 100ms delay doesn't work. I'll let you know though. Regards!

silverbullet gravatar imagesilverbullet ( 2016-08-25 22:45:11 -0600 )edit

Have you checked the publisher? Whether the publisher really publish the message

alienmon gravatar imagealienmon ( 2016-08-25 23:40:18 -0600 )edit

Yes. When I do rostopic echo imu/data I get the output there.

silverbullet gravatar imagesilverbullet ( 2016-08-26 00:36:04 -0600 )edit

Hmm , maybe check whether there's something in the callbackqueue using isEmpty. (whether the message is really published or not)

alienmon gravatar imagealienmon ( 2016-08-26 00:59:51 -0600 )edit

This is a little confusing. If this is the code you're running on your Arduino, shouldn't it be publishing the sensor data and not subscribing to it? Or is the sensor connected to the computer (not the ardunio)?

Airuno2L gravatar imageAiruno2L ( 2016-08-26 06:21:01 -0600 )edit

Yes. Like I said that the sensor is not attached to the Arduino. It's attached to a separate usb port and it publishes data to the arduino which thereafter drives motors etc.

silverbullet gravatar imagesilverbullet ( 2016-08-26 06:45:40 -0600 )edit