From e18dc1328d42fa3aa1e9ede8978e584bc5afaec9 Mon Sep 17 00:00:00 2001 From: Thomas Hettasch Date: Thu, 26 Oct 2023 11:33:04 +0000 Subject: [PATCH 1/3] 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 a588223cc24..3ff39d15bc3 100644 --- a/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp @@ -130,6 +130,7 @@ void UpsampleTrajectoryTask::upsample(CompositeInstruction& composite, if (start_instruction.isNull()) { start_instruction = i.as(); + composite.push_back(i); continue; } From cc0dc2ce650b856a3f2b5af040e689326399d65c Mon Sep 17 00:00:00 2001 From: Thomas Hettasch Date: Wed, 8 Nov 2023 08:49:21 +0000 Subject: [PATCH 2/3] Add clarifying comment for bugfix --- .../planning/src/nodes/upsample_trajectory_task.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 3ff39d15bc3..50fdc5211ab 100644 --- a/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp @@ -130,7 +130,7 @@ void UpsampleTrajectoryTask::upsample(CompositeInstruction& composite, if (start_instruction.isNull()) { start_instruction = i.as(); - composite.push_back(i); + composite.push_back(i); // Prevents loss of very first waypoint when upsampling continue; } From e6c1446f9a6dbcad6822a666cb93c3b3ad196901 Mon Sep 17 00:00:00 2001 From: Thomas Hettasch Date: Wed, 8 Nov 2023 09:43:10 +0000 Subject: [PATCH 3/3] Update tests After bugfix, there is one more waypoint in any plan using Upsample task --- .../tesseract_task_composer_planning_unit.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp index 0f0bb46e470..b436d269f5d 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp @@ -1615,7 +1615,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpsampleTrajectoryTaskTest EXPECT_EQ(node_info->isAborted(), false); EXPECT_EQ(context->isAborted(), false); EXPECT_EQ(context->isSuccessful(), true); - EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 17); + EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 18); EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } @@ -1736,7 +1736,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerIterativeSplineParameteriz UpsampleTrajectoryTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*context), 1); data->setData("input_data", context->data_storage->getData("output_data")); - EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 17); + EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 18); } auto profiles = std::make_shared(); auto problem = std::make_unique(env_, manip_, profiles, "abc"); @@ -1750,7 +1750,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerIterativeSplineParameteriz EXPECT_EQ(node_info->isAborted(), false); EXPECT_EQ(context->isAborted(), false); EXPECT_EQ(context->isSuccessful(), true); - EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 17); + EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 18); EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); } @@ -1874,7 +1874,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerTimeOptimalParameterizatio UpsampleTrajectoryTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*context), 1); data->setData("input_data", context->data_storage->getData("output_data")); - EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 17); + EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 18); } auto profiles = std::make_shared(); auto problem = std::make_unique(env_, manip_, profiles, "abc"); @@ -1888,7 +1888,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerTimeOptimalParameterizatio EXPECT_EQ(node_info->isAborted(), false); EXPECT_EQ(context->isAborted(), false); EXPECT_EQ(context->isSuccessful(), true); - EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 17); + EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 18); EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); // Serialization @@ -2015,7 +2015,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRuckigTrajectorySmoothingT auto context = std::make_unique(std::move(problem), std::move(data2)); UpsampleTrajectoryTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*context), 1); - EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 17); + EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 18); auto data3 = std::make_unique(); data3->setData("input_data", context->data_storage->getData("output_data")); @@ -2024,7 +2024,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRuckigTrajectorySmoothingT TimeOptimalParameterizationTask task2("abc", "input_data", "output_data", true); EXPECT_EQ(task2.run(*context2), 1); data->setData("input_data", context2->data_storage->getData("output_data")); - EXPECT_EQ(context2->data_storage->getData("output_data").as().size(), 17); + EXPECT_EQ(context2->data_storage->getData("output_data").as().size(), 18); } auto profiles = std::make_shared(); auto problem = std::make_unique(env_, manip_, profiles, "abc"); @@ -2038,7 +2038,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRuckigTrajectorySmoothingT EXPECT_EQ(node_info->isAborted(), false); EXPECT_EQ(context->isAborted(), false); EXPECT_EQ(context->isSuccessful(), true); - EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 17); + EXPECT_EQ(context->data_storage->getData("output_data").as().size(), 18); EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); }