The loop_rate() for my node doesn't change the publish frequency
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 ...
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.
How do you set the frequency, and have you confirmed that the value is correct after setting it?
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.
Ah, then yes, I have to agree :/
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();
.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!
It is a good idea to describe the answer and pretend that it has been resolved for those who will follow.