planning_pipeline.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
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;          // this is set to true below
00089   publish_received_requests_ = false;
00090   display_computed_motion_plans_ = false; // this is set to true below
00091 
00092   // load the planning plugin
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   // load the planner request adapters
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   // broadcast the request we are about to work on, if needed
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         // check to see if there is any problem with the states that are found to be invalid
00266         // they are considered ok if they were added by a planning request adapter
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) // ignore cases when the robot starts at invalid location
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             // display error messages
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             // call validity checks in verbose mode for the problematic states
00297             visualization_msgs::MarkerArray arr;
00298             for (std::size_t i = 0 ; i < index.size() ; ++i)
00299             {
00300               // check validity with verbose on
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               // compute the contacts if any
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   // display solution path if needed
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 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Aug 26 2015 12:43:30