Nodes are takin 100% cpu on any computer
Hi,
I am using couple callback queues in my nodes with asyncSpinner. It does not matter on which computer I'm placing it (Odroid XU4, NUC) it does consume 100% CPU. Can anyone point o what I'm missing in these?
#include "ros/ros.h"
#include "ros/spinner.h"
#include "ros/callback_queue.h"
#include "sensor_msgs/LaserScan.h"
#include "mavros_msgs/State.h"
#include "std_msgs/Bool.h"
class SubscribeAndPublish
{
private :
ros::NodeHandle nh_1;
ros::NodeHandle nh_2;
// Subscribers and publishers
ros::Subscriber sub_range;
ros::Subscriber sub_system_state;
ros::Publisher pub_block_state;
bool is_armed; // parameter check that vehicle is armed
bool on_hold = false; // parameter, stating that hold sequence initiated
std::string flight_mode;
float range;
float range_threshold = 1.2;
float range_min = 0.011;
double time_start; // parameter for checking, when hold sequence initiated
double time_end = 0.0; // parameter for checking, when hold sequence ended
uint16_t range_count = 0;
uint16_t last_obstacle_before = 0;
bool last_on_hold = false;
bool avoidance_active = false;
public :
SubscribeAndPublish(ros::NodeHandle &nh_simple, ros::NodeHandle &nh_state);
void system_state_callback(const mavros_msgs::State::ConstPtr& state_msg);
void range_callback(const sensor_msgs::LaserScan::ConstPtr& range_msg);
bool run();
};
SubscribeAndPublish::SubscribeAndPublish(ros::NodeHandle &nh_simple, ros::NodeHandle &nh_state)
{
nh_1 = nh_simple;
nh_2 = nh_state;
sub_system_state = nh_2.subscribe("mavros/state", 2, &SubscribeAndPublish::system_state_callback, this);
pub_block_state = nh_1.advertise< std_msgs::Bool >("/block_state", 10);
sub_range = nh_1.subscribe("/scan", 10, &SubscribeAndPublish::range_callback, this);
}
// Callback Handling
void SubscribeAndPublish::range_callback(const sensor_msgs::LaserScan::ConstPtr& range_msg)
{
std_msgs::Bool msg_block;
avoidance_active = ((is_armed) && ((flight_mode == "LEARNING") || (flight_mode == "AUTO") || (flight_mode == "GUIDED") ));
//ROS_INFO("Got mode of: %s", flight_mode.c_str());
for (unsigned int i = 0; i < range_msg->ranges.size(); i++) {
range = range_msg->ranges[i];
if (std::isinf(range_msg->ranges[i])) {
continue;
}
if (!avoidance_active) {
last_obstacle_before = 1000;
} else {
if (range > range_threshold || range < range_min) {
last_obstacle_before++;
} else {
last_obstacle_before = 0;
range_count ++;
}
}
if (last_obstacle_before %100 == 0) {
//ROS_INFO("Last %d", last_obstacle_before);
//ROS_INFO("Avoidance_active %d", avoidance_active);
}
last_on_hold = on_hold;
on_hold = last_obstacle_before < 250 and avoidance_active;
if (last_obstacle_before > 15000) {
last_obstacle_before = 1000;
if (!ros::ok()) {
return;
}
if (avoidance_active) {
//ROS_INFO("RELEASE NMESSAGE 1500");
on_hold = false;
msg_block.data = false;
pub_block_state.publish(msg_block);
}
}
if (on_hold != last_on_hold) {
if (!(ros::ok())) {
return;
}
if (on_hold) {
//ROS_INFO("ON HOLD NMESSAGE");
on_hold = true;
msg_block.data = true;
pub_block_state.publish(msg_block);
} else {
//ROS_INFO("RELEASE MESSAGE");
on_hold = false;
msg_block.data = false;
pub_block_state.publish(msg_block);
}
}
}
return;
}
void SubscribeAndPublish::system_state_callback(const mavros_msgs::State::ConstPtr& state_msg)
{
is_armed = state_msg->armed;
flight_mode = state_msg->mode.c_str();
//ROS_INFO("(state callback) Got mode of: %s", flight_mode.c_str());
//ROS_INFO("Got arming state of: %d", is_armed;
}
bool SubscribeAndPublish::run()
{
while((nh_1.ok()) && (nh_2.ok()))
{
ros::spinOnce();
}
return true;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "listener");
ROS_INFO("Starting MIO node");
usleep(5000000);
ros::NodeHandle nh;
ros::CallbackQueue my_callback_queue0;
ros::AsyncSpinner spinner0(1, &my_callback_queue0);
nh.setCallbackQueue(&my_callback_queue0);
spinner0.start();
usleep(5000000);
ros::NodeHandle nh_state;
ros::CallbackQueue my_callback_queue1;
ros::AsyncSpinner spinner1(1, &my_callback_queue1);
nh_state.setCallbackQueue(&my_callback_queue1);
spinner1.start();
SubscribeAndPublish mysupernode(nh, nh_state);
mysupernode.run();
return 0;
}