From 181364c03312c520b78e2af5e5f96597f76f1047 Mon Sep 17 00:00:00 2001 From: Luca Lach Date: Tue, 10 Oct 2023 16:40:00 +0200 Subject: [PATCH 1/2] Add "controllers" property to Stage and pass them to PlanExecution #123 --- capabilities/src/execute_task_solution_capability.cpp | 6 ++++++ core/include/moveit/task_constructor/stage.h | 3 +++ core/src/container.cpp | 3 +++ core/src/stage.cpp | 1 + core/src/storage.cpp | 9 +++++++++ msgs/msg/SubTrajectory.msg | 3 +++ 6 files changed, 25 insertions(+) diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 5c13e9ddf..5d7a8e303 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -171,6 +171,12 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr exec_traj.trajectory = std::make_shared(model, group); exec_traj.trajectory->setRobotTrajectoryMsg(state, sub_traj.trajectory); + if (!sub_traj.controller_names.empty()) { + for (auto cn : sub_traj.controller_names) { + exec_traj.controller_names_.push_back(cn.data.data()); + } + } + /* TODO add action feedback and markers */ exec_traj.effect_on_success = [this, sub_traj, description](const plan_execution::ExecutableMotionPlan* /*plan*/) { diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 4d1a6706f..c60663111 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -201,6 +201,9 @@ class Stage /// marker namespace of solution markers const std::string& markerNS() { return properties().get("marker_ns"); } + void setControllers(std::vector controllers) { setProperty("controllers", controllers); } + std::vector controllers() { return properties().get>("controllers"); } + /// forwarding of properties between interface states void forwardProperties(const InterfaceState& source, InterfaceState& dest); std::set forwardedProperties() const { diff --git a/core/src/container.cpp b/core/src/container.cpp index 8d4298a84..4effcd369 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -367,6 +367,9 @@ void ContainerBase::insert(Stage::pointer&& stage, int before) { StagePrivate* impl = stage->pimpl(); impl->setParent(this); + if (!this->controllers().empty()) + stage->setControllers(this->controllers()); + ContainerBasePrivate::const_iterator where = pimpl()->childByIndex(before, true); ContainerBasePrivate::iterator it = pimpl()->children_.insert(where, std::move(stage)); impl->setParentPosition(it); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 9aff982be..b19d2382a 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -308,6 +308,7 @@ Stage::Stage(StagePrivate* impl) : pimpl_(impl) { auto& p = properties(); p.declare("timeout", "timeout per run (s)"); p.declare("marker_ns", name(), "marker namespace"); + p.declare>("controllers", {}, "list of controllers to use for execution"); p.declare>("forwarded_properties", std::set(), "set of interface properties to forward"); diff --git a/core/src/storage.cpp b/core/src/storage.cpp index f08859baf..de16b74ba 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -229,6 +229,15 @@ void SubTrajectory::appendTo(moveit_task_constructor_msgs::msg::Solution& msg, I moveit_task_constructor_msgs::msg::SubTrajectory& t = msg.sub_trajectory.back(); SolutionBase::fillInfo(t.info, introspection); + std::vector controllers = creator()->me()->properties().get>("controllers"); + if (!controllers.empty()) { + for (auto cn : controllers) { + std_msgs::String s; + s.data = cn; + t.controller_names.push_back(s); + } + } + if (trajectory()) trajectory()->getRobotTrajectoryMsg(t.trajectory); diff --git a/msgs/msg/SubTrajectory.msg b/msgs/msg/SubTrajectory.msg index 10e3fde8a..4810c32a1 100644 --- a/msgs/msg/SubTrajectory.msg +++ b/msgs/msg/SubTrajectory.msg @@ -6,3 +6,6 @@ moveit_msgs/RobotTrajectory trajectory # planning scene of end state as diff w.r.t. start state moveit_msgs/PlanningScene scene_diff + +# list of controllers to use for execution +std_msgs/String[] controller_names From 662597526438e300cec3a8710aa505d03837abc6 Mon Sep 17 00:00:00 2001 From: Joe Schornak Date: Tue, 19 Apr 2022 16:16:51 -0700 Subject: [PATCH 2/2] Change new property from controller names string to TrajectoryExecutionInfo --- .../src/execute_task_solution_capability.cpp | 7 +-- core/include/moveit/task_constructor/stage.h | 10 +++- .../trajectory_execution_info.h | 47 +++++++++++++++++++ core/src/container.cpp | 6 ++- core/src/stage.cpp | 3 +- core/src/storage.cpp | 11 ++--- msgs/CMakeLists.txt | 1 + msgs/msg/SubTrajectory.msg | 6 +-- msgs/msg/TrajectoryExecutionInfo.msg | 2 + 9 files changed, 71 insertions(+), 22 deletions(-) create mode 100644 core/include/moveit/task_constructor/trajectory_execution_info.h create mode 100644 msgs/msg/TrajectoryExecutionInfo.msg diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 5d7a8e303..f20672a4b 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -170,12 +170,7 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr } exec_traj.trajectory = std::make_shared(model, group); exec_traj.trajectory->setRobotTrajectoryMsg(state, sub_traj.trajectory); - - if (!sub_traj.controller_names.empty()) { - for (auto cn : sub_traj.controller_names) { - exec_traj.controller_names_.push_back(cn.data.data()); - } - } + exec_traj.controller_name = sub_traj.execution_info.controller_names; /* TODO add action feedback and markers */ exec_traj.effect_on_success = [this, sub_traj, diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index c60663111..ad93a0a83 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -39,6 +39,7 @@ #pragma once +#include "trajectory_execution_info.h" #include "utils.h" #include #include @@ -201,8 +202,13 @@ class Stage /// marker namespace of solution markers const std::string& markerNS() { return properties().get("marker_ns"); } - void setControllers(std::vector controllers) { setProperty("controllers", controllers); } - std::vector controllers() { return properties().get>("controllers"); } + /// Set and get info to use when executing the stage's trajectory + void setTrajectoryExecutionInfo(TrajectoryExecutionInfo trajectory_execution_info) { + setProperty("trajectory_execution_info", trajectory_execution_info); + } + TrajectoryExecutionInfo trajectoryExecutionInfo() { + return properties().get("trajectory_execution_info"); + } /// forwarding of properties between interface states void forwardProperties(const InterfaceState& source, InterfaceState& dest); diff --git a/core/include/moveit/task_constructor/trajectory_execution_info.h b/core/include/moveit/task_constructor/trajectory_execution_info.h new file mode 100644 index 000000000..222b26748 --- /dev/null +++ b/core/include/moveit/task_constructor/trajectory_execution_info.h @@ -0,0 +1,47 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Joe Schornak, Sebastian Jahr */ + +#pragma once + +#include +#include +#include + +namespace moveit { +namespace task_constructor { +using TrajectoryExecutionInfo = moveit_task_constructor_msgs::msg::TrajectoryExecutionInfo; +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/container.cpp b/core/src/container.cpp index 4effcd369..1bf39e433 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -365,10 +365,12 @@ void ContainerBase::insert(Stage::pointer&& stage, int before) { if (!stage) throw std::runtime_error(name() + ": received invalid stage pointer"); + if (stage->trajectoryExecutionInfo().controller_names.empty()) { + stage->setTrajectoryExecutionInfo(this->trajectoryExecutionInfo()); + } + StagePrivate* impl = stage->pimpl(); impl->setParent(this); - if (!this->controllers().empty()) - stage->setControllers(this->controllers()); ContainerBasePrivate::const_iterator where = pimpl()->childByIndex(before, true); ContainerBasePrivate::iterator it = pimpl()->children_.insert(where, std::move(stage)); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index b19d2382a..a79e1d50f 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -308,7 +308,8 @@ Stage::Stage(StagePrivate* impl) : pimpl_(impl) { auto& p = properties(); p.declare("timeout", "timeout per run (s)"); p.declare("marker_ns", name(), "marker namespace"); - p.declare>("controllers", {}, "list of controllers to use for execution"); + p.declare("trajectory_execution_info", TrajectoryExecutionInfo(), + "settings used when executing the trajectory"); p.declare>("forwarded_properties", std::set(), "set of interface properties to forward"); diff --git a/core/src/storage.cpp b/core/src/storage.cpp index de16b74ba..b5bb827c0 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -229,14 +229,9 @@ void SubTrajectory::appendTo(moveit_task_constructor_msgs::msg::Solution& msg, I moveit_task_constructor_msgs::msg::SubTrajectory& t = msg.sub_trajectory.back(); SolutionBase::fillInfo(t.info, introspection); - std::vector controllers = creator()->me()->properties().get>("controllers"); - if (!controllers.empty()) { - for (auto cn : controllers) { - std_msgs::String s; - s.data = cn; - t.controller_names.push_back(s); - } - } + const auto trajectory_execution_info = + creator()->properties().get("trajectory_execution_info"); + t.execution_info = trajectory_execution_info; if (trajectory()) trajectory()->getRobotTrajectoryMsg(t.trajectory); diff --git a/msgs/CMakeLists.txt b/msgs/CMakeLists.txt index faf4c4d4f..9e56f27ae 100644 --- a/msgs/CMakeLists.txt +++ b/msgs/CMakeLists.txt @@ -18,6 +18,7 @@ set(msg_files "msg/SubTrajectory.msg" "msg/TaskDescription.msg" "msg/TaskStatistics.msg" + "msg/TrajectoryExecutionInfo.msg" ) set(srv_files diff --git a/msgs/msg/SubTrajectory.msg b/msgs/msg/SubTrajectory.msg index 4810c32a1..8b06a5e5a 100644 --- a/msgs/msg/SubTrajectory.msg +++ b/msgs/msg/SubTrajectory.msg @@ -1,11 +1,11 @@ # generic solution information SolutionInfo info +# trajectory execution information, like controller configuration +TrajectoryExecutionInfo execution_info + # trajectory moveit_msgs/RobotTrajectory trajectory # planning scene of end state as diff w.r.t. start state moveit_msgs/PlanningScene scene_diff - -# list of controllers to use for execution -std_msgs/String[] controller_names diff --git a/msgs/msg/TrajectoryExecutionInfo.msg b/msgs/msg/TrajectoryExecutionInfo.msg new file mode 100644 index 000000000..ccadd2286 --- /dev/null +++ b/msgs/msg/TrajectoryExecutionInfo.msg @@ -0,0 +1,2 @@ +# List of controllers to use when executing the trajectory +string[] controller_names