Ros Node Hang up

edit

Hi,

I am writing a subscriber node with a long callback function. The node calls the callback function once or twice and then hangs up sometimes inside the callback function or sometimes outside the callback function. I have been able to detect where it hangs by publishing ROS_INFO messages at various points in the code.

Inside the callback function, I modify global variables (matrix). Could that be the reason for the node getting stuck during runtime ?

Any other guesses for why a node will hang such that I cant even ctrl+c and stop it ?

Here is the code:

#include "ros/ros.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <Eigen/Dense>
#include <Eigen/LU>
#include "std_msgs/String.h"
#include <std_msgs/Float32.h>
#include <std_msgs/Int32.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <coop_est/FeatureB.h>
#include <coop_est/FeatureBArray.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_datatypes.h>
#include "/opt/ros/hydro/include/tf/LinearMath/Matrix3x3.h"
#include "/opt/ros/hydro/include/tf/LinearMath/Transform.h"
#include "tf/tf.h"
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

using namespace Eigen;
using namespace message_filters;
using namespace sensor_msgs;

// Filter Initialization

VectorXf X(3); // Define Starting position and orientation as the inertial frame
MatrixXf Q(3,3);
MatrixXf P(3,3);

void FeaturesReceived(const coop_est::FeatureBArray::ConstPtr& msg, const nav_msgs::Odometry::ConstPtr& odo) /
{

   DO A NUMBER OF MATRIX OPERATIONS ON THE GLOBAL MATRICES Q, P, Xf

}


int main(int argc, char** argv)
{

  ros::init(argc, argv, "coop_node");

  ros::NodeHandle n;

  int should_continue = 1;
  ros::Rate r(5); // 10 hz


  message_filters::Subscriber<coop_est::FeatureBArray> fea_sub(n, "Feature", 1);
  message_filters::Subscriber<nav_msgs::Odometry> odo_sub(n, "odom", 1);
  typedef sync_policies::ApproximateTime<coop_est::FeatureBArray, nav_msgs::Odometry> MySyncPolicy;
  Synchronizer<MySyncPolicy> sync(MySyncPolicy(1), fea_sub, odo_sub);
  sync.registerCallback(boost::bind(&FeaturesReceived, _1, _2));

  ros::spin();

  return 0;

}

------ UPDATE------

After making the callback function much simpler, I found that the callback function got stuck at the same line repeatedly. I have a for loop inside the callback function and when I change the number of iterations on the for loop, the place where the node gets stuck changes. If the number of iterations are very low then the node does not get stuck.

void FeaturesReceived(const coop_est::FeatureBArray::ConstPtr& msg, const nav_msgs::Odometry::ConstPtr& odo) /
{

for (int i = 0; i< n; i++)
{
   DO A NUMBER OF MATRIX OPERATIONS ON THE GLOBAL MATRICES Q, P, Xf
}
}

My thoughts are that as I am operating on the global Eigen2 type matrix inside the callback, when a second callback is created before the execution of the first one is complete, the program hangs as two callback instances are trying to modify the same memory location.

How can I sequentially execute the callback in ROS and wait until the previous callback has fnished executing ?

Thank you for your help !