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
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 <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 "";
std::ostringstream oss;
oss << boost::any_cast<T>(value);
@ -90,7 +92,8 @@ class Property {
}
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 "";
}
@ -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(); }

View File

@ -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<double>("timeout"); }
typedef std::function<void(const SolutionBase &s)> SolutionCallback;
typedef std::list<SolutionCallback> SolutionCallbackList;
/// 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 onNewSolution(const SolutionBase &s) override;
void setTimeout(double timeout) {
setProperty("timeout", timeout);
}
void setEndEffector(const std::string& eef) {
setProperty("eef", eef);
}

View File

@ -65,10 +65,6 @@ public:
typedef std::vector<std::pair<std::string, solvers::PlannerInterfacePtr>> 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));
}

View File

@ -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_;

View File

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

View File

@ -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<double>("timeout", 1.0);
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>("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");
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();
while (ik_solutions.size() < max_ik_solutions && remaining_time > 0) {
if(tried_current_state_as_seed)

View File

@ -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<double>("timeout", 10.0, "planning timeout");
p.declare<std::string>("group", "name of planning group");
p.declare<moveit_msgs::Constraints>("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<double>("timeout");
double timeout = this->timeout();
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
std::vector<robot_trajectory::RobotTrajectoryConstPtr> sub_trajectories;

View File

@ -50,7 +50,9 @@ CurrentState::CurrentState(const std::string &name)
: Generator(name)
{
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)
@ -70,7 +72,7 @@ void CurrentState::compute() {
ros::NodeHandle h;
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)) {
moveit_msgs::GetPlanningScene::Request req;
moveit_msgs::GetPlanningScene::Response res;

View File

@ -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<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<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose");
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);
const auto& props = properties();
double timeout = props.get<double>("timeout");
double timeout = this->timeout();
const std::string& group = props.get<std::string>("group");
const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group);
if (!jmg) {

View File

@ -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<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<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);
const auto& props = properties();
double timeout = props.get<double>("timeout");
double timeout = this->timeout();
const std::string& group = props.get<std::string>("group");
const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group);
if (!jmg) {