Nodes are takin 100% cpu on any computer

edit

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;
}