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


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