From 11db2df7232ce9aa1ae3e488e65c39d7be8c192c Mon Sep 17 00:00:00 2001
From: Thomas Hettasch <thomas.hettasch@gmail.com>
Date: Thu, 26 Oct 2023 11:33:04 +0000
Subject: [PATCH] Fix loss of first waypoint in upsample trajectory

---
 .../planning/src/nodes/upsample_trajectory_task.cpp              | 1 +
 1 file changed, 1 insertion(+)

diff --git a/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp b/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp
index 2d942820e09..4cdbdd0b933 100644
--- a/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp
+++ b/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp
@@ -137,6 +137,7 @@ void UpsampleTrajectoryTask::upsample(CompositeInstruction& composite,
       if (start_instruction.isNull())
       {
         start_instruction = i.as<MoveInstructionPoly>();
+        composite.push_back(i);
         continue;
       }