PlannerInterface: provide "timeout" property

The MultiPlanner requires to set individual timeouts for its planners.
This commit is contained in:
Robert Haschke 2023-02-17 14:01:58 +01:00
parent 573858e51a
commit 6dc70b1d49
7 changed files with 10 additions and 4 deletions

View File

@ -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>
{

View File

@ -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) {

View File

@ -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,

View File

@ -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() };

View File

@ -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) {

View File

@ -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");

View File

@ -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>());