How to close a path msg

edit

Short Answer You cannot "close a path" using the nav_msgs/Path message, except for adding a last point that is identical to the first one.

If you only care about visualization, you could write another rviz plugin for the nav_msgs/Path type that takes the path (without any additional points) and displays it the way you want.

Long Answer

I'd say what you want warrants another approach then directly using the nav_msgs/Path anyways...

A few points to consider:

• In your current setup, the start and endpoint would always be at the same position. Even though this might be fine for your current use case, this limits flexibility. You'll always only be able to start at that point. What happens, if you experience something on your mission (failure, event to react to, ...) that requires you to stop following the current path, and restart later. How would you find the point to start from your full path?

• The nav_msgs/Path consists of an array of geometry_msgs/PoseStampeds. See the (expandend) message definition:

  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  geometry_msgs/PoseStamped[] poses
    std_msgs/Header header
      uint32 seq
      time stamp
      string frame_id
    geometry_msgs/Pose pose
      geometry_msgs/Point position
        float64 x
        float64 y
        float64 z
      geometry_msgs/Quaternion orientation
        float64 x
        float64 y
        float64 z
        float64 w
  

  This is 7 floats, a string, a uint32 and a time object per data point. Assuming your mission isn't just a few meters, this will be a lot of data. Except for the position object (three floats), the others are (most probably) unnecessary. (Most probably, because if you want to drive using the same orientation, you'd have to store the orientation as well, obviously. I'm assuming you are not).

• The way you describe your "recording" of data, seems to be a path (actual path), where start and end points are pretty far apart. Otherwise you probably wouldn't suggest to artificially add another "end point" close to the start point. You should probably check if your end point and start point are sufficiently close to each other. Same holds for all other solutions.

Thus, back to my original suggestion: Use a geometry_msgs/Polygon (you actually don't even need the stamped version) for storing the points that you want to drive. If you care about orientation, go for geometry_msgs/PoseArray. Both of those types have an rviz plugin that you can use directly (though the pose array is shown as multiple arrows, so I'm not sure if you'd be satisfied with that).

Then implement a simple logic that creates the path from the polygon once you need to send it to the navigation. For a start, just take the polygon points in order, and you'd be right where you are now. But this could be easily extended to later start and stop anywhere "in between" by searching for the closest point on the path w.r.t. your current position and creating the new path from there.