plan_with_sensing.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
40 #include <boost/algorithm/string/join.hpp>
41 
42 #include <dynamic_reconfigure/server.h>
43 #include <moveit_ros_planning/SenseForPlanDynamicReconfigureConfig.h>
44 
45 namespace plan_execution
46 {
47 using namespace moveit_ros_planning;
48 
50 {
51 public:
53  : owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle("~/sense_for_plan"))
54  {
55  dynamic_reconfigure_server_.setCallback(
56  boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
57  }
58 
59 private:
60  void dynamicReconfigureCallback(SenseForPlanDynamicReconfigureConfig& config, uint32_t level)
61  {
62  owner_->setMaxSafePathCost(config.max_safe_path_cost);
63  owner_->setMaxCostSources(config.max_cost_sources);
64  owner_->setMaxLookAttempts(config.max_look_attempts);
65  owner_->setDiscardOverlappingCostSources(config.discard_overlapping_cost_sources);
66  }
67 
69  dynamic_reconfigure::Server<SenseForPlanDynamicReconfigureConfig> dynamic_reconfigure_server_;
70 };
71 }
72 
74  const trajectory_execution_manager::TrajectoryExecutionManagerPtr& trajectory_execution)
75  : node_handle_("~"), trajectory_execution_manager_(trajectory_execution)
76 {
79 
81  max_cost_sources_ = 100;
82 
83  // by default we do not display path cost sources
84  display_cost_sources_ = false;
85 
86  // load the sensor manager plugin, if needed
87  if (node_handle_.hasParam("moveit_sensor_manager"))
88  {
89  try
90  {
92  "moveit_core", "moveit_sensor_manager::MoveItSensorManager"));
93  }
95  {
96  ROS_ERROR_STREAM("Exception while creating sensor manager plugin loader: " << ex.what());
97  }
99  {
100  std::string manager;
101  if (node_handle_.getParam("moveit_sensor_manager", manager))
102  try
103  {
104  sensor_manager_ = sensor_manager_loader_->createUniqueInstance(manager);
105  }
107  {
108  ROS_ERROR_STREAM("Exception while loading sensor manager '" << manager << "': " << ex.what());
109  }
110  }
111  if (sensor_manager_)
112  {
113  std::vector<std::string> sensors;
114  sensor_manager_->getSensorsList(sensors);
115  ROS_INFO_STREAM("PlanWithSensing is aware of the following sensors: " << boost::algorithm::join(sensors, ", "));
116  }
117  }
118 
119  // start the dynamic-reconfigure server
121 }
122 
124 {
125  delete reconfigure_impl_;
126 }
127 
129 {
130  if (flag && !display_cost_sources_)
131  // publisher for cost sources
133  node_handle_.advertise<visualization_msgs::MarkerArray>("display_cost_sources", 100, true);
134  else if (!flag && display_cost_sources_)
136  display_cost_sources_ = flag;
137 }
138 
140  const ExecutableMotionPlanComputationFn& motion_planner,
141  unsigned int max_look_attempts, double max_safe_path_cost)
142 {
143  if (max_safe_path_cost <= std::numeric_limits<double>::epsilon())
144  max_safe_path_cost = default_max_safe_path_cost_;
145 
146  if (max_look_attempts == 0)
147  max_look_attempts = default_max_look_attempts_;
148 
149  double previous_cost = 0.0;
150  unsigned int look_attempts = 0;
151 
152  // this flag is set to true when all conditions for looking around are met, and the command is sent.
153  // the intention is for the planning looop not to terminate when having just looked around
154  bool just_looked_around = false;
155 
156  // this flag indicates whether the last lookAt() operation failed. If this operation fails once, we assume that
157  // maybe some information was gained anyway (the sensor moved part of the way) and we try to plan one more time.
158  // If we have two sensor pointing failures in a row, we fail
159  bool look_around_failed = false;
160 
161  // there can be a maximum number of looking attempts as well that lead to replanning, if the cost
162  // of the path is above a maximum threshold.
163  do
164  {
165  bool solved = motion_planner(plan);
166  if (!solved || plan.error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
167  return solved;
168 
169  // determine the sources of cost for this path
170  std::set<collision_detection::CostSource> cost_sources;
171  {
173  // planning_scene_monitor_ is
174  // null; there just will be
175  // no locking done
176  for (std::size_t i = 0; i < plan.plan_components_.size(); ++i)
177  {
178  std::set<collision_detection::CostSource> cost_sources_i;
179  plan.planning_scene_->getCostSources(*plan.plan_components_[i].trajectory_, max_cost_sources_,
180  plan.plan_components_[i].trajectory_->getGroupName(), cost_sources_i,
182  cost_sources.insert(cost_sources_i.begin(), cost_sources_i.end());
183  if (cost_sources.size() > max_cost_sources_)
184  {
185  std::set<collision_detection::CostSource> other;
186  other.swap(cost_sources);
187  std::size_t j = 0;
188  for (std::set<collision_detection::CostSource>::iterator it = other.begin(); j < max_cost_sources_; ++it, ++j)
189  cost_sources.insert(*it);
190  }
191  }
192  }
193 
194  // display the costs if needed
196  {
197  visualization_msgs::MarkerArray arr;
198  collision_detection::getCostMarkers(arr, plan.planning_scene_->getPlanningFrame(), cost_sources);
200  }
201 
202  double cost = collision_detection::getTotalCost(cost_sources);
203  ROS_DEBUG("The total cost of the trajectory is %lf.", cost);
204  if (previous_cost > 0.0)
205  ROS_DEBUG("The change in the trajectory cost is %lf after the perception step.", cost - previous_cost);
206  if (cost > max_safe_path_cost && look_attempts < max_look_attempts)
207  {
208  ++look_attempts;
209  ROS_INFO("The cost of the trajectory is %lf, which is above the maximum safe cost of %lf. Attempt %u (of at most "
210  "%u) at looking around.",
211  cost, max_safe_path_cost, look_attempts, max_look_attempts);
212 
213  bool looked_at_result = lookAt(cost_sources, plan.planning_scene_->getPlanningFrame());
214  if (looked_at_result)
215  ROS_INFO("Sensor was succesfully actuated. Attempting to recompute a motion plan.");
216  else
217  {
218  if (look_around_failed)
219  ROS_WARN("Looking around seems to keep failing. Giving up.");
220  else
221  ROS_WARN("Looking around seems to have failed. Attempting to recompute a motion plan anyway.");
222  }
223  if (looked_at_result || !look_around_failed)
224  {
225  previous_cost = cost;
226  just_looked_around = true;
227  }
228  look_around_failed = !looked_at_result;
229  // if we are unable to look, let this loop continue into the next if statement
230  if (just_looked_around)
231  continue;
232  }
233 
234  if (cost > max_safe_path_cost)
235  {
236  plan.error_code_.val = moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA;
237  return true;
238  }
239  else
240  return true;
241  } while (true);
242 
243  return false;
244 }
245 
246 bool plan_execution::PlanWithSensing::lookAt(const std::set<collision_detection::CostSource>& cost_sources,
247  const std::string& frame_id)
248 {
249  if (!sensor_manager_)
250  {
251  ROS_WARN("It seems looking around would be useful, but no MoveIt! Sensor Manager is loaded. Did you set "
252  "~moveit_sensor_manager ?");
253  return false;
254  }
255 
258 
259  std::vector<std::string> names;
260  sensor_manager_->getSensorsList(names);
261  geometry_msgs::PointStamped point;
262  for (std::size_t i = 0; i < names.size(); ++i)
263  if (collision_detection::getSensorPositioning(point.point, cost_sources))
264  {
265  point.header.stamp = ros::Time::now();
266  point.header.frame_id = frame_id;
267  ROS_DEBUG_STREAM("Pointing sensor " << names[i] << " to:\n" << point);
268  moveit_msgs::RobotTrajectory sensor_trajectory;
269  if (sensor_manager_->pointSensorTo(names[i], point, sensor_trajectory))
270  {
271  if (!trajectory_processing::isTrajectoryEmpty(sensor_trajectory))
272  return trajectory_execution_manager_->push(sensor_trajectory) &&
273  trajectory_execution_manager_->executeAndWait();
274  else
275  return true;
276  }
277  }
278  return false;
279 }
void getCostMarkers(visualization_msgs::MarkerArray &arr, const std::string &frame_id, std::set< CostSource > &cost_sources)
bool lookAt(const std::set< collision_detection::CostSource > &cost_sources, const std::string &frame_id)
void publish(const boost::shared_ptr< M > &message) const
double getTotalCost(const std::set< CostSource > &cost_sources)
moveit_msgs::MoveItErrorCodes error_code_
An error code reflecting what went wrong (if anything)
std::unique_ptr< pluginlib::ClassLoader< moveit_sensor_manager::MoveItSensorManager > > sensor_manager_loader_
#define ROS_WARN(...)
boost::function< bool(ExecutableMotionPlan &plan)> ExecutableMotionPlanComputationFn
The signature of a function that can compute a motion plan.
planning_scene::PlanningSceneConstPtr planning_scene_
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
moveit_sensor_manager::MoveItSensorManagerPtr sensor_manager_
bool computePlan(ExecutableMotionPlan &plan, const ExecutableMotionPlanComputationFn &motion_planner, unsigned int max_look_attempts, double max_safe_path_cost)
bool getSensorPositioning(geometry_msgs::Point &point, const std::set< CostSource > &cost_sources)
#define ROS_INFO(...)
boost::function< void()> before_look_callback_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
#define ROS_DEBUG_STREAM(args)
bool isTrajectoryEmpty(const moveit_msgs::RobotTrajectory &trajectory)
void dynamicReconfigureCallback(SenseForPlanDynamicReconfigureConfig &config, uint32_t level)
#define ROS_INFO_STREAM(args)
This namespace includes functionality specific to the execution and monitoring of motion plans...
bool getParam(const std::string &key, std::string &s) const
static Time now()
A generic representation on what a computed motion plan looks like.
PlanWithSensing(const trajectory_execution_manager::TrajectoryExecutionManagerPtr &trajectory_execution)
bool hasParam(const std::string &key) const
#define ROS_ERROR_STREAM(args)
std::vector< ExecutableTrajectory > plan_components_
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
DynamicReconfigureImpl * reconfigure_impl_
dynamic_reconfigure::Server< SenseForPlanDynamicReconfigureConfig > dynamic_reconfigure_server_
trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_
#define ROS_DEBUG(...)


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jul 10 2019 04:03:32