Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Discuss some force-pushed changes #545

Closed
wants to merge 5 commits into from
Closed

Discuss some force-pushed changes #545

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
f6d0b3b
Use MoveItErrorCode and revert some changes for discussion
sjahr Mar 18, 2024
da874df
Update core/src/solvers/pipeline_planner.cpp
sjahr Mar 18, 2024
e11f89c
Update core/src/solvers/pipeline_planner.cpp
sjahr Mar 25, 2024
6039ef8
Remove planning pipelines argument
sjahr Mar 25, 2024
e397b52
Update core/src/solvers/pipeline_planner.cpp
sjahr Mar 25, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions core/include/moveit/task_constructor/solvers/cartesian_path.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,11 +63,11 @@ class CartesianPath : public PlannerInterface

void init(const moveit::core::RobotModelConstPtr& robot_model) override;

Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
MoveItErrorCode plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why did you decide to introduce Result instead of using error codes? Aren't we loosing information by reducing the error code to a boolean?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Originally, #525 introduced tl::expected<bool, std::string> to return both, a boolean result and an error message. Later, this was changed to MoveItErrorCode, which - in ROS2 - supports the error message and an error source. As this extended MoveItErrorCode is not available in ROS1 and the purpose was just to return an additional string message, I introduced an explicit data type that is uniformly usable in ROS1 and ROS2.
If at all, the source information is lost (it was never fed into the error code).

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't consider reverting to MoveItErrorCode and I don't see something missing so far.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If at all, the source information is lost (it was never fed into the error code).

Good point that should be done, otherwise that field is pointless.

I see, have you considered enhancing the error code with a string message in moveit1? The main motivation here was to improve the debug-ability of moveit in general. The source field was introduced to infer the planning pipeline that created the error message when using parallel planning. Without that, I think it is just possible to tell that one of the planners failed with the given error message. Not which one. Alternatively, the string could be enhanced with that information.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, I shortly considered augmenting MoveItErrorCode in MoveIt1 as well. However, I decided against, because the error message is redundant as we already have a fixed mapping from the error code to a human-readable message (MoveItErrorCode::toString()). Further, the source member was not filled regularly - only in planning adapters. As far as I can see, the planning pipeline name is not yet stored in that field.

Note, that there are two MoveItErrorCode header files (I think one should be removed):

  • moveit_core/utils/include/moveit/utils/moveit_error_code.h (regularly used)
  • moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/moveit_error_code_interface.h (used only once)

const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;

Result 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;
Expand Down
16 changes: 9 additions & 7 deletions core/include/moveit/task_constructor/solvers/joint_interpolation.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,14 +58,16 @@ class JointInterpolationPlanner : public PlannerInterface

void init(const moveit::core::RobotModelConstPtr& robot_model) override;

Result 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;

Result 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 "JointInterpolationPlanner"; }
};
Expand Down
16 changes: 9 additions & 7 deletions core/include/moveit/task_constructor/solvers/multi_planner.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,14 +63,16 @@ class MultiPlanner : public PlannerInterface, public std::vector<solvers::Planne

void init(const moveit::core::RobotModelConstPtr& robot_model) override;

Result 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;

