0
$\begingroup$

Rosanswers logo

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.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = 5.0;
    odom_trans.transform.rotation = odom_quat;

    //send the transform
    odom_broadcaster.sendTransform(odom_trans);

    //next, we'll publish the odometry message over ROS
    nav_msgs::Odometry odom;
    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";

    //set the position
    odom.pose.pose.position.x = x;
    odom.pose.pose.position.y = y;
    odom.pose.pose.position.z = 6.0;
    odom.pose.pose.orientation = odom_quat;

    //set the velocity
    odom.child_frame_id = "base_link";
    odom.twist.twist.linear.x = vx;
    odom.twist.twist.linear.y = vy;
    odom.twist.twist.angular.z = vth;

    //publish the message
    odom_pub.publish(odom);

    last_time = current_time;
    ros::spinOnce();
    r.sleep();
  }
}

What I get when I do: rostopic echo /odom

header: 
  seq: 19
  stamp: 
    secs: 1458240875
    nsecs: 620201731
  frame_id: odom
child_frame_id: base_link
pose: 
  pose: 
    position: 
      x: 0.0
      y: 0.0
      z: 6.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

This data doesn't change...


Originally posted by Bert on ROS Answers with karma: 41 on 2016-03-17

Post score: 0


Original comments

Comment by jarvisschultz on 2016-03-17:
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.

Comment by jarvisschultz on 2016-03-17:
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?

Comment by Bert on 2016-03-17:
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?

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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;

Originally posted by ahendrix with karma: 47576 on 2016-03-17

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by Bert on 2016-03-18:
Thanks! This works

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.