From fee4296c28df045a67de85749159364ffcddf49c Mon Sep 17 00:00:00 2001 From: SteveMacenski Date: Tue, 16 Sep 2025 11:24:58 -0700 Subject: [PATCH 1/4] Adding the Trajectory and trajectoryPoint messages Signed-off-by: SteveMacenski --- nav_msgs/CMakeLists.txt | 2 ++ nav_msgs/README.md | 2 ++ nav_msgs/msg/Trajectory.msg | 7 +++++++ nav_msgs/msg/TrajectoryPoint.msg | 16 ++++++++++++++++ 4 files changed, 27 insertions(+) create mode 100644 nav_msgs/msg/Trajectory.msg create mode 100644 nav_msgs/msg/TrajectoryPoint.msg diff --git a/nav_msgs/CMakeLists.txt b/nav_msgs/CMakeLists.txt index ee3d3c6d..b6b37763 100644 --- a/nav_msgs/CMakeLists.txt +++ b/nav_msgs/CMakeLists.txt @@ -24,6 +24,8 @@ set(msg_files "msg/OccupancyGrid.msg" "msg/Odometry.msg" "msg/Path.msg" + "msg/Trajectory.msg" + "msg/TrajectoryPoint.msg" ) set(srv_files "srv/GetMap.srv" diff --git a/nav_msgs/README.md b/nav_msgs/README.md index d30926b4..6856323a 100644 --- a/nav_msgs/README.md +++ b/nav_msgs/README.md @@ -12,6 +12,8 @@ For more information about ROS 2 interfaces, see [docs.ros.org](https://docs.ros * [OccupancyGrid](msg/OccupancyGrid.msg): Represents a 2-D grid map, in which each cell represents the probability of occupancy. * [Odometry](msg/Odometry.msg): This represents an estimate of a position and velocity in free space. * [Path](msg/Path.msg): An array of poses that represents a Path for a robot to follow. +* [Trajectory](msg/Trajectory.msg): A representation of a trajectory as a series of time-stamped trajectory points. +* [TrajectoryPoint](msg/TrajectoryPoint.msg): A single point in a trajectory. ## Services (.srv) * [GetMap](srv/GetMap.srv): Get the map as a nav_msgs/OccupancyGrid. diff --git a/nav_msgs/msg/Trajectory.msg b/nav_msgs/msg/Trajectory.msg new file mode 100644 index 00000000..defa0d1d --- /dev/null +++ b/nav_msgs/msg/Trajectory.msg @@ -0,0 +1,7 @@ +# An array of trajectory points that represents a trajectory for a robot to follow. + +# Indicates the frame_id and timestamp of the trajectory. +std_msgs/Header header + +# Array of trajectory points to follow. +TrajectoryPoint[] points diff --git a/nav_msgs/msg/TrajectoryPoint.msg b/nav_msgs/msg/TrajectoryPoint.msg new file mode 100644 index 00000000..d1f067e3 --- /dev/null +++ b/nav_msgs/msg/TrajectoryPoint.msg @@ -0,0 +1,16 @@ +# Trajectory point state + +# Absolute time and frame of reference of the point along a trajectory +std_msgs/Header header + +# Pose of the trajectory sample. +geometry_msgs/Pose pose + +# Velocity of the trajectory sample. +geometry_msgs/Twist velocity + +# Acceleration of the trajectory (optional, NaN if not set). +geometry_msgs/Accel acceleration + +# Force/Torque to apply at trajectory sample (optional, NaN if not set). +geometry_msgs/Wrench effort From 50e05fedb324ff8632319c307ad0b75af943c4ed Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 1 Oct 2025 13:29:11 -0700 Subject: [PATCH 2/4] Update nav_msgs/msg/TrajectoryPoint.msg Co-authored-by: Tully Foote Signed-off-by: Steve Macenski --- nav_msgs/msg/TrajectoryPoint.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav_msgs/msg/TrajectoryPoint.msg b/nav_msgs/msg/TrajectoryPoint.msg index d1f067e3..834900d5 100644 --- a/nav_msgs/msg/TrajectoryPoint.msg +++ b/nav_msgs/msg/TrajectoryPoint.msg @@ -9,7 +9,7 @@ geometry_msgs/Pose pose # Velocity of the trajectory sample. geometry_msgs/Twist velocity -# Acceleration of the trajectory (optional, NaN if not set). +# Acceleration of the trajectory (optional, linear.x component is NaN if not set). geometry_msgs/Accel acceleration # Force/Torque to apply at trajectory sample (optional, NaN if not set). From 050d19b8fed1a1fb2716c2871f49c4ef2b4d67f4 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 1 Oct 2025 13:29:17 -0700 Subject: [PATCH 3/4] Update nav_msgs/msg/TrajectoryPoint.msg Co-authored-by: Tully Foote Signed-off-by: Steve Macenski --- nav_msgs/msg/TrajectoryPoint.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav_msgs/msg/TrajectoryPoint.msg b/nav_msgs/msg/TrajectoryPoint.msg index 834900d5..dde4b17f 100644 --- a/nav_msgs/msg/TrajectoryPoint.msg +++ b/nav_msgs/msg/TrajectoryPoint.msg @@ -12,5 +12,5 @@ geometry_msgs/Twist velocity # Acceleration of the trajectory (optional, linear.x component is NaN if not set). geometry_msgs/Accel acceleration -# Force/Torque to apply at trajectory sample (optional, NaN if not set). +# Force/Torque to apply at trajectory sample (optional, force.x component is NaN if not set). geometry_msgs/Wrench effort From ffb769e4c53b08e0b8c92e8acb3a5324456adea9 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 1 Oct 2025 13:29:29 -0700 Subject: [PATCH 4/4] Update nav_msgs/msg/Trajectory.msg Co-authored-by: Tully Foote Signed-off-by: Steve Macenski --- nav_msgs/msg/Trajectory.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav_msgs/msg/Trajectory.msg b/nav_msgs/msg/Trajectory.msg index defa0d1d..cb796629 100644 --- a/nav_msgs/msg/Trajectory.msg +++ b/nav_msgs/msg/Trajectory.msg @@ -1,6 +1,6 @@ # An array of trajectory points that represents a trajectory for a robot to follow. -# Indicates the frame_id and timestamp of the trajectory. +# Indicates the frame_id in which the planner ran and timestamp represents when the trajectory was generated. std_msgs/Header header # Array of trajectory points to follow.