ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

The loop_rate() for my node doesn't change the publish frequency

asked 2021-02-05 13:37:49 -0500

Marcus Barnet gravatar image

updated 2021-02-05 13:43:34 -0500

Hi to all,

I'm having problems with the loop_rate() function for my node. I set it at 10hz and then I publish some topics in the while() loop but when I rostopic them, I see that they are published only at 4 hertz! What am I doing wrong?

This is my code:

 #include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"
#include <unistd.h>

#include <sstream>
#include <iostream>
#include <stdio.h>
#include <string.h>

#include "../include/robo_base/RoboteqDevice.h"
#include "../include/robo_base/ErrorCodes.h"
#include "../include/robo_base/Constants.h"
#include "robo_explorer/robo_io.h"

int main(int argc, char **argv)
{
  ros::init(argc, argv, "robo_explorer");
  ros::NodeHandle n;

  std::string port = "";

  ...  

  n.getParam("/robo_explorer/port", port);
  n.getParam("/robo_explorer/sys", sys);
  n.getParam("/robo_explorer/board", riox_board);
  n.getParam("/robo_explorer/enc", enc);
  n.getParam("/robo_explorer/io", io);
  n.getParam("/robo_explorer/imu", imu);
  n.getParam("/robo_explorer/velocity", velocity);
  n.getParam("/robo_explorer/rate", rate);


  ros::Publisher sys_pub = n.advertise<std_msgs::String>("robo_explorer/sys", 1);
  ros::Publisher enc_pub = n.advertise<std_msgs::String>("robo_explorer/enc", 1);
  ros::Publisher vel_pub = n.advertise<std_msgs::String>("robo_explorer/velocity", 1);
  ros::Publisher io_pub = n.advertise<std_msgs::String>("robo_explorer/io", 1);
  ros::Publisher imu_pub = n.advertise<std_msgs::String>("robo_explorer/imu", 1);

  ros::Subscriber cmd_sub = n.subscribe("/robo_explorer/cmd_vel", 1, &RoboteqDevice::cmdCallback, &device);
  ros::Subscriber cmd_io = n.subscribe("/robo_explorer/io_status", 1, &RoboteqDevice::cmd_ioCallback, &device);

  double current_time =ros::Time::now().toSec();
  double last_time =ros::Time::now().toSec();


  ros::Rate loop_rate(rate);
  while (ros::ok())
  {


    //System Topic
    if (sys == 1){
    .....
  sys_pub.publish(msg_sys);

  }


    //Encoder Topic
    if (enc == 1){
vel_pub.publish(msg_vel);
enc_pub.publish(msg_enc);
...
    }


   //IO Topic
   if (io == 1){
     ...
io_pub.publish(msg_io);
    }

   if (imu == 1){
..
imu_pub.publish(msg_imu);

    }

    ros::spinOnce();

    loop_rate.sleep();
    ++count;
  }

  return 0;
}

These are all the published topics:

/robo_explorer/cmd_vel
/robo_explorer/enc
/robo_explorer/imu
/robo_explorer/io
/robo_explorer/io_status
/robo_explorer/sys
/robo_explorer/velocity

and for example this is one of the output:

rostopic echo /robo_explorer/io

data: "1612553682.755857579,0,0,0,0,0,0,0,0,0,0,0,0,100"
---
data: "1612553683.020352972,0,0,0,0,0,0,0,0,0,0,0,0,101"
---
data: "1612553683.285076521,0,0,0,0,0,0,0,0,0,0,0,0,102"
---
data: "1612553683.551031712,0,0,0,0,0,0,0,0,0,0,0,0,103"
---
data: "1612553683.816192936,0,0,0,0,0,0,0,0,0,0,0,0,104"
---
data: "1612553684.082027678,0,0,0,0,0,0,0,0,0,0,0,0,105"
---
data: "1612553684.347815952,0,0,0,0,0,0,0,0,0,0,0,0,106"
---
data: "1612553684.613140284,0,0,0,0,0,0,0,0,0,0,0,0,107"
---
data: "1612553684.878549824,0,0,0,0,0,0,0,0,0,0,0,0,108"
---
data: "1612553685.144171685,0,0,0,0,0,0,0,0,0,0,0,0,109"

as you can see, there are only 4 samples for each second. How can I solve this?

P.S. this is the output for the hz:

rostopic ...
(more)
edit retag flag offensive close merge delete

Comments

1

I found out that probably the issue is related to the sensor reading. The cpu requires some additional time to read from each sensor and so the node cannot respects the frequency.

Marcus Barnet gravatar image Marcus Barnet  ( 2021-02-05 14:16:59 -0500 )edit
1

How do you set the frequency, and have you confirmed that the value is correct after setting it?

tryan gravatar image tryan  ( 2021-02-05 14:22:00 -0500 )edit
1

I get the parameter from the launch file and it is correctly received since I can print it in the main(). Moreover, I tried to directly write, i.e, loop_rate(20) and it still runs at 4 hz. However, I'm pretty sure it is the payload that slows down the whole process, unfortunately.

Marcus Barnet gravatar image Marcus Barnet  ( 2021-02-05 15:01:03 -0500 )edit
1

Ah, then yes, I have to agree :/

tryan gravatar image tryan  ( 2021-02-05 15:15:04 -0500 )edit
1

You may want to experiment by changing the setting to less than 4Hz, say 2Hz. If the maximum speed is 4Hz because of the sensor reading time, then it should work correctly at 2Hz. Alternatively, you can check if the speed is still 4Hz without running loop_rate.sleep();.

miura gravatar image miura  ( 2021-02-06 00:12:01 -0500 )edit
3

If I decrease the frequency, i.e., 2 Hz, it goes to 2 Hz, so unfortunately, it's the acquisition time that lowers the overall frequency. If I shuts down some topics, the frequency agrees with the setting, so I can assume that the loop_rate() is working fine. Thank you for your help, guys!

Marcus Barnet gravatar image Marcus Barnet  ( 2021-02-06 03:41:46 -0500 )edit
2

It is a good idea to describe the answer and pretend that it has been resolved for those who will follow.

miura gravatar image miura  ( 2021-02-06 22:09:29 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2021-02-09 14:58:16 -0500

Marcus Barnet gravatar image

I found out that the loop_rate() function is working fine, the problem is related to the acquisition time needed by the functions to retrieve the data from the sensors. By skipping some of the functions used to acquire some data, the node runs at the correct rate.

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2021-02-05 13:37:49 -0500

Seen: 379 times

Last updated: Feb 09 '21