mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
PlannerInterface: provide "timeout" property
The MultiPlanner requires to set individual timeouts for its planners.
This commit is contained in:
parent
573858e51a
commit
6dc70b1d49
@ -51,6 +51,9 @@ MOVEIT_CLASS_FORWARD(MultiPlanner);
|
||||
*
|
||||
* This is useful to sequence different planning strategies of increasing complexity,
|
||||
* e.g. Cartesian or joint-space interpolation first, then OMPL, ...
|
||||
* This is (slightly) different from the Fallbacks container, as the MultiPlanner directly applies its planners to each
|
||||
* individual planning job. In contrast, the Fallbacks container first runs the active child to exhaustion before
|
||||
* switching to the next child, which possibly applies a different planning strategy.
|
||||
*/
|
||||
class MultiPlanner : public PlannerInterface, public std::vector<solvers::PlannerInterfacePtr>
|
||||
{
|
||||
|
||||
@ -78,6 +78,7 @@ public:
|
||||
const PropertyMap& properties() const { return properties_; }
|
||||
|
||||
void setProperty(const std::string& name, const boost::any& value) { properties_.set(name, value); }
|
||||
void setTimeout(double timeout) { properties_.set("timeout", timeout); }
|
||||
void setMaxVelocityScalingFactor(double factor) { properties_.set("max_velocity_scaling_factor", factor); }
|
||||
void setMaxAccelerationScalingFactor(double factor) { properties_.set("max_acceleration_scaling_factor", factor); }
|
||||
void setTimeParameterization(const trajectory_processing::TimeParameterizationPtr& tp) {
|
||||
|
||||
@ -71,7 +71,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||
|
||||
// reach pose of forward kinematics
|
||||
return plan(from, *link, Eigen::Isometry3d::Identity(), to->getCurrentState().getGlobalLinkTransform(link), jmg,
|
||||
timeout, result, path_constraints);
|
||||
std::min(timeout, properties().get<double>("timeout")), result, path_constraints);
|
||||
}
|
||||
|
||||
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
||||
|
||||
@ -103,6 +103,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
|
||||
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
|
||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::Constraints& path_constraints) {
|
||||
timeout = std::min(timeout, properties().get<double>("timeout"));
|
||||
const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration<double, std::ratio<1>>(timeout);
|
||||
|
||||
auto to{ from->diff() };
|
||||
|
||||
@ -53,7 +53,7 @@ bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
|
||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::Constraints& path_constraints) {
|
||||
double remaining_time = timeout;
|
||||
double remaining_time = std::min(timeout, properties().get<double>("timeout"));
|
||||
auto start_time = std::chrono::steady_clock::now();
|
||||
|
||||
for (const auto& p : *this) {
|
||||
@ -76,7 +76,7 @@ bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const
|
||||
const moveit::core::JointModelGroup* jmg, double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::Constraints& path_constraints) {
|
||||
double remaining_time = timeout;
|
||||
double remaining_time = std::min(timeout, properties().get<double>("timeout"));
|
||||
auto start_time = std::chrono::steady_clock::now();
|
||||
|
||||
for (const auto& p : *this) {
|
||||
|
||||
@ -143,7 +143,7 @@ void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req, const PropertyMa
|
||||
const moveit::core::JointModelGroup* jmg, double timeout) {
|
||||
req.group_name = jmg->getName();
|
||||
req.planner_id = p.get<std::string>("planner");
|
||||
req.allowed_planning_time = timeout;
|
||||
req.allowed_planning_time = std::min(timeout, p.get<double>("timeout"));
|
||||
req.start_state.is_diff = true; // we don't specify an extra start state
|
||||
|
||||
req.num_planning_attempts = p.get<uint>("num_planning_attempts");
|
||||
|
||||
@ -47,6 +47,7 @@ namespace solvers {
|
||||
|
||||
PlannerInterface::PlannerInterface() {
|
||||
auto& p = properties();
|
||||
p.declare<double>("timeout", std::numeric_limits<double>::infinity(), "timeout for planner (s)");
|
||||
p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
|
||||
p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor");
|
||||
p.declare<TimeParameterizationPtr>("time_parameterization", std::make_shared<TimeOptimalTrajectoryGeneration>());
|
||||
|
||||
Loading…
Reference in New Issue
Block a user