Proposal - New nav_msgs/Trajectory message type

[SteveMacenski]

Description

In line with having sharp, pointed tools, I think we need a nav_msgs/Trajectory message to augment nav_msgs/Path to include time series information from modern controllers like MPC and Reinforcement Learning.

The Path only includes geometry_msgs/PoseStamped for each via point and we also need to be able to communicate velocity, acceleration, and possibly force/torque.

Motivation

See description above.

Design / Implementation Considerations

There already exists today the trajectory_msgs package, but these broadly apply to manipulators or multi-joint actuation systems. I don't believe that in the theme of conversations we've had recently on Goals vs PoseStampedArray that reusing that for a vasting different application is the maintainer's intent.

I am creating temporary versions of these in nav2_msgs so we can keep moving forward, but given the dominance of MPC-based techniques (and RL, others) that have time-varying trajectory outputs, I think this is an important general standard for ROS interfaces

Additional Information

I propose the following, which make full use of standard geometry_msgs fields for each data type, with some similar structural analogs to the trajectory_msgs definitions:

nav_msgs/Trajectory

# An array of trajectory points that represents a trajectory for a robot to follow.

# Indicates the frame_id of the trajectory.
std_msgs/Header header

# Array of trajectory points to follow.
TrajectoryPoint[] points

nav_msgs/TrajectoryPoint

# Trajectory point state

# Desired time from the trajectory start to arrive at this trajectory sample.
builtin_interfaces/Duration time_from_start

# Pose of the trajectory sample.
geometry_msgs/Pose pose

# Velocity of the trajectory sample.
geometry_msgs/Twist velocity

# Acceleration of the trajectory (optional).
geometry_msgs/Accel acceleration

# Force/Torque to apply at trajectory sample (optional).
geometry_msgs/Wrench effort

[SteveMacenski]

See ros-navigation/navigation2#5057 for example use-case

[SteveMacenski]

@tfoote can you take a look at this when you have a moment?

[tfoote]

Thanks for the ping sorry I didn't see this earlier.

I think that you're right that the navigation trajectory makes sense as a well defined entity which is used consistently in a specific manner for navigation.

I've got a handful of thoughts from my first read through.

My first thought is that we need to be encoding the expected reference frame into the message. (For example is the trajectory for base_footprint, or base_link, or some other point.) Most of the time it likely will work, but we should find a way to be explicit when handing off between systems which might make different assumptions.

