Multiple callbacks in class. Node is randomly dying

edit

Hi all,

I am quite new to ROS and I have the following problem: I have three node publish each to a specific topic. On node has a class with three subscribers to each of the topics from before. However, this node is randomly dying after some seconds. Here is the code of this node:

#include <iostream>
#include <iomanip>
#include <queue>
#include <string>
#include <math.h>
#include <time.h>
#include <cstring>
#include <cstdlib>
#include <ros/console.h>
#include <fstream>
#include <planner/path_finder.h>
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "planner/point.h"
#include "planner/arrayOfPoints.h"

#define TARGET_TOPIC "/planner/target"
#define PLAN_TOPIC "/planner/plan"
#define ACTUAL_POSITION_TOPIC "/perception/global_position"
#define OBSTACLES_TOPIC "/obstacles"

class Planner
{
    private:
        planner::point actual_position;
        planner::point target;
        pathFinder testPathFinder;
        std::vector<Point> route;
        ros::Publisher plan_pub;
        ros::NodeHandle n;
        std::vector<Point> obs;
    public:
        Planner();
        ~Planner();
        void set_obstacles(const planner::arrayOfPoints & msg);
        void set_actual_position(const planner::point & msg);
        void set_target(const planner::point & msg);
        void compute_plan(void);
        void init_path_finder(void);
        void set_obstacles(void);
};

Planner::Planner()
{
    testPathFinder=pathFinder();
    actual_position.x = 0;
    actual_position.y = 0;
    target.x = 0;
    target.y = 0;
    plan_pub = n.advertise<planner::arrayOfPoints>(PLAN_TOPIC, 1000);
}
Planner::~Planner()
{

}

void Planner::set_obstacles(const planner::arrayOfPoints & msg)
{
    ROS_INFO_STREAM("[PLANNER] New obstacles set");
    std::vector<Point> obs;
    Point tmp;
    for(int i=0;i<sizeof(msg);i++)
    {
        tmp.x = (msg.arrayOfPoints[i]).x;
        tmp.y = (msg.arrayOfPoints[i]).y;
        obs.push_back(tmp);
    }
    testPathFinder.setObstacles(obs);

}

void Planner::set_actual_position(const planner::point & msg)
{
    ROS_INFO_STREAM("[PLANNER] I got my starting point");
    //actual_position = msg;
}

void Planner::set_target(const planner::point & msg)
{
    ros::Time begin = ros::Time::now();
    ROS_INFO_STREAM("[PLANNER] I got a new target"<<msg);
    /*target = msg;
    testPathFinder.printMapToFile();
    route=testPathFinder.getPath(actual_position.x, actual_position.y, msg.x, msg.y);
    planner::point p;
    planner::arrayOfPoints plan;
    for(int i=0;i<route.size();i++)
    {
        p.x = (route[i]).x;
        p.y = (route[i]).y;
        plan.arrayOfPoints.push_back(p);
    }
    plan_pub.publish(plan);*/
    ros::Time end = ros::Time::now();
    ROS_INFO_STREAM("[PLANNER] A new plan was published.");
    ROS_WARN_STREAM("[PLANNER] Generating the plan took:"<< end - begin);
}


using namespace std;






int main(int argc, char **argv)
{
    ros::init(argc, argv, "planner_node");
    ROS_INFO_STREAM("[PLANNER] Planner node initialized");
    ros::NodeHandle n;

    Planner planner;
    ros::Subscriber sub_obstacles = n.subscribe(OBSTACLES_TOPIC, 1, &Planner::set_obstacles, &planner);
    ros::Subscriber sub_actual_position = n.subscribe(ACTUAL_POSITION_TOPIC, 1, &Planner::set_actual_position, &planner);
    ros::Subscriber sub_target = n.subscribe(TARGET_TOPIC, 1, &Planner::set_target, &planner);
    ros::spin();

    return(0);
}

I hope you can help me! Thanks in advance!