From 004f3037231849736e97266feecd26c5d12fd276 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 6 Sep 2023 14:23:38 +0000 Subject: [PATCH 1/6] Handle case where execute is called with single SubTrajectory --- .../src/execute_task_solution_capability.cpp | 20 ++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 05171582e..0c99bd42e 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -193,26 +193,28 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr auto make_description = [size = solution.sub_trajectory.size()](const std::size_t index) { return std::to_string(index + 1) + "/" + std::to_string(size); }; - // TODO: Case for only one subtrajectory - if (!make_executable_trajectory(solution.sub_trajectory[0], make_description(0), - make_apply_planning_scene_diff_cb({ solution.sub_trajectory[0].scene_diff, - solution.sub_trajectory[1].scene_diff }))) - return false; - for (size_t i = 1; i < solution.sub_trajectory.size() - 1; ++i) { + + // always include initial scene diff + std::vector scene_diffs = { solution.sub_trajectory[0].scene_diff }; + for (size_t i = 0; i < solution.sub_trajectory.size(); ++i) { const moveit_task_constructor_msgs::msg::SubTrajectory& sub_traj = solution.sub_trajectory[i]; // define individual variable for use in closure below const std::string description = make_description(i); - if (!make_executable_trajectory(sub_traj, description, - make_apply_planning_scene_diff_cb({ solution.sub_trajectory[i + 1].scene_diff }))) + + // apply scene diff of following sub trajectory + if (i < solution.sub_trajectory.size() - 1) { + scene_diffs.push_back(solution.sub_trajectory[i + 1].scene_diff); + } + if (!make_executable_trajectory(sub_traj, description, make_apply_planning_scene_diff_cb(scene_diffs))) return false; if (!moveit::core::isEmpty(sub_traj.scene_diff.robot_state) && !moveit::core::robotStateMsgToRobotState(sub_traj.scene_diff.robot_state, state, true)) { RCLCPP_ERROR_STREAM(LOGGER, "invalid intermediate robot state in scene diff of subtrajectory " << description); return false; } + scene_diffs.clear(); } - make_executable_trajectory(solution.sub_trajectory.back(), make_description(solution.sub_trajectory.size() - 1), {}); return true; } From 968869ea35a733a12452f0cb2a4b6fa5219f6bed Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Mon, 11 Sep 2023 14:33:57 +0000 Subject: [PATCH 2/6] Set sub trajectory vector capacity to 1 --- capabilities/src/execute_task_solution_capability.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 0c99bd42e..4b7c7f513 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -196,6 +196,7 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr // always include initial scene diff std::vector scene_diffs = { solution.sub_trajectory[0].scene_diff }; + scene_diffs.reserve(1); // number of diffs used by all sub trajectories besides the first one for (size_t i = 0; i < solution.sub_trajectory.size(); ++i) { const moveit_task_constructor_msgs::msg::SubTrajectory& sub_traj = solution.sub_trajectory[i]; From 0985f1b0de9553c5214c9e29b6a8a6f4618dbe2b Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Mon, 25 Sep 2023 15:55:39 +0000 Subject: [PATCH 3/6] Fix clang-tidy --- .../src/execute_task_solution_capability.cpp | 33 +++++++++---------- .../src/execute_task_solution_capability.h | 2 +- 2 files changed, 17 insertions(+), 18 deletions(-) diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 4b7c7f513..e13d0d2f5 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -94,14 +94,14 @@ void ExecuteTaskSolutionCapability::initialize() { std::placeholders::_1, std::placeholders::_2)), ActionServerType::CancelCallback( std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this, std::placeholders::_1)), - [this](const std::shared_ptr> goal_handle) { - last_goal_future_ = - std::async(std::launch::async, &ExecuteTaskSolutionCapability::goalCallback, this, goal_handle); + [this](std::shared_ptr> goal_handle) { + last_goal_future_ = std::async(std::launch::async, &ExecuteTaskSolutionCapability::goalCallback, this, + std::move(goal_handle)); }); } void ExecuteTaskSolutionCapability::goalCallback( - const std::shared_ptr>& goal_handle) { + const std::shared_ptr> goal_handle) { auto result = std::make_shared(); const auto& goal = goal_handle->get_goal(); @@ -176,19 +176,18 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr }; auto make_apply_planning_scene_diff_cb = [this](const std::vector& scene_diffs) { - return - [this, scene_diffs = std::move(scene_diffs)](const plan_execution::ExecutableMotionPlan* /*plan*/) mutable { - for (auto& scene_diff : scene_diffs) { - if (!moveit::core::isEmpty(scene_diff)) { - /* RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description); */ - scene_diff.robot_state.joint_state = sensor_msgs::msg::JointState(); - scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::msg::MultiDOFJointState(); - if (!context_->planning_scene_monitor_->newPlanningSceneMessage(scene_diff)) - return false; - } - } - return true; - }; + return [=, this](const plan_execution::ExecutableMotionPlan* /*plan*/) mutable { + for (auto& scene_diff : scene_diffs) { + if (!moveit::core::isEmpty(scene_diff)) { + /* RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description); */ + scene_diff.robot_state.joint_state = sensor_msgs::msg::JointState(); + scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::msg::MultiDOFJointState(); + if (!context_->planning_scene_monitor_->newPlanningSceneMessage(scene_diff)) + return false; + } + } + return true; + }; }; auto make_description = [size = solution.sub_trajectory.size()](const std::size_t index) { return std::to_string(index + 1) + "/" + std::to_string(size); diff --git a/capabilities/src/execute_task_solution_capability.h b/capabilities/src/execute_task_solution_capability.h index b79ec17db..ce64e9ad2 100644 --- a/capabilities/src/execute_task_solution_capability.h +++ b/capabilities/src/execute_task_solution_capability.h @@ -63,7 +63,7 @@ class ExecuteTaskSolutionCapability : public MoveGroupCapability bool constructMotionPlan(const moveit_task_constructor_msgs::msg::Solution& solution, plan_execution::ExecutableMotionPlan& plan); - void goalCallback(const std::shared_ptr>& goal_handle); + void goalCallback(const std::shared_ptr> goal_handle); rclcpp_action::CancelResponse preemptCallback(const std::shared_ptr>& goal_handle); From e3e599379cbbdf369c6db734a42b3adc916002f6 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 18 Oct 2023 09:27:29 +0200 Subject: [PATCH 4/6] Fix clang tidy --- capabilities/src/execute_task_solution_capability.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 184b01663..e1707d2d1 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -101,7 +101,7 @@ void ExecuteTaskSolutionCapability::initialize() { } void ExecuteTaskSolutionCapability::goalCallback( - const std::shared_ptr> goal_handle) { + const std::shared_ptr>& goal_handle) { auto result = std::make_shared(); const auto& goal = goal_handle->get_goal(); From f5f29b746b41dbbebde1d467501fa6c667991aca Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 18 Oct 2023 09:53:48 +0200 Subject: [PATCH 5/6] Fix cleanup mistake --- capabilities/src/execute_task_solution_capability.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/capabilities/src/execute_task_solution_capability.h b/capabilities/src/execute_task_solution_capability.h index ce64e9ad2..b79ec17db 100644 --- a/capabilities/src/execute_task_solution_capability.h +++ b/capabilities/src/execute_task_solution_capability.h @@ -63,7 +63,7 @@ class ExecuteTaskSolutionCapability : public MoveGroupCapability bool constructMotionPlan(const moveit_task_constructor_msgs::msg::Solution& solution, plan_execution::ExecutableMotionPlan& plan); - void goalCallback(const std::shared_ptr> goal_handle); + void goalCallback(const std::shared_ptr>& goal_handle); rclcpp_action::CancelResponse preemptCallback(const std::shared_ptr>& goal_handle); From ec8955b5fa298ed91625d3fcc8ab377b754a2d3b Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 18 Oct 2023 09:57:20 +0200 Subject: [PATCH 6/6] Revert unnecessary changes --- capabilities/src/execute_task_solution_capability.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index e1707d2d1..62a76dbd8 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -94,9 +94,9 @@ void ExecuteTaskSolutionCapability::initialize() { std::placeholders::_1, std::placeholders::_2)), ActionServerType::CancelCallback( std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this, std::placeholders::_1)), - [this](std::shared_ptr> goal_handle) { - last_goal_future_ = std::async(std::launch::async, &ExecuteTaskSolutionCapability::goalCallback, this, - std::move(goal_handle)); + [this](const std::shared_ptr>& goal_handle) { + last_goal_future_ = + std::async(std::launch::async, &ExecuteTaskSolutionCapability::goalCallback, this, goal_handle); }); }