jeudi 4 mars 2021

Vector of Object and pointer in Class Object - Core Dump (segmentation fault)

I'm trying to create a ROS node in c++11 that would take coordinates stored in a json file to publish them on a particular rostopic (json file is enclosed for you to see the structure).

I decided to create a class object called "DroneTopic" that will read the logs from the json file, create a publisher object and then publish in it.

The main issue I'm facing is the use of a pointer. As the Json type from the library I'm using (jsoncpp) are special type (const signed int for ex) I have to use pointer to take the value and transfer it as an int to the right attribute.

I also use a vector of this class object to handle easier the number of object as I don't know in the beginning how many rostopic I will have to deal with.

The same situation happens inside the class object regarding the number of waypoints. All my drone object won't have the same number of waypoint, that's why I created a vector of a structure (x,y,z) to handle the changing number.

Here is my full code:

  class DroneTopic
    {
      protected:
        struct Waypoint{
          int x,y,z;
        };
        std::string topicName;
        ros::Publisher pub;
        std::vector<Waypoint> waypoints;
    
      public:
        DroneTopic(std::string topic,ros::NodeHandle &nh)
        {
          topicName = topic;
          pub = nh.advertise<geometry_msgs::Pose>(topicName,1000);
        }
        //Methodes
        std::vector<Waypoint> read_logs(Json::Value &logs){
          int nb_waypts = logs.size();
          Json::Value *ptr;
          Waypoint tmp {0,0,0};
    
          for(int i=0; i < nb_waypts; i++){
            ptr = &logs[i]["x"];
            tmp.x = ptr->asInt();
            ptr = &logs[i]["y"];
            tmp.y = ptr->asInt();
            ptr = &logs[i]["z"];
            tmp.z = ptr->asInt();
            waypoints.push_back(tmp);
            std::cout << i << std::endl;
          }
          delete ptr;
          ptr=NULL;
        }
    
        void publish_message()
        {
          for(int i = 0; i<waypoints.size();i++){
            geometry_msgs::Pose msg;
            msg.position.x=waypoints[i].x;
            msg.position.y=waypoints[i].y;
            msg.position.z=waypoints[i].z;
            pub.publish(msg);
          }
        }
    
    };
    
    
    std::string filename;
    
    int main(int argc, char **argv)
    {
      //Init ROS Node
      ros::init(argc,argv,"mesa2drone");
      ros::NodeHandle nh;
      ROS_INFO_STREAM("MESA Node Up");
      nh.param<std::string>("Json_File_Name", filename, "output");
      std::string extension = ".json";
      filename+=extension;
    
      Json::Value logs;
      std::ifstream jfile(filename);
    
      jfile >> logs;
    
      // Create vector of DroneTopic
      std::vector<DroneTopic> drone;
    
      drone.push_back(DroneTopic("test",nh));
      int nb_drone = logs.size();
    
      if(nb_drone > 0){
        for(int i=1;i<=nb_drone;i++){
          std::string j = std::to_string(i);
          std::ostringstream flux;
          flux << "intelaero_laserscan_" << j <<"/new_target";
          std::string name =  flux.str();
          std::cout<< "ok_3" <<std::endl;
          drone.push_back(DroneTopic(name,nh));
          std::cout<< "ok_5" <<std::endl;
        }
      }
    
      ros::Rate rate(1);
      while(ros::ok())
      {
        for(int k=1; k<=nb_drone;k++)
        {
          std::cout<<"ok_6"<<std::endl;
          std::string j = std::to_string(k);
          drone[k].read_logs(logs[j]);
          std::cout<<"ok_7"<<std::endl;
          drone[k].publish_message();
          std::cout<<"ok_8"<<std::endl;
        }
    
        //j++;
        rate.sleep();
      }
    
      ros::shutdown();
    
      return 0;
    }

I used (but erased for clarity sake) std::cout marker to see where the code is crashing and it seems that it crashes at this line:

drone[k].read_logs(logs[j]);

It never actually gets out of the function. It executes the full "for-loop" but for some reason I don't get it doesn't get out and gives me this error:

Erreur de segmentation (core dumped)

From what I saw it has to do with a wrong assignation, but I checked the size of my vector and everything is ok (or so I think).

Any help would be appreciated as I'm struggling with this for a couple days.

Thank you !

FIY: I'm using Linux Ubuntu 16, c++11 (not my choice).

PS: here is the json file I'm using

{
 "1": [
  {
   "x": -2,
   "y": 27.5,
   "z": 20.0
  },
  {
   "x": -2,
   "y": 67.5,
   "z": 12.0
  },
  {
   "x": -2,
   "y": 91.5,
   "z": 11.5
  },
  {
   "x": -2,
   "y": 89.5,
   "z": 9.5
  },
  {
   "x": -2,
   "y": 23.0,
   "z": 17.0
  },
  {
   "x": -2,
   "y": 50.5,
   "z": 1.5
  },
  {
   "x": -2,
   "y": 82.5,
   "z": 5.5
  },
  {
   "x": -2,
   "y": 97.5,
   "z": 21.0
  }
 ],
 "2": [
  {
   "x": -2,
   "y": 82.5,
   "z": 20.0
  },
  {
   "x": -2,
   "y": 64.0,
   "z": 0.0
  },
  {
   "x": -2,
   "y": 160.5,
   "z": 11.0
  },
  {
   "x": -2,
   "y": 85.5,
   "z": 14.0
  },
  {
   "x": -2,
   "y": 132.0,
   "z": 19.0
  },
  {
   "x": -2,
   "y": 146.0,
   "z": 1.0
  }
 ],
 "3": [
  {
   "x": -2,
   "y": 137.5,
   "z": 20.0
  },
  {
   "x": -2,
   "y": 156.5,
   "z": 17.5
  },
  {
   "x": -2,
   "y": 28.0,
   "z": 4.0
  },
  {
   "x": -2,
   "y": 67.0,
   "z": 15.0
  },
  {
   "x": -2,
   "y": 40.0,
   "z": 9.5
  },
  {
   "x": -2,
   "y": 81.0,
   "z": 5.0
  },
  {
   "x": -2,
   "y": 77.0,
   "z": 0.0
  }
 ]
}


Aucun commentaire:

Enregistrer un commentaire