Go to the documentation of this file.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/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
00086 display_cost_sources_ = false;
00087
00088
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
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
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
00152
00153 bool just_looked_around = false;
00154
00155
00156
00157
00158 bool look_around_failed = false;
00159
00160
00161
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
00169 std::set<collision_detection::CostSource> cost_sources;
00170 {
00171 planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_);
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
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
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 }