00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/planning_pipeline/planning_pipeline.h>
00038 #include <moveit/robot_state/conversions.h>
00039 #include <moveit/collision_detection/collision_tools.h>
00040 #include <moveit/trajectory_processing/trajectory_tools.h>
00041 #include <moveit_msgs/DisplayTrajectory.h>
00042 #include <visualization_msgs/MarkerArray.h>
00043 #include <boost/tokenizer.hpp>
00044 #include <boost/algorithm/string/join.hpp>
00045 #include <sstream>
00046
00047 const std::string planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC = "display_planned_path";
00048 const std::string planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC = "motion_plan_request";
00049 const std::string planning_pipeline::PlanningPipeline::MOTION_CONTACTS_TOPIC = "display_contacts";
00050
00051 planning_pipeline::PlanningPipeline::PlanningPipeline(const robot_model::RobotModelConstPtr& model,
00052 const ros::NodeHandle& nh,
00053 const std::string& planner_plugin_param_name,
00054 const std::string& adapter_plugins_param_name)
00055 : nh_(nh), kmodel_(model)
00056 {
00057 std::string planner;
00058 if (nh_.getParam(planner_plugin_param_name, planner))
00059 planner_plugin_name_ = planner;
00060
00061 std::string adapters;
00062 if (nh_.getParam(adapter_plugins_param_name, adapters))
00063 {
00064 boost::char_separator<char> sep(" ");
00065 boost::tokenizer<boost::char_separator<char> > tok(adapters, sep);
00066 for (boost::tokenizer<boost::char_separator<char> >::iterator beg = tok.begin(); beg != tok.end(); ++beg)
00067 adapter_plugin_names_.push_back(*beg);
00068 }
00069
00070 configure();
00071 }
00072
00073 planning_pipeline::PlanningPipeline::PlanningPipeline(const robot_model::RobotModelConstPtr& model,
00074 const ros::NodeHandle& nh, const std::string& planner_plugin_name,
00075 const std::vector<std::string>& adapter_plugin_names)
00076 : nh_(nh), planner_plugin_name_(planner_plugin_name), adapter_plugin_names_(adapter_plugin_names), kmodel_(model)
00077 {
00078 configure();
00079 }
00080
00081 void planning_pipeline::PlanningPipeline::configure()
00082 {
00083 check_solution_paths_ = false;
00084 publish_received_requests_ = false;
00085 display_computed_motion_plans_ = false;
00086
00087
00088 try
00089 {
00090 planner_plugin_loader_.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
00091 "moveit_core", "planning_interface::PlannerManager"));
00092 }
00093 catch (pluginlib::PluginlibException& ex)
00094 {
00095 ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
00096 }
00097
00098 std::vector<std::string> classes;
00099 if (planner_plugin_loader_)
00100 classes = planner_plugin_loader_->getDeclaredClasses();
00101 if (planner_plugin_name_.empty() && classes.size() == 1)
00102 {
00103 planner_plugin_name_ = classes[0];
00104 ROS_INFO("No '~planning_plugin' parameter specified, but only '%s' planning plugin is available. Using that one.",
00105 planner_plugin_name_.c_str());
00106 }
00107 if (planner_plugin_name_.empty() && classes.size() > 1)
00108 {
00109 planner_plugin_name_ = classes[0];
00110 ROS_INFO("Multiple planning plugins available. You should specify the '~planning_plugin' parameter. Using '%s' for "
00111 "now.",
00112 planner_plugin_name_.c_str());
00113 }
00114 try
00115 {
00116 planner_instance_.reset(planner_plugin_loader_->createUnmanagedInstance(planner_plugin_name_));
00117 if (!planner_instance_->initialize(kmodel_, nh_.getNamespace()))
00118 throw std::runtime_error("Unable to initialize planning plugin");
00119 ROS_INFO_STREAM("Using planning interface '" << planner_instance_->getDescription() << "'");
00120 }
00121 catch (pluginlib::PluginlibException& ex)
00122 {
00123 ROS_ERROR_STREAM("Exception while loading planner '"
00124 << planner_plugin_name_ << "': " << ex.what() << std::endl
00125 << "Available plugins: " << boost::algorithm::join(classes, ", "));
00126 }
00127
00128
00129 if (!adapter_plugin_names_.empty())
00130 {
00131 std::vector<planning_request_adapter::PlanningRequestAdapterConstPtr> ads;
00132 try
00133 {
00134 adapter_plugin_loader_.reset(new pluginlib::ClassLoader<planning_request_adapter::PlanningRequestAdapter>(
00135 "moveit_core", "planning_request_adapter::PlanningRequestAdapter"));
00136 }
00137 catch (pluginlib::PluginlibException& ex)
00138 {
00139 ROS_ERROR_STREAM("Exception while creating planning plugin loader " << ex.what());
00140 }
00141
00142 if (adapter_plugin_loader_)
00143 for (std::size_t i = 0; i < adapter_plugin_names_.size(); ++i)
00144 {
00145 planning_request_adapter::PlanningRequestAdapterConstPtr ad;
00146 try
00147 {
00148 ad.reset(adapter_plugin_loader_->createUnmanagedInstance(adapter_plugin_names_[i]));
00149 }
00150 catch (pluginlib::PluginlibException& ex)
00151 {
00152 ROS_ERROR_STREAM("Exception while loading planning adapter plugin '" << adapter_plugin_names_[i]
00153 << "': " << ex.what());
00154 }
00155 if (ad)
00156 ads.push_back(ad);
00157 }
00158 if (!ads.empty())
00159 {
00160 adapter_chain_.reset(new planning_request_adapter::PlanningRequestAdapterChain());
00161 for (std::size_t i = 0; i < ads.size(); ++i)
00162 {
00163 ROS_INFO_STREAM("Using planning request adapter '" << ads[i]->getDescription() << "'");
00164 adapter_chain_->addAdapter(ads[i]);
00165 }
00166 }
00167 }
00168 displayComputedMotionPlans(true);
00169 checkSolutionPaths(true);
00170 }
00171
00172 void planning_pipeline::PlanningPipeline::displayComputedMotionPlans(bool flag)
00173 {
00174 if (display_computed_motion_plans_ && !flag)
00175 display_path_publisher_.shutdown();
00176 else if (!display_computed_motion_plans_ && flag)
00177 display_path_publisher_ = nh_.advertise<moveit_msgs::DisplayTrajectory>(DISPLAY_PATH_TOPIC, 10, true);
00178 display_computed_motion_plans_ = flag;
00179 }
00180
00181 void planning_pipeline::PlanningPipeline::publishReceivedRequests(bool flag)
00182 {
00183 if (publish_received_requests_ && !flag)
00184 received_request_publisher_.shutdown();
00185 else if (!publish_received_requests_ && flag)
00186 received_request_publisher_ = nh_.advertise<moveit_msgs::MotionPlanRequest>(MOTION_PLAN_REQUEST_TOPIC, 10, true);
00187 publish_received_requests_ = flag;
00188 }
00189
00190 void planning_pipeline::PlanningPipeline::checkSolutionPaths(bool flag)
00191 {
00192 if (check_solution_paths_ && !flag)
00193 contacts_publisher_.shutdown();
00194 else if (!check_solution_paths_ && flag)
00195 contacts_publisher_ = nh_.advertise<visualization_msgs::MarkerArray>(MOTION_CONTACTS_TOPIC, 100, true);
00196 check_solution_paths_ = flag;
00197 }
00198
00199 bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
00200 const planning_interface::MotionPlanRequest& req,
00201 planning_interface::MotionPlanResponse& res) const
00202 {
00203 std::vector<std::size_t> dummy;
00204 return generatePlan(planning_scene, req, res, dummy);
00205 }
00206
00207 bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
00208 const planning_interface::MotionPlanRequest& req,
00209 planning_interface::MotionPlanResponse& res,
00210 std::vector<std::size_t>& adapter_added_state_index) const
00211 {
00212
00213 if (publish_received_requests_)
00214 received_request_publisher_.publish(req);
00215 adapter_added_state_index.clear();
00216
00217 if (!planner_instance_)
00218 {
00219 ROS_ERROR("No planning plugin loaded. Cannot plan.");
00220 return false;
00221 }
00222
00223 bool solved = false;
00224 try
00225 {
00226 if (adapter_chain_)
00227 {
00228 solved = adapter_chain_->adaptAndPlan(planner_instance_, planning_scene, req, res, adapter_added_state_index);
00229 if (!adapter_added_state_index.empty())
00230 {
00231 std::stringstream ss;
00232 for (std::size_t i = 0; i < adapter_added_state_index.size(); ++i)
00233 ss << adapter_added_state_index[i] << " ";
00234 ROS_INFO("Planning adapters have added states at index positions: [ %s]", ss.str().c_str());
00235 }
00236 }
00237 else
00238 {
00239 planning_interface::PlanningContextPtr context =
00240 planner_instance_->getPlanningContext(planning_scene, req, res.error_code_);
00241 solved = context ? context->solve(res) : false;
00242 }
00243 }
00244 catch (std::runtime_error& ex)
00245 {
00246 ROS_ERROR("Exception caught: '%s'", ex.what());
00247 return false;
00248 }
00249 catch (...)
00250 {
00251 ROS_ERROR("Unknown exception thrown by planner");
00252 return false;
00253 }
00254 bool valid = true;
00255
00256 if (solved && res.trajectory_)
00257 {
00258 std::size_t state_count = res.trajectory_->getWayPointCount();
00259 ROS_DEBUG_STREAM("Motion planner reported a solution path with " << state_count << " states");
00260 if (check_solution_paths_)
00261 {
00262 std::vector<std::size_t> index;
00263 if (!planning_scene->isPathValid(*res.trajectory_, req.path_constraints, req.group_name, false, &index))
00264 {
00265
00266
00267 bool problem = false;
00268 for (std::size_t i = 0; i < index.size() && !problem; ++i)
00269 {
00270 bool found = false;
00271 for (std::size_t j = 0; j < adapter_added_state_index.size(); ++j)
00272 if (index[i] == adapter_added_state_index[j])
00273 {
00274 found = true;
00275 break;
00276 }
00277 if (!found)
00278 problem = true;
00279 }
00280 if (problem)
00281 {
00282 if (index.size() == 1 && index[0] == 0)
00283 ROS_DEBUG("It appears the robot is starting at an invalid state, but that is ok.");
00284 else
00285 {
00286 valid = false;
00287 res.error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
00288
00289
00290 std::stringstream ss;
00291 for (std::size_t i = 0; i < index.size(); ++i)
00292 ss << index[i] << " ";
00293 ROS_ERROR_STREAM("Computed path is not valid. Invalid states at index locations: [ "
00294 << ss.str() << "] out of " << state_count
00295 << ". Explanations follow in command line. Contacts are published on "
00296 << nh_.resolveName(MOTION_CONTACTS_TOPIC));
00297
00298
00299 visualization_msgs::MarkerArray arr;
00300 for (std::size_t i = 0; i < index.size(); ++i)
00301 {
00302
00303 const robot_state::RobotState& kstate = res.trajectory_->getWayPoint(index[i]);
00304 planning_scene->isStateValid(kstate, req.path_constraints, req.group_name, true);
00305
00306
00307 collision_detection::CollisionRequest c_req;
00308 collision_detection::CollisionResult c_res;
00309 c_req.contacts = true;
00310 c_req.max_contacts = 10;
00311 c_req.max_contacts_per_pair = 3;
00312 c_req.verbose = false;
00313 planning_scene->checkCollision(c_req, c_res, kstate);
00314 if (c_res.contact_count > 0)
00315 {
00316 visualization_msgs::MarkerArray arr_i;
00317 collision_detection::getCollisionMarkersFromContacts(arr_i, planning_scene->getPlanningFrame(),
00318 c_res.contacts);
00319 arr.markers.insert(arr.markers.end(), arr_i.markers.begin(), arr_i.markers.end());
00320 }
00321 }
00322 ROS_ERROR_STREAM("Completed listing of explanations for invalid states.");
00323 if (!arr.markers.empty())
00324 contacts_publisher_.publish(arr);
00325 }
00326 }
00327 else
00328 ROS_DEBUG("Planned path was found to be valid, except for states that were added by planning request "
00329 "adapters, but that is ok.");
00330 }
00331 else
00332 ROS_DEBUG("Planned path was found to be valid when rechecked");
00333 }
00334 }
00335
00336
00337 if (display_computed_motion_plans_ && solved)
00338 {
00339 moveit_msgs::DisplayTrajectory disp;
00340 disp.model_id = kmodel_->getName();
00341 disp.trajectory.resize(1);
00342 res.trajectory_->getRobotTrajectoryMsg(disp.trajectory[0]);
00343 robot_state::robotStateToRobotStateMsg(res.trajectory_->getFirstWayPoint(), disp.trajectory_start);
00344 display_path_publisher_.publish(disp);
00345 }
00346
00347 return solved && valid;
00348 }
00349
00350 void planning_pipeline::PlanningPipeline::terminate() const
00351 {
00352 if (planner_instance_)
00353 planner_instance_->terminate();
00354 }