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
|
/// 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(); }
|
||||||
|
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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);
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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));
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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_;
|
||||||
|
|||||||
@ -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()
|
||||||
|
|||||||
@ -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)
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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) {
|
||||||
|
|||||||
@ -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) {
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user