Merge pull request #525 from Abishalini/pr-get-error-msg-from-planners

Propagate errors from planners.
This commit is contained in:
Abishalini Sivaraman 2024-01-25 07:38:00 -07:00 committed by GitHub
commit 8a89a9762b
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
12 changed files with 160 additions and 101 deletions

View File

@ -63,11 +63,11 @@ public:
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
MoveItErrorCode 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::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
MoveItErrorCode plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;

View File

@ -58,14 +58,16 @@ public:
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
MoveItErrorCode
plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
MoveItErrorCode
plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
std::string getPlannerId() const override { return std::string("JointInterpolationPlanner"); }
};

View File

@ -63,14 +63,16 @@ public:
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool 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::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
MoveItErrorCode
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::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
MoveItErrorCode
plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
};
} // namespace solvers
} // namespace task_constructor

View File

@ -124,9 +124,9 @@ public:
* \param [in] timeout Maximum planning timeout for an individual stage that is using the pipeline planner in seconds
* \param [in] result Reference to the location where the created trajectory is stored if planning is successful
* \param [in] path_constraints Path contraints for the planning problem
* \return true If the solver found a successful solution for the given planning problem
* \return MoveItErrorCode - Return MoveItErrorCodes::SUCCESS if solver found a successful solution. Otherwise, return an appropriate error type and message explaining why it failed.
*/
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
MoveItErrorCode plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const core::JointModelGroup* joint_model_group, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
@ -140,9 +140,9 @@ public:
* \param [in] timeout Maximum planning timeout for an individual stage that is using the pipeline planner in seconds
* \param [in] result Reference to the location where the created trajectory is stored if planning is successful
* \param [in] path_constraints Path contraints for the planning problem
* \return true If the solver found a successful solution for the given planning problem
* \return MoveItErrorCode - Return MoveItErrorCodes::SUCCESS if solver found a successful solution. Otherwise, return an appropriate error type and message explaining why it failed.
*/
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
MoveItErrorCode plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* joint_model_group, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
@ -170,7 +170,7 @@ protected:
* problem
* \return true If the solver found a successful solution for the given planning problem
*/
bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
MoveItErrorCode plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
const moveit::core::JointModelGroup* joint_model_group,
const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,

View File

@ -41,6 +41,7 @@
#include <moveit/macros/class_forward.h>
#include <moveit_msgs/msg/constraints.hpp>
#include <moveit/task_constructor/properties.h>
#include <moveit/utils/moveit_error_code.h>
#include <Eigen/Geometry>
namespace planning_scene {
@ -64,6 +65,9 @@ namespace moveit {
namespace task_constructor {
namespace solvers {
using MoveItErrorCode = moveit::core::MoveItErrorCode;
using MoveItErrorCodes = moveit_msgs::msg::MoveItErrorCodes;
MOVEIT_CLASS_FORWARD(PlannerInterface);
class PlannerInterface
{
@ -88,17 +92,17 @@ public:
virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0;
/// plan trajectory between to robot states
virtual bool 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::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
virtual MoveItErrorCode
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::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
/// plan trajectory from current robot state to Cartesian target, such that pose(link)*offset == target
virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
virtual MoveItErrorCode
plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
// get name of the planner
virtual std::string getPlannerId() const = 0;

View File

@ -63,14 +63,15 @@ CartesianPath::CartesianPath() {
void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {}
bool CartesianPath::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::msg::Constraints& path_constraints) {
MoveItErrorCode CartesianPath::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::msg::Constraints& path_constraints) {
const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip();
if (!link) {
RCLCPP_WARN_STREAM(LOGGER, "no unique tip for joint model group: " << jmg->getName());
return false;
return MoveItErrorCode(MoveItErrorCodes::FAILURE, "no unique tip for joint model group: " + jmg->getName());
}
// reach pose of forward kinematics
@ -78,11 +79,11 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
std::min(timeout, properties().get<double>("timeout")), result, path_constraints);
}
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* jmg, double /*timeout*/,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints) {
MoveItErrorCode CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double /*timeout*/, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints) {
const auto& props = properties();
planning_scene::PlanningScenePtr sandbox_scene = from->diff();
@ -114,7 +115,11 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));
return achieved_fraction >= props.get<double>("min_fraction");
if (achieved_fraction < props.get<double>("min_fraction")) {
return MoveItErrorCode(MoveItErrorCodes::FAILURE,
"Min fraction not met. Achieved fraction : " + std::to_string(achieved_fraction));
}
return MoveItErrorCode(MoveItErrorCodes::SUCCESS);
}
} // namespace solvers
} // namespace task_constructor

View File

