Ask Your Question
0

Callback function doesn't get data from message

asked 2016-03-17 10:05:37 -0500

Bert gravatar image

updated 2016-03-17 13:55:43 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

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 like rosnode and rostopic are very helpful for debugging.

jarvisschultz gravatar imagejarvisschultz ( 2016-03-17 12:10:38 -0500 )edit

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? Does rostopic hz /tick_topic report the publishing?

jarvisschultz gravatar imagejarvisschultz ( 2016-03-17 12:12:08 -0500 )edit

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?

Bert gravatar imageBert ( 2016-03-17 13:42:48 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2016-03-17 14:01:29 -0500

ahendrix gravatar image

You're not using the msg variable that is passed into the callback; you're trying to get the ticks from a global variable instead.

Your callback should probably be more like:

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 = msg->ticks_links - _PreviousLeftEncoderCounts;
  deltaRight = msg->ticks_rechts - _PreviousRightEncoderCounts;
edit flag offensive delete link more

Comments

Thanks! This works

Bert gravatar imageBert ( 2016-03-18 04:52:43 -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

1 follower

Stats

Asked: 2016-03-17 10:05:37 -0500

Seen: 355 times

Last updated: Mar 17 '16