From c72a6cddac02a9061307b8c859587676ce166cdd Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 13 Feb 2019 23:48:44 +0100 Subject: [PATCH] ComputeIK: auto-configure default timeout from JMG's default TODO: actually set the default value but not the current value! --- core/include/moveit/task_constructor/properties.h | 4 +++- core/src/properties.cpp | 8 ++++++++ core/src/stages/compute_ik.cpp | 2 +- 3 files changed, 12 insertions(+), 2 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 7d911054..3e033386 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -65,7 +65,7 @@ boost::any fromName(const PropertyMap& other, const std::string& other_name); * * Setting the value via setValue() updates both, the current value and the default value. * Using reset() the default value can be restored. - * Using setCurrentValue() only updates the current value, allowing for later reset to the original default. + * setCurrentValue() and setDefaultValue() only set the specific value. */ class Property { friend class PropertyMap; @@ -97,6 +97,7 @@ public: /// set current value and default value void setValue(const boost::any& value); void setCurrentValue(const boost::any& value); + void setDefaultValue(const boost::any& value); /// reset to default value (which can be empty) void reset(); @@ -274,6 +275,7 @@ public: /// declare given property name as other_name in other PropertyMap void exposeTo(PropertyMap& other, const std::string& name, const std::string& other_name) const; + /// check whether given property is declared bool hasProperty(const std::string &name) const; /// get the property with given name, throws Property::undeclared for unknown name diff --git a/core/src/properties.cpp b/core/src/properties.cpp index a5297fe5..f12b1572 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -142,6 +142,14 @@ void Property::setCurrentValue(const boost::any &value) initialized_from_ = 1; // manually initialized TODO: use enums } +void Property::setDefaultValue(const boost::any& value) +{ + if (!value.empty() && type_info_ != typeid(boost::any) && value.type() != type_info_) + throw Property::type_error(value.type().name(), type_info_.name()); + + default_ = value; +} + void Property::reset() { if (initialized_from_ == 0) // TODO: use enum diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index de2dc019..f5c1468b 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -54,7 +54,6 @@ 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("eef", "name of end-effector group"); p.declare("group", "name of active group (derived from eef if not provided)"); @@ -229,6 +228,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s) ROS_WARN_STREAM_NAMED("ComputeIK", "Neither eef nor group are well defined"); return; } + properties().property("timeout").setDefaultValue(jmg->getDefaultIKTimeout()); // extract target_pose geometry_msgs::PoseStamped target_pose_msg = props.get("target_pose");