From 941e9df7374b5e50818a6c4c99d1080136067f29 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 26 May 2018 19:29:09 +0200 Subject: [PATCH] declare properties "timeout", "marker_ns" for all stages --- core/include/moveit/task_constructor/properties.h | 9 +++++++-- core/include/moveit/task_constructor/stage.h | 3 +++ core/include/moveit/task_constructor/stages/compute_ik.h | 3 --- core/include/moveit/task_constructor/stages/connect.h | 4 ---- .../moveit/task_constructor/stages/current_state.h | 4 ---- core/src/stage.cpp | 3 +++ core/src/stages/compute_ik.cpp | 4 ++-- core/src/stages/connect.cpp | 4 ++-- core/src/stages/current_state.cpp | 6 ++++-- core/src/stages/move_relative.cpp | 6 ++---- core/src/stages/move_to.cpp | 4 ++-- 11 files changed, 25 insertions(+), 25 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index cea3e61d..6fce3790 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -63,6 +63,7 @@ class PropertyMap; /// initializer function, using given name from the passed property map boost::any fromName(const PropertyMap& other, const std::string& other_name); + /** Property is a wrapper for a boost::any value, also providing a default value and a description. * * Properties can be configured to be initialized from another PropertyMap - if still undefined. @@ -82,7 +83,8 @@ class Property { const Property::SerializeFunction &serialize); template - static typename std::enable_if::value, std::string>::type serialize(const boost::any& value) { + static typename std::enable_if::value, std::string>::type + serialize(const boost::any& value) { if (value.empty()) return ""; std::ostringstream oss; oss << boost::any_cast(value); @@ -90,7 +92,8 @@ class Property { } template - static typename std::enable_if::value, std::string>::type serialize(const boost::any& value) { + static typename std::enable_if::value, std::string>::type + serialize(const boost::any& value) { return ""; } @@ -129,6 +132,8 @@ public: /// get description text const std::string& description() const { return description_; } + void setDescription(const std::string& desc) { description_ = desc; } + /// get typename std::string typeName() const { return type_index_.name(); } diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 1bbd7d46..58efe2a2 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -156,6 +156,9 @@ public: const std::string& name() const; void setName(const std::string& name); + void setTimeout(double timeout) { setProperty("timeout", timeout); } + double timeout() const { return properties().get("timeout"); } + typedef std::function SolutionCallback; typedef std::list SolutionCallbackList; /// add function to be called for every newly found solution or failure diff --git a/core/include/moveit/task_constructor/stages/compute_ik.h b/core/include/moveit/task_constructor/stages/compute_ik.h index 2bf6f925..d982b55e 100644 --- a/core/include/moveit/task_constructor/stages/compute_ik.h +++ b/core/include/moveit/task_constructor/stages/compute_ik.h @@ -69,9 +69,6 @@ public: void init(const core::RobotModelConstPtr &robot_model); void onNewSolution(const SolutionBase &s) override; - void setTimeout(double timeout) { - setProperty("timeout", timeout); - } void setEndEffector(const std::string& eef) { setProperty("eef", eef); } diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index 426dfce9..efd1f127 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -65,10 +65,6 @@ public: typedef std::vector> GroupPlannerVector; Connect(const std::string& name, const GroupPlannerVector& planners); - void setTimeout(const ros::Duration& timeout) { - setProperty("timeout", timeout.toSec()); - } - void setPathConstraints(moveit_msgs::Constraints path_constraints) { setProperty("path_constraints", std::move(path_constraints)); } diff --git a/core/include/moveit/task_constructor/stages/current_state.h b/core/include/moveit/task_constructor/stages/current_state.h index 1cca86e5..f4bcb2ce 100644 --- a/core/include/moveit/task_constructor/stages/current_state.h +++ b/core/include/moveit/task_constructor/stages/current_state.h @@ -53,10 +53,6 @@ public: bool canCompute() const override; void compute() override; - void setTimeout(const ros::Duration& timeout) { - setProperty("timeout", timeout); - } - protected: moveit::core::RobotModelConstPtr robot_model_; planning_scene::PlanningScenePtr scene_; diff --git a/core/src/stage.cpp b/core/src/stage.cpp index ed758b2f..1f01125a 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -179,6 +179,9 @@ Stage::Stage(StagePrivate *impl) : pimpl_(impl) { assert(impl); + auto& p = properties(); + p.declare("timeout", "timeout per run (s)"); + p.declare("marker_ns", "marker namespace"); } Stage::~Stage() diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 3db63e22..db5cdb8e 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -54,8 +54,8 @@ namespace moveit { namespace task_constructor { namespace stages { ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child) : WrapperBase(name, std::move(child)) { + setTimeout(1.0); auto& p = properties(); - p.declare("timeout", 1.0); p.declare("eef", "name of end-effector group"); p.declare("group", "name of active group (derived from eef if not provided)"); p.declare("default_pose", "", "default joint pose of active group (defines cost of IK)"); @@ -334,7 +334,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s) uint32_t max_ik_solutions = props.get("max_ik_solutions"); bool tried_current_state_as_seed = false; - double remaining_time = props.get("timeout"); + double remaining_time = timeout(); auto start_time = std::chrono::steady_clock::now(); while (ik_solutions.size() < max_ik_solutions && remaining_time > 0) { if(tried_current_state_as_seed) diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index cb77ec27..d1fc2f12 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -46,8 +46,8 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : Connecting(name) , planner_(planners) { + setTimeout(1.0); auto& p = properties(); - p.declare("timeout", 10.0, "planning timeout"); p.declare("group", "name of planning group"); p.declare("path_constraints", moveit_msgs::Constraints(), "constraints to maintain during trajectory"); @@ -137,7 +137,7 @@ bool Connect::compatible(const InterfaceState& from_state, const InterfaceState& void Connect::compute(const InterfaceState &from, const InterfaceState &to) { const auto& props = properties(); - double timeout = props.get("timeout"); + double timeout = this->timeout(); const auto& path_constraints = props.get("path_constraints"); std::vector sub_trajectories; diff --git a/core/src/stages/current_state.cpp b/core/src/stages/current_state.cpp index cfd7d319..f2629558 100644 --- a/core/src/stages/current_state.cpp +++ b/core/src/stages/current_state.cpp @@ -50,7 +50,9 @@ CurrentState::CurrentState(const std::string &name) : Generator(name) { auto &p = properties(); - p.declare("timeout", ros::Duration(-1), "max time to wait for get_planning_scene service"); + Property& timeout = p.property("timeout"); + timeout.setDescription("max time to wait for get_planning_scene service"); + timeout.setValue(-1.0); } void CurrentState::init(const moveit::core::RobotModelConstPtr& robot_model) @@ -70,7 +72,7 @@ void CurrentState::compute() { ros::NodeHandle h; ros::ServiceClient client = h.serviceClient("get_planning_scene"); - ros::Duration timeout = properties().get("timeout"); + ros::Duration timeout(this->timeout()); if (client.waitForExistence(timeout)) { moveit_msgs::GetPlanningScene::Request req; moveit_msgs::GetPlanningScene::Response res; diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index a36c0a98..871ebb13 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -47,10 +47,8 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf : PropagatingEitherWay(name) , planner_(planner) { + setTimeout(10.0); auto& p = properties(); - p.declare("timeout", 10.0, "planning timeout"); - // +1 TODO: make "marker" a common property of all stages. However, I would stick with "marker_ns" - p.declare("marker_ns", "", "marker namespace"); p.declare("group", "name of planning group"); p.declare("ik_frame", "frame to be moved towards goal pose"); p.declare("min_distance", -1.0, "minimum distance to move"); @@ -85,7 +83,7 @@ void MoveRelative::compute(const InterfaceState &state, planning_scene::Planning assert(robot_model); const auto& props = properties(); - double timeout = props.get("timeout"); + double timeout = this->timeout(); const std::string& group = props.get("group"); const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group); if (!jmg) { diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index cb46e93d..0fc03836 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -48,8 +48,8 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan : PropagatingEitherWay(name) , planner_(planner) { + setTimeout(10.0); auto& p = properties(); - p.declare("timeout", 10.0, "planning timeout"); // TODO: make this a common property in Stage p.declare("group", "name of planning group"); p.declare("ik_frame", "frame to be moved towards goal pose"); @@ -126,7 +126,7 @@ void MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP assert(robot_model); const auto& props = properties(); - double timeout = props.get("timeout"); + double timeout = this->timeout(); const std::string& group = props.get("group"); const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group); if (!jmg) {