mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
declare properties "timeout", "marker_ns" for all stages
This commit is contained in:
parent
c2dd28abae
commit
941e9df737
@ -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(); }
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
@ -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));
|
||||
}
|
||||
|
||||
@ -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_;
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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) {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user