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),
00056 kmodel_(model)
00057 {
00058 std::string planner;
00059 if (nh_.getParam(planner_plugin_param_name, planner))
00060 planner_plugin_name_ = planner;
00061
00062 std::string adapters;
00063 if (nh_.getParam(adapter_plugins_param_name, adapters))
00064 {
00065 boost::char_separator<char> sep(" ");
00066 boost::tokenizer<boost::char_separator<char> > tok(adapters, sep);
00067 for(boost::tokenizer<boost::char_separator<char> >::iterator beg = tok.begin() ; beg != tok.end(); ++beg)
00068 adapter_plugin_names_.push_back(*beg);
00069 }
00070
00071 configure();
00072 }
00073
00074 planning_pipeline::PlanningPipeline::PlanningPipeline(const robot_model::RobotModelConstPtr& model,
00075 const ros::NodeHandle &nh,
00076 const std::string &planner_plugin_name,
00077 const std::vector<std::string> &adapter_plugin_names) :
00078 nh_(nh),
00079 planner_plugin_name_(planner_plugin_name),
00080 adapter_plugin_names_(adapter_plugin_names),
00081 kmodel_(model)
00082 {
00083 configure();
00084 }
00085
00086 void planning_pipeline::PlanningPipeline::configure()
00087 {
00088 check_solution_paths_ = false;
00089 publish_received_requests_ = false;
00090 display_computed_motion_plans_ = false;
00091
00092
00093 try
00094 {
00095 planner_plugin_loader_.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>("moveit_core", "planning_interface::PlannerManager"));
00096 }
00097 catch(pluginlib::PluginlibException& ex)
00098 {
00099 ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
00100 }
00101
00102 std::vector<std::string> classes;
00103 if (planner_plugin_loader_)
00104 classes = planner_plugin_loader_->getDeclaredClasses();
00105 if (planner_plugin_name_.empty() && classes.size() == 1)
00106 {
00107 planner_plugin_name_ = classes[0];
00108 ROS_INFO("No '~planning_plugin' parameter specified, but only '%s' planning plugin is available. Using that one.", planner_plugin_name_.c_str());
00109 }
00110 if (planner_plugin_name_.empty() && classes.size() > 1)
00111 {
00112 planner_plugin_name_ = classes[0];
00113 ROS_INFO("Multiple planning plugins available. You should specify the '~planning_plugin' parameter. Using '%s' for now.", planner_plugin_name_.c_str());
00114 }
00115 try
00116 {
00117 planner_instance_.reset(planner_plugin_loader_->createUnmanagedInstance(planner_plugin_name_));
00118 if (!planner_instance_->initialize(kmodel_, nh_.getNamespace()))
00119 throw std::runtime_error("Unable to initialize planning plugin");
00120 ROS_INFO_STREAM("Using planning interface '" << planner_instance_->getDescription() << "'");
00121 }
00122 catch(pluginlib::PluginlibException& ex)
00123 {
00124 ROS_ERROR_STREAM("Exception while loading planner '" << 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>("moveit_core", "planning_request_adapter::PlanningRequestAdapter"));
00135 }
00136 catch(pluginlib::PluginlibException& ex)
00137 {
00138 ROS_ERROR_STREAM("Exception while creating planning plugin loader " << ex.what());
00139 }
00140
00141 if (adapter_plugin_loader_)
00142 for (std::size_t i = 0 ; i < adapter_plugin_names_.size() ; ++i)
00143 {
00144 planning_request_adapter::PlanningRequestAdapterConstPtr ad;
00145 try
00146 {
00147 ad.reset(adapter_plugin_loader_->createUnmanagedInstance(adapter_plugin_names_[i]));
00148 }
00149 catch (pluginlib::PluginlibException& ex)
00150 {
00151 ROS_ERROR_STREAM("Exception while loading planning adapter plugin '" << adapter_plugin_names_[i] << "': " << ex.what());
00152 }
00153 if (ad)
00154 ads.push_back(ad);
00155 }
00156 if (!ads.empty())
00157 {
00158 adapter_chain_.reset(new planning_request_adapter::PlanningRequestAdapterChain());
00159 for (std::size_t i = 0 ; i < ads.size() ; ++i)
00160 {
00161 ROS_INFO_STREAM("Using planning request adapter '" << ads[i]->getDescription() << "'");
00162 adapter_chain_->addAdapter(ads[i]);
00163 }
00164 }
00165 }
00166 displayComputedMotionPlans(true);
00167 checkSolutionPaths(true);
00168 }
00169
00170 void planning_pipeline::PlanningPipeline::displayComputedMotionPlans(bool flag)
00171 {
00172 if (display_computed_motion_plans_ && !flag)
00173 display_path_publisher_.shutdown();
00174 else
00175 if (!display_computed_motion_plans_ && flag)
00176 display_path_publisher_ = nh_.advertise<moveit_msgs::DisplayTrajectory>(DISPLAY_PATH_TOPIC, 10, true);
00177 display_computed_motion_plans_ = flag;
00178 }
00179
00180 void planning_pipeline::PlanningPipeline::publishReceivedRequests(bool flag)
00181 {
00182 if (publish_received_requests_ && !flag)
00183 received_request_publisher_.shutdown();
00184 else
00185 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
00195 if (!check_solution_paths_ && flag)
00196 contacts_publisher_ = nh_.advertise<visualization_msgs::MarkerArray>(MOTION_CONTACTS_TOPIC, 100, true);
00197 check_solution_paths_ = flag;
00198 }
00199
00200 bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
00201 const planning_interface::MotionPlanRequest& req,
00202 planning_interface::MotionPlanResponse& res) const
00203 {
00204 std::vector<std::size_t> dummy;
00205 return generatePlan(planning_scene, req, res, dummy);
00206 }
00207
00208 bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
00209 const planning_interface::MotionPlanRequest& req,
00210 planning_interface::MotionPlanResponse& res,
00211 std::vector<std::size_t> &adapter_added_state_index) const
00212 {
00213
00214 if (publish_received_requests_)
00215 received_request_publisher_.publish(req);
00216 adapter_added_state_index.clear();
00217
00218 if (!planner_instance_)
00219 {
00220 ROS_ERROR("No planning plugin loaded. Cannot plan.");
00221 return false;
00222 }
00223
00224 bool solved = false;
00225 try
00226 {
00227 if (adapter_chain_)
00228 {
00229 solved = adapter_chain_->adaptAndPlan(planner_instance_, planning_scene, req, res, adapter_added_state_index);
00230 if (!adapter_added_state_index.empty())
00231 {
00232 std::stringstream ss;
00233 for (std::size_t i = 0 ; i < adapter_added_state_index.size() ; ++i)
00234 ss << adapter_added_state_index[i] << " ";
00235 ROS_INFO("Planning adapters have added states at index positions: [ %s]", ss.str().c_str());
00236 }
00237 }
00238 else
00239 {
00240 planning_interface::PlanningContextPtr context = 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: [ " << ss.str() << "] out of " << state_count
00294 << ". Explanations follow in command line. Contacts are published on " << nh_.resolveName(MOTION_CONTACTS_TOPIC));
00295
00296
00297 visualization_msgs::MarkerArray arr;
00298 for (std::size_t i = 0 ; i < index.size() ; ++i)
00299 {
00300
00301 const robot_state::RobotState &kstate = res.trajectory_->getWayPoint(index[i]);
00302 planning_scene->isStateValid(kstate, req.path_constraints, req.group_name, true);
00303
00304
00305 collision_detection::CollisionRequest c_req;
00306 collision_detection::CollisionResult c_res;
00307 c_req.contacts = true;
00308 c_req.max_contacts = 10;
00309 c_req.max_contacts_per_pair = 3;
00310 c_req.verbose = false;
00311 planning_scene->checkCollision(c_req, c_res, kstate);
00312 if (c_res.contact_count > 0)
00313 {
00314 visualization_msgs::MarkerArray arr_i;
00315 collision_detection::getCollisionMarkersFromContacts(arr_i, planning_scene->getPlanningFrame(), c_res.contacts);
00316 arr.markers.insert(arr.markers.end(), arr_i.markers.begin(), arr_i.markers.end());
00317 }
00318 }
00319 ROS_ERROR_STREAM("Completed listing of explanations for invalid states.");
00320 if (!arr.markers.empty())
00321 contacts_publisher_.publish(arr);
00322 }
00323 }
00324 else
00325 ROS_DEBUG("Planned path was found to be valid, except for states that were added by planning request adapters, but that is ok.");
00326 }
00327 else
00328 ROS_DEBUG("Planned path was found to be valid when rechecked");
00329 }
00330 }
00331
00332
00333 if (display_computed_motion_plans_ && solved)
00334 {
00335 moveit_msgs::DisplayTrajectory disp;
00336 disp.model_id = kmodel_->getName();
00337 disp.trajectory.resize(1);
00338 res.trajectory_->getRobotTrajectoryMsg(disp.trajectory[0]);
00339 robot_state::robotStateToRobotStateMsg(res.trajectory_->getFirstWayPoint(), disp.trajectory_start);
00340 display_path_publisher_.publish(disp);
00341 }
00342
00343 return solved && valid;
00344 }
00345
00346 void planning_pipeline::PlanningPipeline::terminate() const
00347 {
00348 if (planner_instance_)
00349 planner_instance_->terminate();
00350 }