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), 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;  // this is set to true below
00084   publish_received_requests_ = false;
00085   display_computed_motion_plans_ = false;  // this is set to true below
00086 
00087   // load the planning plugin
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   // 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>(
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   // broadcast the request we are about to work on, if needed
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         // 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: [ "
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             // call validity checks in verbose mode for the problematic states
00299             visualization_msgs::MarkerArray arr;
00300             for (std::size_t i = 0; i < index.size(); ++i)
00301             {
00302               // check validity with verbose on
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               // compute the contacts if any
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   // display solution path if needed
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 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jun 19 2019 19:24:16