declare properties "timeout", "marker_ns" for all stages

This commit is contained in:
Robert Haschke 2018-05-26 19:29:09 +02:00 committed by v4hn
parent c2dd28abae
commit 941e9df737
11 changed files with 25 additions and 25 deletions

View File

@ -63,6 +63,7 @@ class PropertyMap;
/// initializer function, using given name from the passed property map /// initializer function, using given name from the passed property map
boost::any fromName(const PropertyMap& other, const std::string& other_name); 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. /** 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. * Properties can be configured to be initialized from another PropertyMap - if still undefined.
@ -82,7 +83,8 @@ class Property {
const Property::SerializeFunction &serialize); const Property::SerializeFunction &serialize);
template <typename T> template <typename T>
static typename std::enable_if<hasSerialize<T>::value, std::string>::type serialize(const boost::any& value) { static typename std::enable_if<hasSerialize<T>::value, std::string>::type
serialize(const boost::any& value) {
if (value.empty()) return ""; if (value.empty()) return "";
std::ostringstream oss; std::ostringstream oss;
oss << boost::any_cast<T>(value); oss << boost::any_cast<T>(value);
@ -90,7 +92,8 @@ class Property {
} }
template <typename T> template <typename T>
static typename std::enable_if<!hasSerialize<T>::value, std::string>::type serialize(const boost::any& value) { static typename std::enable_if<!hasSerialize<T>::value, std::string>::type
serialize(const boost::any& value) {
return ""; return "";
} }
@ -129,6 +132,8 @@ public:
/// get description text /// get description text
const std::string& description() const { return description_; } const std::string& description() const { return description_; }
void setDescription(const std::string& desc) { description_ = desc; }
/// get typename /// get typename
std::string typeName() const { return type_index_.name(); } std::string typeName() const { return type_index_.name(); }

View File

@ -156,6 +156,9 @@ public:
const std::string& name() const; const std::string& name() const;
void setName(const std::string& name); void setName(const std::string& name);
void setTimeout(double timeout) { setProperty("timeout", timeout); }
double timeout() const { return properties().get<double>("timeout"); }
typedef std::function<void(const SolutionBase &s)> SolutionCallback; typedef std::function<void(const SolutionBase &s)> SolutionCallback;
typedef std::list<SolutionCallback> SolutionCallbackList; typedef std::list<SolutionCallback> SolutionCallbackList;
/// add function to be called for every newly found solution or failure /// add function to be called for every newly found solution or failure

View File

@ -69,9 +69,6 @@ public:
void init(const core::RobotModelConstPtr &robot_model); void init(const core::RobotModelConstPtr &robot_model);
void onNewSolution(const SolutionBase &s) override; void onNewSolution(const SolutionBase &s) override;
void setTimeout(double timeout) {
setProperty("timeout", timeout);
}
void setEndEffector(const std::string& eef) { void setEndEffector(const std::string& eef) {
setProperty("eef", eef); setProperty("eef", eef);
} }

View File

@ -65,10 +65,6 @@ public:
typedef std::vector<std::pair<std::string, solvers::PlannerInterfacePtr>> GroupPlannerVector; typedef std::vector<std::pair<std::string, solvers::PlannerInterfacePtr>> GroupPlannerVector;
Connect(const std::string& name, const GroupPlannerVector& planners); 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) { void setPathConstraints(moveit_msgs::Constraints path_constraints) {
setProperty("path_constraints", std::move(path_constraints)); setProperty("path_constraints", std::move(path_constraints));
} }

View File

@ -53,10 +53,6 @@ public:
bool canCompute() const override; bool canCompute() const override;
void compute() override; void compute() override;
void setTimeout(const ros::Duration& timeout) {
setProperty("timeout", timeout);
}
protected: protected:
moveit::core::RobotModelConstPtr robot_model_; moveit::core::RobotModelConstPtr robot_model_;
planning_scene::PlanningScenePtr scene_; planning_scene::PlanningScenePtr scene_;

View File

@ -179,6 +179,9 @@ Stage::Stage(StagePrivate *impl)
: pimpl_(impl) : pimpl_(impl)
{ {
assert(impl); assert(impl);
auto& p = properties();
p.declare<double>("timeout", "timeout per run (s)");
p.declare<std::string>("marker_ns", "marker namespace");
} }
Stage::~Stage() Stage::~Stage()

View File

@ -54,8 +54,8 @@ namespace moveit { namespace task_constructor { namespace stages {
ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child) ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child)
: WrapperBase(name, std::move(child)) : WrapperBase(name, std::move(child))
{ {
setTimeout(1.0);
auto& p = properties(); auto& p = properties();
p.declare<double>("timeout", 1.0);
p.declare<std::string>("eef", "name of end-effector group"); p.declare<std::string>("eef", "name of end-effector group");
p.declare<std::string>("group", "name of active group (derived from eef if not provided)"); p.declare<std::string>("group", "name of active group (derived from eef if not provided)");
p.declare<std::string>("default_pose", "", "default joint pose of active group (defines cost of IK)"); p.declare<std::string>("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<uint32_t>("max_ik_solutions"); uint32_t max_ik_solutions = props.get<uint32_t>("max_ik_solutions");
bool tried_current_state_as_seed = false; bool tried_current_state_as_seed = false;
double remaining_time = props.get<double>("timeout"); double remaining_time = timeout();
auto start_time = std::chrono::steady_clock::now(); auto start_time = std::chrono::steady_clock::now();
while (ik_solutions.size() < max_ik_solutions && remaining_time > 0) { while (ik_solutions.size() < max_ik_solutions && remaining_time > 0) {
if(tried_current_state_as_seed) if(tried_current_state_as_seed)

View File

@ -46,8 +46,8 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners)
: Connecting(name) : Connecting(name)
, planner_(planners) , planner_(planners)
{ {
setTimeout(1.0);
auto& p = properties(); auto& p = properties();
p.declare<double>("timeout", 10.0, "planning timeout");
p.declare<std::string>("group", "name of planning group"); p.declare<std::string>("group", "name of planning group");
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(), p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
"constraints to maintain during trajectory"); "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) { void Connect::compute(const InterfaceState &from, const InterfaceState &to) {
const auto& props = properties(); const auto& props = properties();
double timeout = props.get<double>("timeout"); double timeout = this->timeout();
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints"); const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
std::vector<robot_trajectory::RobotTrajectoryConstPtr> sub_trajectories; std::vector<robot_trajectory::RobotTrajectoryConstPtr> sub_trajectories;

View File

@ -50,7 +50,9 @@ CurrentState::CurrentState(const std::string &name)
: Generator(name) : Generator(name)
{ {
auto &p = properties(); auto &p = properties();
p.declare<ros::Duration>("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) void CurrentState::init(const moveit::core::RobotModelConstPtr& robot_model)
@ -70,7 +72,7 @@ void CurrentState::compute() {
ros::NodeHandle h; ros::NodeHandle h;
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene"); ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
ros::Duration timeout = properties().get<ros::Duration>("timeout"); ros::Duration timeout(this->timeout());
if (client.waitForExistence(timeout)) { if (client.waitForExistence(timeout)) {
moveit_msgs::GetPlanningScene::Request req; moveit_msgs::GetPlanningScene::Request req;
moveit_msgs::GetPlanningScene::Response res; moveit_msgs::GetPlanningScene::Response res;

View File

@ -47,10 +47,8 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf
: PropagatingEitherWay(name) : PropagatingEitherWay(name)
, planner_(planner) , planner_(planner)
{ {
setTimeout(10.0);
auto& p = properties(); auto& p = properties();
p.declare<double>("timeout", 10.0, "planning timeout");
// +1 TODO: make "marker" a common property of all stages. However, I would stick with "marker_ns"
p.declare<std::string>("marker_ns", "", "marker namespace");
p.declare<std::string>("group", "name of planning group"); p.declare<std::string>("group", "name of planning group");
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose"); p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose");
p.declare<double>("min_distance", -1.0, "minimum distance to move"); p.declare<double>("min_distance", -1.0, "minimum distance to move");
@ -85,7 +83,7 @@ void MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
assert(robot_model); assert(robot_model);
const auto& props = properties(); const auto& props = properties();
double timeout = props.get<double>("timeout"); double timeout = this->timeout();
const std::string& group = props.get<std::string>("group"); const std::string& group = props.get<std::string>("group");
const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group); const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group);
if (!jmg) { if (!jmg) {

View File

@ -48,8 +48,8 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan
: PropagatingEitherWay(name) : PropagatingEitherWay(name)
, planner_(planner) , planner_(planner)
{ {
setTimeout(10.0);
auto& p = properties(); auto& p = properties();
p.declare<double>("timeout", 10.0, "planning timeout"); // TODO: make this a common property in Stage
p.declare<std::string>("group", "name of planning group"); p.declare<std::string>("group", "name of planning group");
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose"); p.declare<geometry_msgs::PoseStamped>("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); assert(robot_model);
const auto& props = properties(); const auto& props = properties();
double timeout = props.get<double>("timeout"); double timeout = this->timeout();
const std::string& group = props.get<std::string>("group"); const std::string& group = props.get<std::string>("group");
const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group); const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group);
if (!jmg) { if (!jmg) {