41 #include <moveit_msgs/DisplayTrajectory.h> 42 #include <visualization_msgs/MarkerArray.h> 43 #include <boost/tokenizer.hpp> 44 #include <boost/algorithm/string/join.hpp> 53 const std::string& planner_plugin_param_name,
54 const std::string& adapter_plugins_param_name)
55 : nh_(nh), kmodel_(model)
58 if (
nh_.
getParam(planner_plugin_param_name, planner))
62 if (
nh_.
getParam(adapter_plugins_param_name, adapters))
64 boost::char_separator<char> sep(
" ");
65 boost::tokenizer<boost::char_separator<char> > tok(adapters, sep);
66 for (boost::tokenizer<boost::char_separator<char> >::iterator beg = tok.begin(); beg != tok.end(); ++beg)
75 const std::vector<std::string>& adapter_plugin_names)
91 "moveit_core",
"planning_interface::PlannerManager"));
95 ROS_FATAL_STREAM(
"Exception while creating planning plugin loader " << ex.what());
98 std::vector<std::string> classes;
104 ROS_INFO(
"No '~planning_plugin' parameter specified, but only '%s' planning plugin is available. Using that one.",
110 ROS_INFO(
"Multiple planning plugins available. You should specify the '~planning_plugin' parameter. Using '%s' for " 118 throw std::runtime_error(
"Unable to initialize planning plugin");
125 <<
"Available plugins: " << boost::algorithm::join(classes,
", "));
131 std::vector<planning_request_adapter::PlanningRequestAdapterConstPtr> ads;
135 "moveit_core",
"planning_request_adapter::PlanningRequestAdapter"));
139 ROS_ERROR_STREAM(
"Exception while creating planning plugin loader " << ex.what());
145 planning_request_adapter::PlanningRequestAdapterConstPtr ad;
153 <<
"': " << ex.what());
161 for (std::size_t i = 0; i < ads.size(); ++i)
163 ROS_INFO_STREAM(
"Using planning request adapter '" << ads[i]->getDescription() <<
"'");
203 std::vector<std::size_t> dummy;
210 std::vector<std::size_t>& adapter_added_state_index)
const 215 adapter_added_state_index.clear();
219 ROS_ERROR(
"No planning plugin loaded. Cannot plan.");
229 if (!adapter_added_state_index.empty())
231 std::stringstream ss;
232 for (std::size_t i = 0; i < adapter_added_state_index.size(); ++i)
233 ss << adapter_added_state_index[i] <<
" ";
234 ROS_INFO(
"Planning adapters have added states at index positions: [ %s]", ss.str().c_str());
239 planning_interface::PlanningContextPtr context =
241 solved = context ? context->solve(res) :
false;
244 catch (std::exception& ex)
246 ROS_ERROR(
"Exception caught: '%s'", ex.what());
253 std::size_t state_count = res.
trajectory_->getWayPointCount();
254 ROS_DEBUG_STREAM(
"Motion planner reported a solution path with " << state_count <<
" states");
257 std::vector<std::size_t>
index;
258 if (!planning_scene->isPathValid(*res.
trajectory_, req.path_constraints, req.group_name,
false, &index))
262 bool problem =
false;
263 for (std::size_t i = 0; i < index.size() && !problem; ++i)
266 for (std::size_t j = 0; j < adapter_added_state_index.size(); ++j)
267 if (index[i] == adapter_added_state_index[j])
277 if (index.size() == 1 && index[0] == 0)
278 ROS_DEBUG(
"It appears the robot is starting at an invalid state, but that is ok.");
282 res.
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
285 std::stringstream ss;
286 for (std::size_t i = 0; i < index.size(); ++i)
287 ss << index[i] <<
" ";
288 ROS_ERROR_STREAM(
"Computed path is not valid. Invalid states at index locations: [ " 289 << ss.str() <<
"] out of " << state_count
290 <<
". Explanations follow in command line. Contacts are published on " 294 visualization_msgs::MarkerArray arr;
295 for (std::size_t i = 0; i < index.size(); ++i)
298 const robot_state::RobotState& kstate = res.
trajectory_->getWayPoint(index[i]);
299 planning_scene->isStateValid(kstate, req.path_constraints, req.group_name,
true);
308 planning_scene->checkCollision(c_req, c_res, kstate);
311 visualization_msgs::MarkerArray arr_i;
314 arr.markers.insert(arr.markers.end(), arr_i.markers.begin(), arr_i.markers.end());
318 if (!arr.markers.empty())
323 ROS_DEBUG(
"Planned path was found to be valid, except for states that were added by planning request " 324 "adapters, but that is ok.");
327 ROS_DEBUG(
"Planned path was found to be valid when rechecked");
334 moveit_msgs::DisplayTrajectory disp;
335 disp.model_id =
kmodel_->getName();
336 disp.trajectory.resize(1);
337 res.
trajectory_->getRobotTrajectoryMsg(disp.trajectory[0]);
338 robot_state::robotStateToRobotStateMsg(res.
trajectory_->getFirstWayPoint(), disp.trajectory_start);
342 return solved && valid;
robot_trajectory::RobotTrajectoryPtr trajectory_
static const std::string MOTION_PLAN_REQUEST_TOPIC
When motion planning requests are received and they are supposed to be automatically published...
void publish(const boost::shared_ptr< M > &message) const
void getCollisionMarkersFromContacts(visualization_msgs::MarkerArray &arr, const std::string &frame_id, const CollisionResult::ContactMap &con, const std_msgs::ColorRGBA &color, const ros::Duration &lifetime, const double radius=0.035)
bool generatePlan(const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res) const
Call the motion planner plugin and the sequence of planning request adapters (if any).
std::string resolveName(const std::string &name, bool remap=true) const
std::unique_ptr< planning_request_adapter::PlanningRequestAdapterChain > adapter_chain_
void publishReceivedRequests(bool flag)
Pass a flag telling the pipeline whether or not to publish the received motion planning requests on M...
void displayComputedMotionPlans(bool flag)
Pass a flag telling the pipeline whether or not to publish the computed motion plans on DISPLAY_PATH_...
moveit_msgs::MoveItErrorCodes error_code_
std::string planner_plugin_name_
std::size_t contact_count
void terminate() const
Request termination, if a generatePlan() function is currently computing plans.
#define ROS_FATAL_STREAM(args)
std::vector< std::string > adapter_plugin_names_
robot_model::RobotModelConstPtr kmodel_
ros::Publisher display_path_publisher_
const std::string & getNamespace() const
void checkSolutionPaths(bool flag)
Pass a flag telling the pipeline whether or not to re-check the solution paths reported by the planne...
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher received_request_publisher_
#define ROS_DEBUG_STREAM(args)
bool check_solution_paths_
Flag indicating whether the reported plans should be checked once again, by the planning pipeline its...
moveit_msgs::MotionPlanRequest MotionPlanRequest
static const std::string DISPLAY_PATH_TOPIC
When motion plans are computed and they are supposed to be automatically displayed, they are sent to this topic (moveit_msgs::DisplauTrajectory)
std::unique_ptr< pluginlib::ClassLoader< planning_interface::PlannerManager > > planner_plugin_loader_
std::size_t max_contacts_per_pair
#define ROS_INFO_STREAM(args)
bool publish_received_requests_
ros::Publisher contacts_publisher_
bool getParam(const std::string &key, std::string &s) const
bool display_computed_motion_plans_
Flag indicating whether motion plans should be published as a moveit_msgs::DisplayTrajectory.
planning_interface::PlannerManagerPtr planner_instance_
#define ROS_ERROR_STREAM(args)
static const std::string MOTION_CONTACTS_TOPIC
When contacts are found in the solution path reported by a planner, they can be published as markers ...
std::unique_ptr< pluginlib::ClassLoader< planning_request_adapter::PlanningRequestAdapter > > adapter_plugin_loader_
PlanningPipeline(const robot_model::RobotModelConstPtr &model, const ros::NodeHandle &nh=ros::NodeHandle("~"), const std::string &planning_plugin_param_name="planning_plugin", const std::string &adapter_plugins_param_name="request_adapters")
Given a robot model (model), a node handle (nh), initialize the planning pipeline.