@ -59,11 +59,11 @@ JointInterpolationPlanner::JointInterpolationPlanner() {
void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_model*/) {}
bool JointInterpolationPlanner::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::msg::Constraints& /*path_constraints*/) {
MoveItErrorCode JointInterpolationPlanner::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::msg::Constraints& /*path_constraints*/) {
const auto& props = properties();
// Get maximum joint distance
@ -78,7 +78,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
// add first point
result->addSuffixWayPoint(from->getCurrentState(), 0.0);
if (from->isStateColliding(from_state, jmg->getName()) || !from_state.satisfiesBounds(jmg))
return false;
return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Start state in collision or not within bounds");
moveit::core::RobotState waypoint(from_state);
double delta = d < 1e-6 ? 1.0 : props.get<double>("max_step") / d;
@ -87,13 +87,14 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
result->addSuffixWayPoint(waypoint, t);
if (from->isStateColliding(waypoint, jmg->getName()) || !waypoint.satisfiesBounds(jmg))
return false;
return MoveItErrorCode(MoveItErrorCodes::FAILURE,
"One of the waypoint's state is in collision or not within bounds");
}
// add goal point
result->addSuffixWayPoint(to_state, 1.0);
if (from->isStateColliding(to_state, jmg->getName()) || !to_state.satisfiesBounds(jmg))
return false;
return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Goal state in collision or not within bounds");
auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
@ -113,14 +114,15 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
}
}
return true;
return MoveItErrorCode(MoveItErrorCodes::SUCCESS);
}
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints) {
MoveItErrorCode JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::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);
@ -137,15 +139,14 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
} };
if (!to->getCurrentStateNonConst().setFromIK(jmg, target * offset.inverse(), link.getName(), timeout, is_valid)) {
// TODO(v4hn): planners need a way to add feedback to failing plans
// in case of an invalid solution feedback should include unwanted collisions or violated constraints
RCLCPP_WARN(LOGGER, "IK failed for pose target");
return false;
return MoveItErrorCode(MoveItErrorCodes::FAILURE, "IK failed for pose target.");
}
to->getCurrentStateNonConst().update();
if (std::chrono::steady_clock::now() >= deadline)
return false;
if (std::chrono::steady_clock::now() >= deadline) {
return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Timed out.");
}
return plan(from, to, jmg, timeout, result, path_constraints);
}

View File

@ -49,49 +49,50 @@ void MultiPlanner::init(const core::RobotModelConstPtr& robot_model) {
p->init(robot_model);
}
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::msg::Constraints& path_constraints) {
MoveItErrorCode 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::msg::Constraints& path_constraints) {
double remaining_time = std::min(timeout, properties().get<double>("timeout"));
auto start_time = std::chrono::steady_clock::now();
for (const auto& p : *this) {
if (remaining_time < 0)
return false; // timeout
return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Timeout"); // timeout
if (result)
result->clear();
if (p->plan(from, to, jmg, remaining_time, result, path_constraints))
return true;
return MoveItErrorCode(MoveItErrorCodes::SUCCESS);
auto now = std::chrono::steady_clock::now();
remaining_time -= std::chrono::duration<double>(now - start_time).count();
start_time = now;
}
return false;
return MoveItErrorCode(MoveItErrorCodes::FAILURE);
}
bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints) {
MoveItErrorCode MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints) {
double remaining_time = std::min(timeout, properties().get<double>("timeout"));
auto start_time = std::chrono::steady_clock::now();
for (const auto& p : *this) {
if (remaining_time < 0)
return false; // timeout
return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Timeout"); // timeout
if (result)
result->clear();
if (p->plan(from, link, offset, target, jmg, remaining_time, result, path_constraints))
return true;
return MoveItErrorCode(MoveItErrorCodes::SUCCESS);
auto now = std::chrono::steady_clock::now();
remaining_time -= std::chrono::duration<double>(now - start_time).count();
start_time = now;
}
return false;
return MoveItErrorCode(MoveItErrorCodes::FAILURE);
}
} // namespace solvers
} // namespace task_constructor

View File

