Robotics StackExchange | Archived questions

Callback function not executed

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.

Asked by silverbullet on 2016-08-25 16:20:05 UTC

Comments

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

Asked by alienmon on 2016-08-25 20:07:57 UTC

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 ?

Asked by alienmon on 2016-08-25 20:21:53 UTC

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!

Asked by silverbullet on 2016-08-25 22:45:11 UTC

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

Asked by alienmon on 2016-08-25 23:40:18 UTC

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

Asked by silverbullet on 2016-08-26 00:36:04 UTC

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

Asked by alienmon on 2016-08-26 00:59:51 UTC

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)?

Asked by Airuno2L on 2016-08-26 06:21:01 UTC

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.

Asked by silverbullet on 2016-08-26 06:45:40 UTC

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

Asked by silverbullet on 2016-08-28 03:07:29 UTC

Can you try ROS_INFO("woot"); instead of n.loginfo("woot")? Or even a std::cout << "woot" << std::endl. I'm wondering if the callback is actually working but that statement just isn't printing. The standard way of printing to screen with roscpp is ROS_INFO.

Asked by Airuno2L on 2016-08-29 06:51:39 UTC

Hey, I'm using rosserial and not roscpp. And the statement is getting executed inside both the functions except UpdateData like i said. I've searched a bit and came to know that IMU messages are not really compatible with rosserial and its a popular problem.

Asked by silverbullet on 2016-08-29 07:11:11 UTC

Answers