Callback function doesn't get data from message
Hi
For testing an Odometry message I generated a message (encoder_msg) with following code:
int32 ticks_links
int32 ticks_rechts
We made a publisher that generates data for ticks_links en ticks_rechts over the topic: tick_topic This works.
Then I wrote a cpp file with the following:
void WheelCallback(const mastering_ros_demo_pkg::encoder_msg::ConstPtr& msg)
{
current_time_encoder = ros::Time::now();
deltaLeft = ticks_links - _PreviousLeftEncoderCounts;
deltaRight = ticks_rechts - _PreviousRightEncoderCounts;
vx = deltaLeft * DistancePerCount / (current_time_encoder - last_time_encoder).toSec();
vy = deltaRight * DistancePerCount / (current_time_encoder - last_time_encoder).toSec();
.....
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "odometry_publisher");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("tick_topic", 100, WheelCallback);
...
}
For some reason, the calculations in this function aren't being executed, is there a mistake in my code? This code isn't complete, if I let out important parts, please tell me.
Thanks!
EDIT 1: Here the complete code:
#include "ros/ros.h"
#include <geometry_msgs/Vector3.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include "mastering_ros_demo_pkg/encoder_msg.h"
long _PreviousLeftEncoderCounts = 0;
long _PreviousRightEncoderCounts = 0;
ros::Time current_time_encoder, last_time_encoder;
double DistancePerCount = (3.14159265 * 0.1524) / 1;
double x;
double y;
double th;
double W;
double V;
double vx;
double vy;
double vth;
double deltaLeft;
double deltaRight;
double d=50; //afstand tussen de wielen
int ticks_links;
int ticks_rechts;
//void ticks_topic_callback(const mastering_ros_demo_pkg::encoder_msg::ConstPtr& msg)
//{
// ROS_INFO("Recieved [%d]",msg->ticks_links);
// ROS_INFO("Recieved [%d]",msg->ticks_rechts);
//}
//void WheelCallback(const geometry_msgs::Vector3::ConstPtr& ticks)
void WheelCallback(const mastering_ros_demo_pkg::encoder_msg::ConstPtr& msg)
{
//tijd van de meting
current_time_encoder = ros::Time::now();
//verschil in ticks tov vorige berekening
deltaLeft = ticks_links - _PreviousLeftEncoderCounts;
deltaRight = ticks_rechts - _PreviousRightEncoderCounts;
//snelheid links (x) en rechts (y)
vx = deltaLeft * DistancePerCount / (current_time_encoder - last_time_encoder).toSec();
vy = deltaRight * DistancePerCount / (current_time_encoder - last_time_encoder).toSec();
if (vx == vy)
{
V = vx;
W = 0;
}
else
{
// Assuming the robot is rotating about point A
// W = vel_left/r = vel_right/(r + d), see the image below for r and d
double r = (vx * d) / (vy - vx); // Anti Clockwise is positive
W = vx/r; // Rotational velocity of the robot
V = W * (r + d/2); // Translation velocity of the robot
}
vth= W;
//aantal ticks onthouden
_PreviousLeftEncoderCounts = ticks_links;
_PreviousRightEncoderCounts = ticks_rechts;
//tijd van de laatste meting onthouden
last_time_encoder = current_time_encoder;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "odometry_publisher");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("tick_topic", 100, WheelCallback);
//ros::Subscriber sub1 = n.subscribe("tick_topic", 100, ticks_topic_callback);
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
tf::TransformBroadcaster odom_broadcaster;
ros::Time current_time, last_time;
current_time = ros::Time::now();
last_time = ros::Time::now();
ros::Rate r(1.0);
while(n.ok()){
current_time = ros::Time::now();
//compute odometry in a typical way given the velocities of the robot
double dt = (current_time - last_time).toSec();
double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
double delta_th = vth * dt;
x += delta_x;
y += delta_y;
th += delta_th;
//since all odometry is 6DOF we'll need a quaternion created from yaw
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
//first, we'll publish the transform over tf
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
odom_trans ...
I don't see any obvious mistakes in your code. How do you know the callback isn't being executed? Are you sure you're running the publisher at the same time and that it's actually publishing on the
tick_topic
topic? Command line tools likerosnode
androstopic
are very helpful for debugging.For example
rosnode info PUBLISHER_NODE_NAME
should tell you what topics it is publishing on and what type they are. The same command for the subscriber node should tell you what topics it is subscribed to and their types. Does this info match? Doesrostopic hz /tick_topic
report the publishing?I used rostopic echo /tick_topic, this topic is being published. When I use rqt_graph I can see that the topic goes to the odometry file. but when I look to the topic that my Odometry publishes, this data doesn't change.. Should I post the entire file?