Result 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
Expand Down
64 changes: 38 additions & 26 deletions core/include/moveit/task_constructor/solvers/pipeline_planner.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,10 +59,10 @@ MOVEIT_CLASS_FORWARD(PipelinePlanner);
class PipelinePlanner : public PlannerInterface
{
public:
/** Simple Constructor to use only a single pipeline
/** \brief Simple Constructor to use only a single pipeline
* \param [in] node ROS 2 node
* \param [in] pipeline_name Name of the planning pipeline to be used.
* This is also the assumed namespace where the parameters of this pipeline can be found
* This is also the assumed namespace where the parameters of this pipeline can be found.
* \param [in] planner_id Planner id to be used for planning. Empty string means default.
*/
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name = "ompl",
Expand All @@ -71,9 +71,9 @@ class PipelinePlanner : public PlannerInterface

/** \brief Constructor
* \param [in] node ROS 2 node
* \param [in] pipeline_id_planner_id_map map containing pairs of pipeline and plugin names to be used for planning
* \param [in] stopping_criterion_callback callback to decide when to stop parallel planning
* \param [in] solution_selection_function callback to choose the best solution from multiple ran pipelines
* \param [in] pipeline_id_planner_id_map Map containing pairs of pipeline and plugin names to be used for planning
* \param [in] stopping_criterion_callback Optional: Callback to decide when to stop parallel planning
* \param [in] solution_selection_function Optional: Callback to choose the best solution from multiple ran pipelines
*/
PipelinePlanner(
const rclcpp::Node::SharedPtr& node,
Expand Down Expand Up @@ -101,7 +101,9 @@ class PipelinePlanner : public PlannerInterface
void setSolutionSelectionFunction(
const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function);

/** \brief If not yet done, initialize pipelines from pipeline_id_planner_id_map
/** \brief This function is called when an MTC task that uses this solver is initialized. If no pipelines are
sjahr marked this conversation as resolved.
Show resolved Hide resolved
* configured when this function is invoked, the planning pipelines named in the 'pipeline_id_planner_id_map' are
* initialized with the given robot model.
* \param [in] robot_model robot model used to initialize the planning pipelines of this solver
*/
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
Expand All @@ -113,14 +115,15 @@ class PipelinePlanner : public PlannerInterface
* \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.
*/
Result 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;
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;

/** \brief Plan a trajectory from a planning scene 'from' to a Cartesian target pose with an offset
/** \brief Plan a trajectory from a planning scene 'from' to a target pose with an offset
* \param [in] from Start planning scene
* \param [in] link Link for which a target pose is given
* \param [in] offset Offset to be applied to a given target pose
Expand All @@ -129,18 +132,26 @@ class PipelinePlanner : public PlannerInterface
* \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.
*/
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,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;

/**
* \brief Get planner name
* \return Name of the last successful planner
*/
Result 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,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;

std::string getPlannerId() const override { return last_successful_planner_; }

protected:
/** \brief Actual plan() implementation, targeting the given goal_constraints.
/** \brief Function that actually uses the planning pipelines to solve the given planning problem. It is called by
* the public plan function after the goal constraints are generated. This function uses a predefined number of
* planning pipelines in parallel to solve the planning problem and choose the best (user-defined) solution.
* \param [in] planning_scene Scene for which the planning should be solved
* \param [in] joint_model_group Group of joints for which this trajectory is created
* \param [in] goal_constraints Set of constraints that need to be satisfied by the solution
Expand All @@ -149,17 +160,18 @@ class PipelinePlanner : public PlannerInterface
* \param [in] path_constraints Path contraints for the planning problem
* \return true if the solver found a successful solution for the given planning problem
*/
Result 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 = moveit_msgs::msg::Constraints());
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,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints());

rclcpp::Node::SharedPtr node_;

std::string last_successful_planner_;

/** \brief Map of instantiated (and named) planning pipelines. */
/** \brief Map of pipelines names and planning pipelines. This map is used to quickly search for a requested motion
* planning pipeline when during plan(..) */
std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;

moveit::planning_pipeline_interfaces::StoppingCriterionFunction stopping_criterion_callback_;
Expand Down
30 changes: 13 additions & 17 deletions core/include/moveit/task_constructor/solvers/planner_interface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand All @@ -64,21 +65,16 @@ namespace moveit {
namespace task_constructor {
namespace solvers {

using MoveItErrorCode = moveit::core::MoveItErrorCode;
using MoveItErrorCodes = moveit_msgs::msg::MoveItErrorCodes;

MOVEIT_CLASS_FORWARD(PlannerInterface);
class PlannerInterface
{
// these properties take precedence over stage properties
PropertyMap properties_;

public:
struct Result
{
bool success;
std::string message;

operator bool() const { return success; }
};

PlannerInterface();
virtual ~PlannerInterface() {}

Expand All @@ -96,17 +92,17 @@ class PlannerInterface
virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0;

/// plan trajectory between to robot states
virtual Result 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 Result 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;
Expand Down
25 changes: 13 additions & 12 deletions core/src/solvers/cartesian_path.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,11 +63,11 @@ CartesianPath::CartesianPath() {

void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {}

PlannerInterface::Result 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)
return { false, "no unique tip for joint model group: " + jmg->getName() };
Expand All @@ -77,11 +77,11 @@ PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningScene
std::min(timeout, properties().get<double>("timeout")), result, path_constraints);
}

PlannerInterface::Result 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();

Expand Down Expand Up @@ -114,9 +114,10 @@ PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningScene
props.get<double>("max_acceleration_scaling_factor"));

if (achieved_fraction < props.get<double>("min_fraction")) {
return { false, "min_fraction not met. Achieved: " + std::to_string(achieved_fraction) };
return MoveItErrorCode(MoveItErrorCodes::FAILURE,
"Min fraction not met. Achieved fraction : " + std::to_string(achieved_fraction));
}
return { true, "achieved fraction: " + std::to_string(achieved_fraction) };
return MoveItErrorCode(MoveItErrorCodes::SUCCESS, "achieved fraction: " + std::to_string(achieved_fraction));
}
} // namespace solvers
} // namespace task_constructor
Expand Down
Loading
Loading