Loading a plan to MoveGroupInterface::Plan

edit

Hey all!

I have a set of waypoints on a txt file, which I wanna use to compute a plan. (I'm doing this because Moveit does not reproduce the same path/trajectory when I plan it everytime).

My questions is, how do I load a set of points to Plan and compute a new plan ? For now I have the code below, But i gives a segmentation fault since I'm not able to define the array size to moveit_msgs::RobotTrajectory msg (there is no way to do it)

std::ifstream infile("wet_points.txt");
  ROS_INFO( "Loaded file");

  float a,b,c;

  moveit_msgs::RobotTrajectory msg;  
  msg = my_plan.trajectory_;


  while (infile >> a >> b >> c)
{
    msg.joint_trajectory.points[count].positions[0] = a;
    msg.joint_trajectory.points[count].positions[1] = b;
    msg.joint_trajectory.points[count].positions[2] = c;


}
  my_plan.trajectory_ = msg;
  move_group.plan(my_plan);
  ROS_INFO( "New plan planned");