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");
↧