plan_with_sensing.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/plan_execution/plan_with_sensing.h>
00038 #include <moveit/trajectory_processing/trajectory_tools.h>
00039 #include <moveit/collision_detection/collision_tools.h>
00040 #include <boost/algorithm/string/join.hpp>
00041 
00042 #include <dynamic_reconfigure/server.h>
00043 #include <moveit_ros_planning/SenseForPlanDynamicReconfigureConfig.h>
00044 
00045 namespace plan_execution
00046 {
00047 using namespace moveit_ros_planning;
00048 
00049 class PlanWithSensing::DynamicReconfigureImpl
00050 {
00051 public:
00052   DynamicReconfigureImpl(PlanWithSensing* owner)
00053     : owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle("~/sense_for_plan"))
00054   {
00055     dynamic_reconfigure_server_.setCallback(
00056         boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
00057   }
00058 
00059 private:
00060   void dynamicReconfigureCallback(SenseForPlanDynamicReconfigureConfig& config, uint32_t level)
00061   {
00062     owner_->setMaxSafePathCost(config.max_safe_path_cost);
00063     owner_->setMaxCostSources(config.max_cost_sources);
00064     owner_->setMaxLookAttempts(config.max_look_attempts);
00065     owner_->setDiscardOverlappingCostSources(config.discard_overlapping_cost_sources);
00066   }
00067 
00068   PlanWithSensing* owner_;
00069   dynamic_reconfigure::Server<SenseForPlanDynamicReconfigureConfig> dynamic_reconfigure_server_;
00070 };
00071 }
00072 
00073 plan_execution::PlanWithSensing::PlanWithSensing(
00074     const trajectory_execution_manager::TrajectoryExecutionManagerPtr& trajectory_execution)
00075   : node_handle_("~"), trajectory_execution_manager_(trajectory_execution)
00076 {
00077   default_max_look_attempts_ = 3;
00078   default_max_safe_path_cost_ = 0.5;
00079 
00080   discard_overlapping_cost_sources_ = 0.8;
00081   max_cost_sources_ = 100;
00082 
00083   // by default we do not display path cost sources
00084   display_cost_sources_ = false;
00085 
00086   // load the sensor manager plugin, if needed
00087   if (node_handle_.hasParam("moveit_sensor_manager"))
00088   {
00089     try
00090     {
00091       sensor_manager_loader_.reset(new pluginlib::ClassLoader<moveit_sensor_manager::MoveItSensorManager>(
00092           "moveit_core", "moveit_sensor_manager::MoveItSensorManager"));
00093     }
00094     catch (pluginlib::PluginlibException& ex)
00095     {
00096       ROS_ERROR_STREAM("Exception while creating sensor manager plugin loader: " << ex.what());
00097     }
00098     if (sensor_manager_loader_)
00099     {
00100       std::string manager;
00101       if (node_handle_.getParam("moveit_sensor_manager", manager))
00102         try
00103         {
00104           sensor_manager_ = sensor_manager_loader_->createInstance(manager);
00105         }
00106         catch (pluginlib::PluginlibException& ex)
00107         {
00108           ROS_ERROR_STREAM("Exception while loading sensor manager '" << manager << "': " << ex.what());
00109         }
00110     }
00111     if (sensor_manager_)
00112     {
00113       std::vector<std::string> sensors;
00114       sensor_manager_->getSensorsList(sensors);
00115       ROS_INFO_STREAM("PlanWithSensing is aware of the following sensors: " << boost::algorithm::join(sensors, ", "));
00116     }
00117   }
00118 
00119   // start the dynamic-reconfigure server
00120   reconfigure_impl_ = new DynamicReconfigureImpl(this);
00121 }
00122 
00123 plan_execution::PlanWithSensing::~PlanWithSensing()
00124 {
00125   delete reconfigure_impl_;
00126 }
00127 
00128 void plan_execution::PlanWithSensing::displayCostSources(bool flag)
00129 {
00130   if (flag && !display_cost_sources_)
00131     // publisher for cost sources
00132     cost_sources_publisher_ =
00133         node_handle_.advertise<visualization_msgs::MarkerArray>("display_cost_sources", 100, true);
00134   else if (!flag && display_cost_sources_)
00135     cost_sources_publisher_.shutdown();
00136   display_cost_sources_ = flag;
00137 }
00138 
00139 bool plan_execution::PlanWithSensing::computePlan(ExecutableMotionPlan& plan,
00140                                                   const ExecutableMotionPlanComputationFn& motion_planner,
00141                                                   unsigned int max_look_attempts, double max_safe_path_cost)
00142 {
00143   if (max_safe_path_cost <= std::numeric_limits<double>::epsilon())
00144     max_safe_path_cost = default_max_safe_path_cost_;
00145 
00146   if (max_look_attempts == 0)
00147     max_look_attempts = default_max_look_attempts_;
00148 
00149   double previous_cost = 0.0;
00150   unsigned int look_attempts = 0;
00151 
00152   // this flag is set to true when all conditions for looking around are met, and the command is sent.
00153   // the intention is for the planning looop not to terminate when having just looked around
00154   bool just_looked_around = false;
00155 
00156   // this flag indicates whether the last lookAt() operation failed. If this operation fails once, we assume that
00157   // maybe some information was gained anyway (the sensor moved part of the way) and we try to plan one more time.
00158   // If we have two sensor pointing failures in a row, we fail
00159   bool look_around_failed = false;
00160 
00161   // there can be a maximum number of looking attempts as well that lead to replanning, if the cost
00162   // of the path is above a maximum threshold.
00163   do
00164   {
00165     bool solved = motion_planner(plan);
00166     if (!solved || plan.error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
00167       return solved;
00168 
00169     // determine the sources of cost for this path
00170     std::set<collision_detection::CostSource> cost_sources;
00171     {
00172       planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_);  // it is ok if
00173                                                                                            // planning_scene_monitor_ is
00174                                                                                            // null; there just will be
00175                                                                                            // no locking done
00176       for (std::size_t i = 0; i < plan.plan_components_.size(); ++i)
00177       {
00178         std::set<collision_detection::CostSource> cost_sources_i;
00179         plan.planning_scene_->getCostSources(*plan.plan_components_[i].trajectory_, max_cost_sources_,
00180                                              plan.plan_components_[i].trajectory_->getGroupName(), cost_sources_i,
00181                                              discard_overlapping_cost_sources_);
00182         cost_sources.insert(cost_sources_i.begin(), cost_sources_i.end());
00183         if (cost_sources.size() > max_cost_sources_)
00184         {
00185           std::set<collision_detection::CostSource> other;
00186           other.swap(cost_sources);
00187           std::size_t j = 0;
00188           for (std::set<collision_detection::CostSource>::iterator it = other.begin(); j < max_cost_sources_; ++it, ++j)
00189             cost_sources.insert(*it);
00190         }
00191       }
00192     }
00193 
00194     // display the costs if needed
00195     if (display_cost_sources_)
00196     {
00197       visualization_msgs::MarkerArray arr;
00198       collision_detection::getCostMarkers(arr, plan.planning_scene_->getPlanningFrame(), cost_sources);
00199       cost_sources_publisher_.publish(arr);
00200     }
00201 
00202     double cost = collision_detection::getTotalCost(cost_sources);
00203     ROS_DEBUG("The total cost of the trajectory is %lf.", cost);
00204     if (previous_cost > 0.0)
00205       ROS_DEBUG("The change in the trajectory cost is %lf after the perception step.", cost - previous_cost);
00206     if (cost > max_safe_path_cost && look_attempts < max_look_attempts)
00207     {
00208       ++look_attempts;
00209       ROS_INFO("The cost of the trajectory is %lf, which is above the maximum safe cost of %lf. Attempt %u (of at most "
00210                "%u) at looking around.",
00211                cost, max_safe_path_cost, look_attempts, max_look_attempts);
00212 
00213       bool looked_at_result = lookAt(cost_sources, plan.planning_scene_->getPlanningFrame());
00214       if (looked_at_result)
00215         ROS_INFO("Sensor was succesfully actuated. Attempting to recompute a motion plan.");
00216       else
00217       {
00218         if (look_around_failed)
00219           ROS_WARN("Looking around seems to keep failing. Giving up.");
00220         else
00221           ROS_WARN("Looking around seems to have failed. Attempting to recompute a motion plan anyway.");
00222       }
00223       if (looked_at_result || !look_around_failed)
00224       {
00225         previous_cost = cost;
00226         just_looked_around = true;
00227       }
00228       look_around_failed = !looked_at_result;
00229       // if we are unable to look, let this loop continue into the next if statement
00230       if (just_looked_around)
00231         continue;
00232     }
00233 
00234     if (cost > max_safe_path_cost)
00235     {
00236       plan.error_code_.val = moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA;
00237       return true;
00238     }
00239     else
00240       return true;
00241   } while (true);
00242 
00243   return false;
00244 }
00245 
00246 bool plan_execution::PlanWithSensing::lookAt(const std::set<collision_detection::CostSource>& cost_sources,
00247                                              const std::string& frame_id)
00248 {
00249   if (!sensor_manager_)
00250   {
00251     ROS_WARN("It seems looking around would be useful, but no MoveIt Sensor Manager is loaded. Did you set "
00252              "~moveit_sensor_manager ?");
00253     return false;
00254   }
00255 
00256   if (before_look_callback_)
00257     before_look_callback_();
00258 
00259   std::vector<std::string> names;
00260   sensor_manager_->getSensorsList(names);
00261   geometry_msgs::PointStamped point;
00262   for (std::size_t i = 0; i < names.size(); ++i)
00263     if (collision_detection::getSensorPositioning(point.point, cost_sources))
00264     {
00265       point.header.stamp = ros::Time::now();
00266       point.header.frame_id = frame_id;
00267       ROS_DEBUG_STREAM("Pointing sensor " << names[i] << " to:\n" << point);
00268       moveit_msgs::RobotTrajectory sensor_trajectory;
00269       if (sensor_manager_->pointSensorTo(names[i], point, sensor_trajectory))
00270       {
00271         if (!trajectory_processing::isTrajectoryEmpty(sensor_trajectory))
00272           return trajectory_execution_manager_->push(sensor_trajectory) &&
00273                  trajectory_execution_manager_->executeAndWait();
00274         else
00275           return true;
00276       }
00277     }
00278   return false;
00279 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Jul 24 2017 02:21:19