28 #include <boost/scoped_ptr.hpp> 56 "pilz::PlanningContextLoader"));
61 for (
const auto& factory : factories)
69 for (
const auto& factory : factories)
77 limits.setCartesianLimits(cartesian_limit_);
79 loader_pointer->setLimits(limits);
80 loader_pointer->setModel(
model_);
91 return "Simple Command Planner";
100 algs.push_back(context_loader.first);
106 const moveit_msgs::MotionPlanRequest& req,
107 moveit_msgs::MoveItErrorCodes& error_code)
const 109 ROS_DEBUG_STREAM(
"Loading PlanningContext for request\n<request>\n" << req <<
"\n</request>");
114 ROS_ERROR_STREAM(
"No ContextLoader for planner_id " << req.planner_id.c_str() <<
" found. Planning not possible.");
118 planning_interface::PlanningContextPtr planning_context;
120 if(
context_loader_map_.at(req.planner_id)->loadContext(planning_context, req.planner_id, req.group_name))
122 ROS_DEBUG_STREAM(
"Found planning context loader for " << req.planner_id <<
" group:" << req.group_name);
123 planning_context->setMotionPlanRequest(req);
124 planning_context->setPlanningScene(planning_scene);
125 return planning_context;
128 error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
145 ROS_INFO_STREAM(
"Registered Algorithm [" << planning_context_loader->getAlgorithm() <<
"]");
150 +
"] is already registered");
std::map< std::string, pilz::PlanningContextLoaderPtr > context_loader_map_
Mapping from command to loader.
virtual planning_interface::PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, moveit_msgs::MoveItErrorCodes &error_code) const override
Returns a PlanningContext that can be used to solve(calculate) the trajectory that corresponds to com...
pilz::CartesianLimit cartesian_limit_
cartesian limit
std::string namespace_
Namespace where the parameters are stored, obtained at initialize.
static JointLimitsContainer getAggregatedLimits(const ros::NodeHandle &nh, const std::vector< const moveit::core::JointModel * > &joint_models)
Aggregates(combines) the joint limits from joint model and parameter server. The rules for the combin...
static CartesianLimit getAggregatedLimits(const ros::NodeHandle &nh)
Loads cartesian limits from the parameter server.
PLUGINLIB_EXPORT_CLASS(cached_ik_kinematics_plugin::CachedIKKinematicsPlugin< kdl_kinematics_plugin::KDLKinematicsPlugin >, kinematics::KinematicsBase)
virtual void getPlanningAlgorithms(std::vector< std::string > &algs) const override
Returns the available planning commands.
pilz::JointLimitsContainer aggregated_limit_active_joints_
aggregated limits of the active joints
virtual bool initialize(const robot_model::RobotModelConstPtr &model, const std::string &ns)
virtual std::string getDescription() const override
Description of the planner.
This class combines CartesianLimit and JointLimits into on single class.
#define ROS_DEBUG_STREAM(args)
boost::scoped_ptr< pluginlib::ClassLoader< PlanningContextLoader > > planner_context_loader
Plugin loader.
Moveit Plugin for Planning with Standart Robot Commands This planner is dedicated to return a instanc...
#define ROS_INFO_STREAM(args)
moveit::core::RobotModelConstPtr model_
Robot model obtained at initialize.
#define ROS_ERROR_STREAM(args)
void registerContextLoader(const pilz::PlanningContextLoaderPtr &planning_context_loader)
Register a PlanningContextLoader to be used by the CommandPlanner.
virtual bool initialize(const robot_model::RobotModelConstPtr &model, const std::string &ns) override
Initializes the planner Upon initialization this planner will look for plugins implementing pilz::Pla...
virtual bool canServiceRequest(const planning_interface::MotionPlanRequest &req) const override
Checks if the request can be handled.
static const std::string PARAM_NAMESPACE_LIMTS
void setJointLimits(JointLimitsContainer &joint_limits)
Set joint limits.