Fix loss of first waypoint in upsample trajectory

tesseract-robotics · Oct 26, 2023 · 11db2df · 11db2df
1 parent b64af98
commit 11db2df
Showing 1 changed file with 1 addition and 0 deletions.
diff --git 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;
       }