mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
tl::expected -> MoveItErrorCode
This commit is contained in:
parent
c5bf96f5f7
commit
c8a75cb2ac
@ -63,11 +63,11 @@ public:
|
||||
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||
|
||||
tl::expected<bool, std::string> 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;
|
||||
|
||||
tl::expected<bool, std::string> 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;
|
||||
|
||||
@ -58,12 +58,12 @@ public:
|
||||
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||
|
||||
tl::expected<bool, std::string>
|
||||
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;
|
||||
|
||||
tl::expected<bool, std::string>
|
||||
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,
|
||||
|
||||
@ -63,12 +63,12 @@ public:
|
||||
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||
|
||||
tl::expected<bool, std::string>
|
||||
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;
|
||||
|
||||
tl::expected<bool, std::string>
|
||||
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,
|
||||
|
||||
@ -126,7 +126,7 @@ public:
|
||||
* \param [in] path_constraints Path contraints for the planning problem
|
||||
* \return true If the solver found a successful solution for the given planning problem
|
||||
*/
|
||||
tl::expected<bool, std::string> 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;
|
||||
@ -142,7 +142,7 @@ public:
|
||||
* \param [in] path_constraints Path contraints for the planning problem
|
||||
* \return true If the solver found a successful solution for the given planning problem
|
||||
*/
|
||||
tl::expected<bool, std::string> 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
|
||||
*/
|
||||
tl::expected<bool, std::string> 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,
|
||||
|
||||
@ -41,8 +41,8 @@
|
||||
#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>
|
||||
#include <tl_expected/expected.hpp>
|
||||
|
||||
namespace planning_scene {
|
||||
MOVEIT_CLASS_FORWARD(PlanningScene);
|
||||
@ -65,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
|
||||
{
|
||||
@ -89,13 +92,13 @@ public:
|
||||
virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0;
|
||||
|
||||
/// plan trajectory between to robot states
|
||||
virtual tl::expected<bool, std::string>
|
||||
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 tl::expected<bool, std::string>
|
||||
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,
|
||||
|
||||
@ -63,15 +63,15 @@ CartesianPath::CartesianPath() {
|
||||
|
||||
void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {}
|
||||
|
||||
tl::expected<bool, std::string> 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 tl::make_unexpected("no unique tip for joint model group: " + jmg->getName());
|
||||
return MoveItErrorCode(MoveItErrorCodes::FAILURE, "no unique tip for joint model group: " + jmg->getName());
|
||||
}
|
||||
|
||||
// reach pose of forward kinematics
|
||||
@ -79,12 +79,11 @@ tl::expected<bool, std::string> CartesianPath::plan(const planning_scene::Planni
|
||||
std::min(timeout, properties().get<double>("timeout")), result, path_constraints);
|
||||
}
|
||||
|
||||
tl::expected<bool, std::string> 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();
|
||||
|
||||
@ -117,9 +116,10 @@ tl::expected<bool, std::string> CartesianPath::plan(const planning_scene::Planni
|
||||
props.get<double>("max_acceleration_scaling_factor"));
|
||||
|
||||
if (achieved_fraction < props.get<double>("min_fraction")) {
|
||||
return tl::make_unexpected("Min fraction not met. Achieved fraction : " + std::to_string(achieved_fraction));
|
||||
return MoveItErrorCode(MoveItErrorCodes::FAILURE,
|
||||
"Min fraction not met. Achieved fraction : " + std::to_string(achieved_fraction));
|
||||
}
|
||||
return true;
|
||||
return MoveItErrorCode(MoveItErrorCodes::SUCCESS);
|
||||
}
|
||||
} // namespace solvers
|
||||
} // namespace task_constructor
|
||||
|
||||
@ -59,10 +59,11 @@ JointInterpolationPlanner::JointInterpolationPlanner() {
|
||||
|
||||
void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_model*/) {}
|
||||
|
||||
tl::expected<bool, std::string> 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
|
||||
@ -77,7 +78,7 @@ tl::expected<bool, std::string> JointInterpolationPlanner::plan(
|
||||
// add first point
|
||||
result->addSuffixWayPoint(from->getCurrentState(), 0.0);
|
||||
if (from->isStateColliding(from_state, jmg->getName()) || !from_state.satisfiesBounds(jmg))
|
||||
return tl::make_unexpected("Start state in collision or not within bounds");
|
||||
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;
|
||||
@ -86,13 +87,14 @@ tl::expected<bool, std::string> JointInterpolationPlanner::plan(
|
||||
result->addSuffixWayPoint(waypoint, t);
|
||||
|
||||
if (from->isStateColliding(waypoint, jmg->getName()) || !waypoint.satisfiesBounds(jmg))
|
||||
return tl::make_unexpected("One of the waypoint's state is in collision or not within bounds");
|
||||
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 tl::make_unexpected("Goal state in collision or not within bounds");
|
||||
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"),
|
||||
@ -112,15 +114,15 @@ tl::expected<bool, std::string> JointInterpolationPlanner::plan(
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
return MoveItErrorCode(MoveItErrorCodes::SUCCESS);
|
||||
}
|
||||
|
||||
tl::expected<bool, std::string>
|
||||
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);
|
||||
|
||||
@ -140,12 +142,13 @@ JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& fro
|
||||
// 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 tl::make_unexpected("IK failed for pose target");
|
||||
return MoveItErrorCode(MoveItErrorCodes::FAILURE, "IK failed for pose target.");
|
||||
}
|
||||
to->getCurrentStateNonConst().update();
|
||||
|
||||
if (std::chrono::steady_clock::now() >= deadline)
|
||||
return tl::make_unexpected("Timed out");
|
||||
if (std::chrono::steady_clock::now() >= deadline) {
|
||||
return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Timed out.");
|
||||
}
|
||||
|
||||
return plan(from, to, jmg, timeout, result, path_constraints);
|
||||
}
|
||||
|
||||
@ -49,51 +49,50 @@ void MultiPlanner::init(const core::RobotModelConstPtr& robot_model) {
|
||||
p->init(robot_model);
|
||||
}
|
||||
|
||||
tl::expected<bool, std::string> 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);
|
||||
}
|
||||
|
||||
tl::expected<bool, std::string> 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
|
||||
|
||||
@ -140,24 +140,23 @@ void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
|
||||
}
|
||||
}
|
||||
|
||||
tl::expected<bool, std::string> 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);
|
||||
}
|
||||
|
||||
tl::expected<bool, std::string> 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
|
||||
@ -172,11 +171,11 @@ tl::expected<bool, std::string> PipelinePlanner::plan(const planning_scene::Plan
|
||||
return plan(from, joint_model_group, goal_constraints, timeout, result, path_constraints);
|
||||
}
|
||||
|
||||
tl::expected<bool, std::string> 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());
|
||||
@ -226,14 +225,12 @@ tl::expected<bool, std::string> PipelinePlanner::plan(const planning_scene::Plan
|
||||
last_successful_planner_ = solution.planner_id;
|
||||
if (!bool(result)) // If the plan was not a success
|
||||
{
|
||||
return tl::make_unexpected(solution.error_code.source + " returned error code " +
|
||||
moveit::core::errorCodeToString(solution.error_code) +
|
||||
". Reason : " + solution.error_code.message); // Maybe print the error code too
|
||||
return MoveItErrorCode(solution.error_code.val, solution.error_code.message, solution.error_code.source);
|
||||
}
|
||||
return true;
|
||||
return MoveItErrorCode(MoveItErrorCodes::SUCCESS);
|
||||
}
|
||||
}
|
||||
return tl::make_unexpected("No solutions generated from Pipeline Planner");
|
||||
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_;
|
||||
|
||||
@ -161,18 +161,17 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
|
||||
intermediate_scenes.push_back(end);
|
||||
|
||||
robot_trajectory::RobotTrajectoryPtr trajectory;
|
||||
const auto planner_solution_maybe = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints);
|
||||
if (planner_solution_maybe.has_value()) {
|
||||
success = planner_solution_maybe.value();
|
||||
}
|
||||
std::string error_message = "";
|
||||
if (!success) {
|
||||
error_message += planner_solution_maybe.error();
|
||||
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;
|
||||
@ -184,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("Hardcode an error message");
|
||||
solution->markAsFailure(error_message);
|
||||
|
||||
connect(from, to, solution);
|
||||
}
|
||||
|
||||
@ -199,13 +199,13 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
|
||||
if (getJointStateFromOffset(direction, dir, jmg, scene->getCurrentStateNonConst())) {
|
||||
// plan to joint-space target
|
||||
const auto planner_solution_maybe =
|
||||
const auto planner_solution_status =
|
||||
planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
|
||||
if (planner_solution_maybe.has_value()) {
|
||||
success = planner_solution_maybe.value();
|
||||
if (bool(planner_solution_status)) {
|
||||
success = true;
|
||||
}
|
||||
if (!success) {
|
||||
error_message = planner_solution_maybe.error();
|
||||
error_message = planner_solution_status.message;
|
||||
}
|
||||
solution.setPlannerId(planner_->getPlannerId());
|
||||
} else {
|
||||
@ -294,13 +294,13 @@ 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;
|
||||
|
||||
const auto planner_solution_maybe =
|
||||
const auto planner_solution_status =
|
||||
planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
|
||||
if (planner_solution_maybe.has_value()) {
|
||||
success = planner_solution_maybe.value();
|
||||
if (bool(planner_solution_status)) {
|
||||
success = true;
|
||||
}
|
||||
if (!success) {
|
||||
error_message = planner_solution_maybe.error();
|
||||
error_message = planner_solution_status.message;
|
||||
}
|
||||
solution.setPlannerId(planner_->getPlannerId());
|
||||
|
||||
|
||||
@ -209,13 +209,13 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
|
||||
|
||||
if (getJointStateGoal(goal, jmg, scene->getCurrentStateNonConst())) {
|
||||
// plan to joint-space target
|
||||
const auto planner_solution_maybe =
|
||||
const auto planner_solution_status =
|
||||
planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
|
||||
if (planner_solution_maybe.has_value()) {
|
||||
success = planner_solution_maybe.value();
|
||||
if (bool(planner_solution_status)) {
|
||||
success = true;
|
||||
}
|
||||
if (!success) {
|
||||
error_message = planner_solution_maybe.error();
|
||||
error_message = planner_solution_status.message;
|
||||
}
|
||||
solution.setPlannerId(planner_->getPlannerId());
|
||||
} else { // Cartesian goal
|
||||
@ -249,13 +249,13 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
|
||||
Eigen::Isometry3d offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
|
||||
|
||||
// plan to Cartesian target
|
||||
const auto planner_solution_maybe =
|
||||
const auto planner_solution_status =
|
||||
planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints);
|
||||
if (planner_solution_maybe.has_value()) {
|
||||
success = planner_solution_maybe.value();
|
||||
if (bool(planner_solution_status)) {
|
||||
success = true;
|
||||
}
|
||||
if (!success) {
|
||||
error_message = planner_solution_maybe.error();
|
||||
error_message = planner_solution_status.message;
|
||||
}
|
||||
solution.setPlannerId(planner_->getPlannerId());
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user