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 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
00084 display_cost_sources_ = false;
00085
00086
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
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
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
00153
00154 bool just_looked_around = false;
00155
00156
00157
00158
00159 bool look_around_failed = false;
00160
00161
00162
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
00170 std::set<collision_detection::CostSource> cost_sources;
00171 {
00172 planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_);
00173
00174
00175
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
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
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 }