I understand the choice of a Duration offset for each point. That's the value as computed often in the algorithm, but it might not be the best way to share it. The storage of the points with the time offset versus the absolute time I'd like to understand if it's something that is being reused regularly or being regenerated at every cycle. If as I think it's being regenerated at every cycle (every TrajectoryPoint will be slowly getting closer and closer to zero as we progress down the trajectory so at every update the value will have to change. If instead of storing the time position as a delta from another number it would actually make sense to store the value of T when the TrajectoryPoint is expected to be passed. This allows all consumers to view render and process the TrajectoryPoints without looking up another value elsewhere and then summing them before doing a lookup or other operation on the data.

And continuing that line of thought when i think about rendering a TrajectoryPoint I would really like it to be able to be self contained. The most complete of this would be a full Header with timestamp and frame_id. I'm assuming that has been eliminated for space /speed optimization. What are common scales of number of TrajectoryPoints per Trajectory in current usage? I'd like to understand how much this optimization is saving as it trades off against the self contained nature of the submessages.

And for the Wrench and acceleration if it's optional we should make sure to have a way to encode if it's zero or not there. Maybe a NaN logic would be enough, or we can use an extra field. But I think at least a comment would be important to pick how to define not-supplied.

And lastly if we're thinking about these as goal trajectories should we be providing space for tolerances for what's considered close enough etc. That may be the wrong level of abstraction for this message but I wanted to bring it up before we go forward without it.

[SteveMacenski]

My first thought is that we need to be encoding the expected reference frame into the message.
The most complete of this would be a full Header with timestamp and frame_id

Meaning, TrajectoryPoint? It is in the header of Trajectory. I don't feel strongly, so sure! We could either use a Header , or add in a separate frame string. I think header would be nice, but it would need to be OK to semantically know that these aren't representing the computation time of the Trajectory Point, but rather the timestamp in which that Point is representing. That's not usually how Header is used in my experience but I think it is a correct usage.

Thoughts?

If instead of storing the time position as a delta from another number it would actually make sense to store the value of T when the TrajectoryPoint is expected to be passed

Agreed, especially if we use a Header.

But I think at least a comment would be important to pick how to define not-supplied.

I think this is the best course. There are several, especially sensor_msgs, that don't have explicit flags about its complete implementation, its just that laser_msg.intensities.size() == 0 or getYaw(imu_msg.orientation) == 0.0.

And lastly if we're thinking about these as goal trajectories should we be providing space for tolerances for what's considered close enough etc. That may be the wrong level of abstraction for this message but I wanted to bring it up before we go forward without it.

Mhm, interesting. I agree that would be useful, but we also don't do that with nav_msgs or trajectory_msgs to date, so I'd probably air on the side of 'not' for consistency reasons alone. If constraints like that were introduced, I think there are many shapes they could take which would never quite fulfill everyone's needs. Though, I think in an announcement on Discourse for comment about this, that can be a point brought up for feedback.

---

So what I'm taking away is to use a Header with a frame for each TrajectoryPoint (TrajectorySample?) + change the timestamp to be absolute and not relative. Also, add a comment about the nature of the acceleration and F/T if not populated.

I am on board with that. Let me know what you think and if I'm OK to open up a draft PR with this.

[tfoote]

I think header would be nice, but it would need to be OK to semantically know that these aren't representing the computation time of the Trajectory Point, but rather the timestamp in which that Point is representing. That's not usually how Header is used in my experience but I think it is a correct usage.

Sorry yes I didn't clarify but the TrajectoryPoint was my thought. For the header it's completely fine to do that. That's just the semantic difference between a sensor detection and a command or goal. Goals should always be in the future. If we are worried about the command going out of date we could add an expiration time or another field to capture the generation time to let downstream processing compute their own timeouts.

I think this is the best course. There are several, especially sensor_msgs, that don't have explicit flags about its complete implementation, its just that laser_msg.intensities.size() == 0 or getYaw(imu_msg.orientation) == 0.0.

Yeah, but since these fields are fixed size you can't check size. I'd suggest something like "If unused set twist.linear.x to NaN."

Other examples:
https://github.com/search?q=repo%3Aros2%2Fcommon_interfaces+path%3A%2F%5Esensor_msgs%5C%2Fmsg%5C%2F%2F+nan&type=code

So what I'm taking away is to use a Header with a frame for each TrajectoryPoint (TrajectorySample?) + change the timestamp to be absolute and not relative. Also, add a comment about the nature of the acceleration and F/T if not populated.
I am on board with that. Let me know what you think and if I'm OK to open up a draft PR with this.

Sounds great. The one other element I'd suggest might be to clarify the header semantics for the Trajectory with a comment to the effect.

• The frame_id in the Header represents the coordinate frame the trajectory was planned in. (For example it might be useful to know if it was planned in odom or map to allow advanced behaviors in terms of dealing with realignment/localization jumps.)
• The timestamp in the Header represents the time at which the Trajectory was computed

[Timple]

Yeah, but since these fields are fixed size you can't check size. I'd suggest something like "If unused set twist.linear.x to NaN."

Would it make sense to make that the default then?
If people don't set it, it's automatically indicated as unused. Instead of accidentally communicated as zero.

[SteveMacenski]

Would it make sense to make that the default then?

I don't think we can set defaults for complex messages like Wrench inside the msg definition. I think defaults are only for trivial types, unless we update Wrench and others to have themselves a default of NaN

[SteveMacenski]

PR: #296