Multiple callbacks in class. Node is randomly dying
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!
Did you check that your "sizeof(msg)" returns the value you want ? it returns a size in bytes not the number of elements in array. It could lead to an out-of-bound access