How to publish a nav_msgs::Path

edit

Hello to everyone. I'm implementing an APF motion planner ant I would the position setpoints generated online to be saved in a nav_msgs::Path msg that I called _planned_path. At each loop i would the new setpoint in position to be added in the path msg. Every time I try to run I get a segmentation fault error. This is my code implementation:

void NAVIGATION::planner(){
   bool moved=false;
   bool rotated=false;
   ros::Rate plan_r(100);
   ros::Rate r_wait(10);
   _planned_path.header.stamp = ros::Time::now();
   _planned_path.header.frame_id = "robot_footprint";
   while(!_first_odom)
      r_wait.sleep();

   while(ros::ok()){
      if(_local_minima){
         local_minima_manager();
      }
      else{
         art_pot();
      }
      _planned_path.poses[_contatore_path].header.stamp = ros::Time::now();
      _planned_path.poses[_contatore_path].header.frame_id = "robot_footprint"; 
      _planned_path.poses[_contatore_path].pose.position.x=(long double)_sp_xy[0];
      _planned_path.poses[_contatore_path].pose.position.y=(long double)_sp_xy[1];
      cout<<_planned_path.poses[_contatore_path].pose.position.x<<endl;
      _planned_path.poses[_contatore_path].pose.position.z = 0.0;
      _contatore_path++;
      _plan_path_pub.publish(_planned_path);
      plan_r.sleep();
   }
}

How may I solve this problem? _contatore_path is a class member variable I set to 0 at the start of the code.