@ -140,22 +140,23 @@ void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
}
}
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* joint_model_group, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints) {
MoveItErrorCode PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* joint_model_group, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints) {
// Construct goal constraints from the goal planning scene
const auto goal_constraints = kinematic_constraints::constructGoalConstraints(
to->getCurrentState(), joint_model_group, properties().get<double>("goal_joint_tolerance"));
return plan(from, joint_model_group, goal_constraints, timeout, result, path_constraints);
}
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target_eigen,
const moveit::core::JointModelGroup* joint_model_group, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints) {
MoveItErrorCode PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
const Eigen::Isometry3d& target_eigen,
const moveit::core::JointModelGroup* joint_model_group, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints) {
last_successful_planner_.clear();
// Construct a Cartesian target pose from the given target transform and offset
@ -170,11 +171,11 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co
return plan(from, joint_model_group, goal_constraints, timeout, result, path_constraints);
}
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
const moveit::core::JointModelGroup* joint_model_group,
const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints) {
MoveItErrorCode PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
const moveit::core::JointModelGroup* joint_model_group,
const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints) {
// Create a request for every planning pipeline that should run in parallel
std::vector<moveit_msgs::msg::MotionPlanRequest> requests;
requests.reserve(pipeline_id_planner_id_map_.size());
@ -222,10 +223,14 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning
// Choose the first solution trajectory as response
result = solution.trajectory;
last_successful_planner_ = solution.planner_id;
return bool(result);
if (!bool(result)) // If the plan was not a success
{
return MoveItErrorCode(solution.error_code.val, solution.error_code.message, solution.error_code.source);
}
return MoveItErrorCode(MoveItErrorCodes::SUCCESS);
}
}
return false;
return MoveItErrorCode(MoveItErrorCodes::FAILURE, "No solutions generated from Pipeline Planner");
}
std::string PipelinePlanner::getPlannerId() const {
return last_successful_planner_.empty() ? std::string("Unknown") : last_successful_planner_;

View File

@ -148,6 +148,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
intermediate_scenes.push_back(start);
bool success = false;
std::string error_message = "";
std::vector<double> positions;
for (const GroupPlannerVector::value_type& pair : planner_) {
// set intermediate goal state
@ -160,11 +161,17 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
intermediate_scenes.push_back(end);
robot_trajectory::RobotTrajectoryPtr trajectory;
success = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints);
const auto planner_solution_status = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints);
if (bool(planner_solution_status)) {
success = true;
}
trajectory_planner_vector.push_back(PlannerIdTrajectoryPair({ pair.second->getPlannerId(), trajectory }));
if (!success)
if (!success) {
error_message = planner_solution_status.message;
break;
}
// continue from reached state
start = end;
@ -176,7 +183,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
if (!solution) // success == false or merging failed: store sequentially
solution = makeSequential(trajectory_planner_vector, intermediate_scenes, from, to);
if (!success) // error during sequential planning
solution->markAsFailure();
solution->markAsFailure(error_message);
connect(from, to, solution);
}

View File

@ -195,10 +195,18 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
bool success = false;
std::string error_message = "";
if (getJointStateFromOffset(direction, dir, jmg, scene->getCurrentStateNonConst())) {
// plan to joint-space target
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
const auto planner_solution_status =
planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
if (bool(planner_solution_status)) {
success = true;
}
if (!success) {
error_message = planner_solution_status.message;
}
solution.setPlannerId(planner_->getPlannerId());
} else {
// Cartesian targets require an IK reference frame
@ -286,8 +294,14 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
// offset from link to ik_frame
const Eigen::Isometry3d& offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
success =
const auto planner_solution_status =
planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
if (bool(planner_solution_status)) {
success = true;
}
if (!success) {
error_message = planner_solution_status.message;
}
solution.setPlannerId(planner_->getPlannerId());
if (robot_trajectory) { // the following requires a robot_trajectory returned from planning
@ -331,8 +345,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
robot_trajectory->reverse();
solution.setTrajectory(robot_trajectory);
if (!success)
if (!success && solution.comment().empty()) {
solution.markAsFailure(error_message);
} else if (!success) {
solution.markAsFailure();
}
return true;
}
return false;

View File

@ -205,10 +205,18 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
const auto& path_constraints = props.get<moveit_msgs::msg::Constraints>("path_constraints");
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
bool success = false;
std::string error_message = "";
if (getJointStateGoal(goal, jmg, scene->getCurrentStateNonConst())) {
// plan to joint-space target
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
const auto planner_solution_status =
planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
if (bool(planner_solution_status)) {
success = true;
}
if (!success) {
error_message = planner_solution_status.message;
}
solution.setPlannerId(planner_->getPlannerId());
} else { // Cartesian goal
// Where to go?
@ -241,7 +249,14 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
Eigen::Isometry3d offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
// plan to Cartesian target
success = planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints);
const auto planner_solution_status =
planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints);
if (bool(planner_solution_status)) {
success = true;
}
if (!success) {
error_message = planner_solution_status.message;
}
solution.setPlannerId(planner_->getPlannerId());
}
@ -258,7 +273,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
solution.setTrajectory(robot_trajectory);
if (!success)
solution.markAsFailure();
solution.markAsFailure(error_message);
return true;
}