Ask Your Question
0

ROS node not publishing at the right frequency.

asked 2016-01-27 20:19:04 -0500

trips323 gravatar image

I am trying to publish an odom message. If i set the rate to 10 then it should publish at a frequency of 10Hz but it is publishing at 50000Hz. I have sim-time turned off. I am using a virtual machine if that matters.

    #include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include "std_msgs/String.h"
#include <sstream>
#include <my_message_types/due_odom.h>

double x = 0;
double y = 0;
double th = 0;

double vx = 0;
double vy = 0;
double vth = 0;

ros::Time time_stamp;
char child_frame_id[] = "base_link";
char frame_id[] = "odom";

void messageCallback(const my_message_types::due_odom& msg)
{
  x = msg.x;
  y = msg.y;
  th = msg.th;
  vx = msg.vx;
  vy = msg.vy;
  vth = msg.vth;
 // child_frame_id = msg.child_frame_id;
  time_stamp = msg.header.stamp;
 // frame_id = msg.header.frame_id;
}

int main(int argc, char** argv){
  ros::init(argc, argv, "odometry_publisher");

  ros::NodeHandle n;
  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
  ros::Subscriber odom_sub = n.subscribe("due_odom", 1000, messageCallback);
  tf::TransformBroadcaster odom_broadcaster;

  ros::Rate r(10);
  while(n.ok()){

    ros::spinOnce();               // check for incoming messages

    //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 = time_stamp;
    odom_trans.header.frame_id = frame_id;
    odom_trans.child_frame_id = child_frame_id;

    odom_trans.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = 0.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 = time_stamp;
    odom.header.frame_id = frame_id;

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

    //set the velocity
    odom.child_frame_id = child_frame_id;
    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);

    r.sleep();
  }
}
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2016-01-27 21:39:00 -0500

ahendrix gravatar image

Your code looks reasonable; with the Rate object that you're using, it should be running at 10Hz.

Some virtual machines don't implement the high-precision timers correctly, and I think ROS uses the high-precision timers for rates and sleeps. As a first debugging step, it would be good to add a ROS_INFO message to each loop iteration, and print the ROS time, so that you can see if ROS is interpreting time correctly or not. Try something like

ROS_INFO_STREAM("Now: " << ros::Time::now());
edit flag offensive delete link more
0

answered 2016-01-28 00:10:37 -0500

trips323 gravatar image

Thanks for the reply.

I figured out that the reason was because I was trying to test the node without filling out the time stamps. I didn't realize that is how it calculates the publish frequency.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2016-01-27 20:19:04 -0500

Seen: 1,246 times

Last updated: Jan 28 '16