From 11db2df7232ce9aa1ae3e488e65c39d7be8c192c 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 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(); + composite.push_back(i); continue; } From bc74936c294721f2fd45f6d9f51f8a67b4692a17 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 4cdbdd0b933..9fdbe4e54b6 100644 --- a/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp @@ -137,7 +137,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 4d9ed52deaf038a0d7fa70ab5531c0f7a26d8a41 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 871128146a1..75258e518f3 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp @@ -1616,7 +1616,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpsampleTrajectoryTaskTest EXPECT_EQ(node_info->isAborted(), false); EXPECT_EQ(input->isAborted(), false); EXPECT_EQ(input->isSuccessful(), true); - EXPECT_EQ(input->data_storage.getData("output_data").as().size(), 17); + EXPECT_EQ(input->data_storage.getData("output_data").as().size(), 18); EXPECT_TRUE(input->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(*input), 1); data.setData("input_data", input->data_storage.getData("output_data")); - EXPECT_EQ(input->data_storage.getData("output_data").as().size(), 17); + EXPECT_EQ(input->data_storage.getData("output_data").as().size(), 18); } auto profiles = std::make_shared(); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); @@ -1750,7 +1750,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerIterativeSplineParameteriz EXPECT_EQ(node_info->isAborted(), false); EXPECT_EQ(input->isAborted(), false); EXPECT_EQ(input->isSuccessful(), true); - EXPECT_EQ(input->data_storage.getData("output_data").as().size(), 17); + EXPECT_EQ(input->data_storage.getData("output_data").as().size(), 18); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); } @@ -1873,7 +1873,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerTimeOptimalParameterizatio UpsampleTrajectoryTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*input), 1); data.setData("input_data", input->data_storage.getData("output_data")); - EXPECT_EQ(input->data_storage.getData("output_data").as().size(), 17); + EXPECT_EQ(input->data_storage.getData("output_data").as().size(), 18); } auto profiles = std::make_shared(); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); @@ -1887,7 +1887,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerTimeOptimalParameterizatio EXPECT_EQ(node_info->isAborted(), false); EXPECT_EQ(input->isAborted(), false); EXPECT_EQ(input->isSuccessful(), true); - EXPECT_EQ(input->data_storage.getData("output_data").as().size(), 17); + EXPECT_EQ(input->data_storage.getData("output_data").as().size(), 18); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); // Serialization @@ -2014,14 +2014,14 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRuckigTrajectorySmoothingT UpsampleTrajectoryTask task("abc", "input_data", "output_data", true); EXPECT_EQ(task.run(*input), 1); data.setData("input_data", input->data_storage.getData("output_data")); - EXPECT_EQ(input->data_storage.getData("output_data").as().size(), 17); + EXPECT_EQ(input->data_storage.getData("output_data").as().size(), 18); auto problem2 = std::make_unique(env_, manip_, data, profiles, "abc"); auto input2 = std::make_unique(std::move(problem2)); TimeOptimalParameterizationTask task2("abc", "input_data", "output_data", true); EXPECT_EQ(task2.run(*input2), 1); data.setData("input_data", input2->data_storage.getData("output_data")); - EXPECT_EQ(input2->data_storage.getData("output_data").as().size(), 17); + EXPECT_EQ(input2->data_storage.getData("output_data").as().size(), 18); } auto profiles = std::make_shared(); auto problem = std::make_unique(env_, manip_, data, profiles, "abc"); @@ -2035,7 +2035,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRuckigTrajectorySmoothingT EXPECT_EQ(node_info->isAborted(), false); EXPECT_EQ(input->isAborted(), false); EXPECT_EQ(input->isSuccessful(), true); - EXPECT_EQ(input->data_storage.getData("output_data").as().size(), 17); + EXPECT_EQ(input->data_storage.getData("output_data").as().size(), 18); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); }