benchmark_execution.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, Mario Prats, Dave Coleman */
36 
37 #include <moveit/benchmarks/benchmark_execution.h>
38 #include <moveit/benchmarks/benchmarks_utils.h>
42 
43 #include <boost/regex.hpp>
44 #include <boost/tokenizer.hpp>
45 #include <boost/lexical_cast.hpp>
46 #include <boost/program_options/parsers.hpp>
47 #include <boost/program_options/variables_map.hpp>
48 #include <boost/algorithm/string/case_conv.hpp>
49 #include <boost/algorithm/string.hpp>
50 #include <boost/math/constants/constants.hpp>
51 #include <boost/progress.hpp>
52 #include <boost/date_time/posix_time/posix_time.hpp>
53 
55 #include <Eigen/Core>
56 #include <Eigen/Geometry>
57 
58 #include <fstream>
59 #include <memory>
60 
62 {
63 namespace
64 {
65 // update the constrained link for Position and Orientation constraints, if that link is empty
66 void checkConstrainedLink(moveit_msgs::Constraints& c, const std::string& link_name)
67 {
68  for (std::size_t i = 0; i < c.position_constraints.size(); ++i)
69  if (c.position_constraints[i].link_name.empty())
70  c.position_constraints[i].link_name = link_name;
71  for (std::size_t i = 0; i < c.orientation_constraints.size(); ++i)
72  if (c.orientation_constraints[i].link_name.empty())
73  c.orientation_constraints[i].link_name = link_name;
74 }
75 
76 void checkHeader(moveit_msgs::Constraints& c, const std::string& header_frame)
77 {
78  for (std::size_t i = 0; i < c.position_constraints.size(); ++i)
79  if (c.position_constraints[i].header.frame_id.empty())
80  {
81  c.position_constraints[i].header.frame_id = header_frame;
82  c.position_constraints[i].header.stamp = ros::Time::now();
83  }
84  for (std::size_t i = 0; i < c.orientation_constraints.size(); ++i)
85  if (c.orientation_constraints[i].header.frame_id.empty())
86  {
87  c.orientation_constraints[i].header.frame_id = header_frame;
88  c.orientation_constraints[i].header.stamp = ros::Time::now();
89  }
90 }
91 }
92 }
93 
94 moveit_benchmarks::BenchmarkExecution::BenchmarkExecution(const planning_scene::PlanningScenePtr& scene,
96  : planning_scene_(scene), pss_(conn), psws_(conn), cs_(conn), tcs_(conn), rs_(conn)
97 {
98  // load the pluginlib class loader
99  try
100  {
101  planner_plugin_loader_.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
102  "moveit_core", "planning_interface::PlannerManager"));
103  }
105  {
106  ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
107  }
108 
109  // load the planning plugins
110  const std::vector<std::string>& classes = planner_plugin_loader_->getDeclaredClasses();
111  for (std::size_t i = 0; i < classes.size(); ++i)
112  {
113  ROS_INFO("Attempting to load and configure %s", classes[i].c_str());
114  try
115  {
116  planning_interface::PlannerManagerPtr p = planner_plugin_loader_->createUniqueInstance(classes[i]);
117  p->initialize(planning_scene_->getRobotModel(), "");
118  planner_interfaces_[classes[i]] = p;
119  }
121  {
122  ROS_ERROR_STREAM("Exception while loading planner '" << classes[i] << "': " << ex.what());
123  }
124  }
125 
126  // error check
127  if (planner_interfaces_.empty())
128  ROS_ERROR("No planning plugins have been loaded. Nothing to do for the benchmarking service.");
129  else
130  {
131  std::stringstream ss;
132  for (std::map<std::string, planning_interface::PlannerManagerPtr>::const_iterator it = planner_interfaces_.begin();
133  it != planner_interfaces_.end(); ++it)
134  ss << it->first << " ";
135  ROS_INFO("Available planner instances: %s", ss.str().c_str());
136  }
137 }
138 
139 void moveit_benchmarks::BenchmarkExecution::runAllBenchmarks(BenchmarkType type)
140 {
143  bool world_only = false;
144 
145  // read the environment geometry
146  if (!pss_.hasPlanningScene(options_.scene))
147  {
148  if (psws_.hasPlanningSceneWorld(options_.scene))
149  {
150  bool ok = false;
151  try
152  {
153  ok = psws_.getPlanningSceneWorld(pswwm, options_.scene);
154  }
155  catch (std::exception& ex)
156  {
157  ROS_ERROR("%s", ex.what());
158  }
159  if (!ok)
160  return;
161  world_only = true;
162  }
163  else
164  {
165  std::stringstream ss;
166  std::vector<std::string> names;
167  pss_.getPlanningSceneNames(names);
168  for (std::size_t i = 0; i < names.size(); ++i)
169  ss << names[i] << " ";
170  ROS_ERROR("Scene '%s' not found in warehouse. Available names: %s", options_.scene.c_str(), ss.str().c_str());
171  return;
172  }
173  }
174  else
175  {
176  bool ok = false;
177  try
178  {
179  ok = pss_.getPlanningScene(pswm, options_.scene);
180  }
181  catch (std::exception& ex)
182  {
183  ROS_ERROR("%s", ex.what());
184  }
185 
186  if (!ok)
187  {
188  ROS_ERROR("Scene '%s' not found in warehouse", options_.scene.c_str());
189  return;
190  }
191  }
192 
193  // fill in the environment geometry in the benchmark request
194  BenchmarkRequest req;
195  req.benchmark_type = type;
196 
197  if (world_only)
198  {
199  req.scene.world = static_cast<const moveit_msgs::PlanningSceneWorld&>(*pswwm);
200  req.scene.robot_model_name =
201  "NO ROBOT INFORMATION. ONLY WORLD GEOMETRY"; // so that run_benchmark sees a different robot name
202  }
203  else
204  req.scene = static_cast<const moveit_msgs::PlanningScene&>(*pswm);
205 
206  // check if this scene has associated queries
207  req.scene.name = options_.scene;
208  std::vector<std::string> planning_queries_names;
209  try
210  {
211  pss_.getPlanningQueriesNames(options_.query_regex, planning_queries_names, options_.scene);
212  }
213  catch (std::exception& ex)
214  {
215  ROS_ERROR("%s", ex.what());
216  }
217  if (planning_queries_names.empty())
218  ROS_DEBUG("Scene '%s' has no associated queries", options_.scene.c_str());
219 
220  // fill in the plugins option
221  req.plugins = options_.plugins;
222 
223  // see if we have any start states specified; if we do, we run the benchmark for
224  // each start state; if not, we run it for one start state only: the current one saved in the loaded scene
225  std::vector<std::string> start_states;
226  if (!options_.start_regex.empty())
227  {
228  boost::regex start_regex(options_.start_regex);
229  std::vector<std::string> state_names;
230  rs_.getKnownRobotStates(state_names);
231  for (std::size_t i = 0; i < state_names.size(); ++i)
232  {
233  boost::cmatch match;
234  if (boost::regex_match(state_names[i].c_str(), match, start_regex))
235  start_states.push_back(state_names[i]);
236  }
237  if (start_states.empty())
238  {
239  ROS_WARN("No stored states matched the provided regex: '%s'", options_.start_regex.c_str());
240  return;
241  }
242  else
243  ROS_INFO("Running benchmark using %u start states.", (unsigned int)start_states.size());
244  }
245  else
246  ROS_INFO("No specified start state. Running benchmark once with the default start state.");
247 
248  unsigned int n_call = 0;
249  bool have_more_start_states = true;
250  std::unique_ptr<moveit_msgs::RobotState> start_state_to_use;
251  while (have_more_start_states)
252  {
253  start_state_to_use.reset();
254 
255  // if no set of start states provided, run once for the current one
256  if (options_.start_regex.empty())
257  have_more_start_states = false;
258  else
259  {
260  // otherwise, extract the start states one by one
261  std::string state_name = start_states.back();
262  start_states.pop_back();
263  have_more_start_states = !start_states.empty();
265  bool got_robot_state = false;
266  try
267  {
268  got_robot_state = rs_.getRobotState(robot_state, state_name);
269  }
270  catch (std::exception& ex)
271  {
272  ROS_ERROR("%s", ex.what());
273  }
274 
275  if (got_robot_state)
276  {
277  start_state_to_use.reset(new moveit_msgs::RobotState(*robot_state));
278  ROS_INFO("Loaded start state '%s'", state_name.c_str());
279  }
280  else
281  continue;
282  }
283 
284  // run benchmarks for specified queries
285  // ************************************
286 
287  if (!options_.query_regex.empty())
288  {
289  for (std::size_t i = 0; i < planning_queries_names.size(); ++i)
290  {
292  pss_.getPlanningQuery(planning_query, options_.scene, planning_queries_names[i]);
293 
294  // Save name of goal - only used for later analysis
295  req.goal_name = planning_queries_names[i];
296 
297  // read request from db
298  req.motion_plan_request = static_cast<const moveit_msgs::MotionPlanRequest&>(*planning_query);
299 
300  // set the workspace bounds
301  req.motion_plan_request.workspace_parameters = options_.workspace_parameters;
302 
303  // update request given .cfg options
304  if (start_state_to_use)
305  req.motion_plan_request.start_state = *start_state_to_use;
306  req.filename = options_.output + "." + boost::lexical_cast<std::string>(++n_call) + ".log";
307  if (!options_.group_override.empty())
308  req.motion_plan_request.group_name = options_.group_override;
309 
310  if (options_.timeout > 0.0)
311  req.motion_plan_request.allowed_planning_time = options_.timeout;
312 
313  if (!options_.default_constrained_link.empty())
314  {
315  checkConstrainedLink(req.motion_plan_request.path_constraints, options_.default_constrained_link);
316  for (std::size_t j = 0; j < req.motion_plan_request.goal_constraints.size(); ++j)
317  checkConstrainedLink(req.motion_plan_request.goal_constraints[j], options_.default_constrained_link);
318  }
319  if (!options_.planning_frame.empty())
320  {
321  checkHeader(req.motion_plan_request.path_constraints, options_.planning_frame);
322  for (std::size_t j = 0; j < req.motion_plan_request.goal_constraints.size(); ++j)
323  checkHeader(req.motion_plan_request.goal_constraints[j], options_.planning_frame);
324  }
325 
326  ROS_INFO("Benckmarking query '%s' (%d of %d)", planning_queries_names[i].c_str(), (int)i + 1,
327  (int)planning_queries_names.size());
328  runBenchmark(req);
329  }
330  }
331 
332  // if we have any goals specified as constraints, run benchmarks for those as well
333  // *******************************************************************************
334 
335  if (!options_.goal_regex.empty())
336  {
337  std::vector<std::string> cnames;
338  cs_.getKnownConstraints(options_.goal_regex, cnames);
339  for (std::size_t i = 0; i < cnames.size(); ++i)
340  {
342  bool got_constraints = false;
343  try
344  {
345  got_constraints = cs_.getConstraints(constr, cnames[i]);
346  }
347  catch (std::exception& ex)
348  {
349  ROS_ERROR("%s", ex.what());
350  }
351 
352  if (got_constraints)
353  {
354  // Save name of goal - only used for later analysis
355  req.goal_name = cnames[i];
356 
357  // construct a planning request from the constraints message
358  req.motion_plan_request = moveit_msgs::MotionPlanRequest();
359  req.motion_plan_request.goal_constraints.resize(1);
360  if (start_state_to_use)
361  req.motion_plan_request.start_state = *start_state_to_use;
362  req.motion_plan_request.goal_constraints[0] = *constr;
363 
364  // set the workspace bounds
365  req.motion_plan_request.workspace_parameters = options_.workspace_parameters;
366 
367  // Apply the goal offset for constraints that appear to specify IK poses
368  if (constr->position_constraints.size() == 1 && constr->orientation_constraints.size() == 1 &&
370  constr->position_constraints[0].constraint_region.primitive_poses.size() == 1 &&
371  constr->position_constraints[0].constraint_region.mesh_poses.empty())
372  {
373  geometry_msgs::Pose wMc_msg;
374  wMc_msg.position = constr->position_constraints[0].constraint_region.primitive_poses[0].position;
375  wMc_msg.orientation = constr->orientation_constraints[0].orientation;
376  Eigen::Affine3d wMc;
377  tf::poseMsgToEigen(wMc_msg, wMc);
378 
379  Eigen::Affine3d offset_tf(Eigen::AngleAxis<double>(options_.offsets[3], Eigen::Vector3d::UnitX()) *
380  Eigen::AngleAxis<double>(options_.offsets[4], Eigen::Vector3d::UnitY()) *
381  Eigen::AngleAxis<double>(options_.offsets[5], Eigen::Vector3d::UnitZ()));
382  offset_tf.translation() = Eigen::Vector3d(options_.offsets[0], options_.offsets[1], options_.offsets[2]);
383 
384  Eigen::Affine3d wMnc = wMc * offset_tf;
385  geometry_msgs::Pose wMnc_msg;
386  tf::poseEigenToMsg(wMnc, wMnc_msg);
387 
388  req.motion_plan_request.goal_constraints[0]
389  .position_constraints[0]
390  .constraint_region.primitive_poses[0]
391  .position = wMnc_msg.position;
392  req.motion_plan_request.goal_constraints[0].orientation_constraints[0].orientation = wMnc_msg.orientation;
393  }
394 
395  if (!options_.group_override.empty())
396  req.motion_plan_request.group_name = options_.group_override;
397  if (options_.timeout > 0.0)
398  req.motion_plan_request.allowed_planning_time = options_.timeout;
399  if (!options_.default_constrained_link.empty())
400  checkConstrainedLink(req.motion_plan_request.goal_constraints[0], options_.default_constrained_link);
401  if (!options_.planning_frame.empty())
402  checkHeader(req.motion_plan_request.goal_constraints[0], options_.planning_frame);
403  req.filename = options_.output + "." + boost::lexical_cast<std::string>(++n_call) + ".log";
404 
405  ROS_INFO("Benckmarking goal '%s' (%d of %d)", cnames[i].c_str(), (int)i + 1, (int)cnames.size());
406  runBenchmark(req);
407  }
408  }
409  }
410 
411  // if we have any goals specified as trajectory constraints, run benchmarks for those as well
412  // ******************************************************************************************
413 
414  if (!options_.trajectory_regex.empty())
415  {
416  std::vector<std::string> cnames;
417  tcs_.getKnownTrajectoryConstraints(options_.trajectory_regex, cnames);
418  for (std::size_t i = 0; i < cnames.size(); ++i)
419  {
421  bool got_constraints = false;
422  try
423  {
424  got_constraints = tcs_.getTrajectoryConstraints(constr, cnames[i]);
425  }
426  catch (std::exception& ex)
427  {
428  ROS_ERROR("%s", ex.what());
429  }
430 
431  // Save name of goal - only used for later analysis
432  req.goal_name = cnames[i];
433 
434  if (got_constraints)
435  {
436  // construct a planning request from the trajectory constraints message
437  req.motion_plan_request = moveit_msgs::MotionPlanRequest();
438  if (start_state_to_use)
439  req.motion_plan_request.start_state = *start_state_to_use;
440  req.motion_plan_request.trajectory_constraints = *constr;
441 
442  // set the workspace bounds
443  req.motion_plan_request.workspace_parameters = options_.workspace_parameters;
444 
445  Eigen::Affine3d offset_tf(Eigen::AngleAxis<double>(options_.offsets[3], Eigen::Vector3d::UnitX()) *
446  Eigen::AngleAxis<double>(options_.offsets[4], Eigen::Vector3d::UnitY()) *
447  Eigen::AngleAxis<double>(options_.offsets[5], Eigen::Vector3d::UnitZ()));
448  offset_tf.translation() = Eigen::Vector3d(options_.offsets[0], options_.offsets[1], options_.offsets[2]);
449 
450  // Apply waypoint offsets, check fields
451  for (std::size_t tc = 0; tc < constr->constraints.size(); ++tc)
452  {
453  // Apply the goal offset for constraints that appear to specify IK poses
454  if (constr->constraints[tc].position_constraints.size() == 1 &&
455  constr->constraints[tc].orientation_constraints.size() == 1 &&
456  kinematic_constraints::countIndividualConstraints(constr->constraints[tc]) == 2 &&
457  constr->constraints[tc].position_constraints[0].constraint_region.primitive_poses.size() == 1 &&
458  constr->constraints[tc].position_constraints[0].constraint_region.mesh_poses.empty())
459  {
460  geometry_msgs::Pose wMc_msg;
461  wMc_msg.position = req.motion_plan_request.trajectory_constraints.constraints[tc]
462  .position_constraints[0]
463  .constraint_region.primitive_poses[0]
464  .position;
465  wMc_msg.orientation =
466  req.motion_plan_request.trajectory_constraints.constraints[tc].orientation_constraints[0].orientation;
467  Eigen::Affine3d wMc;
468  tf::poseMsgToEigen(wMc_msg, wMc);
469 
470  Eigen::Affine3d wMnc = wMc * offset_tf;
471  geometry_msgs::Pose wMnc_msg;
472  tf::poseEigenToMsg(wMnc, wMnc_msg);
473 
474  req.motion_plan_request.trajectory_constraints.constraints[tc]
475  .position_constraints[0]
476  .constraint_region.primitive_poses[0]
477  .position = wMnc_msg.position;
478  req.motion_plan_request.trajectory_constraints.constraints[tc].orientation_constraints[0].orientation =
479  wMnc_msg.orientation;
480  }
481  if (!options_.default_constrained_link.empty())
482  checkConstrainedLink(req.motion_plan_request.trajectory_constraints.constraints[tc],
483  options_.default_constrained_link);
484  if (!options_.planning_frame.empty())
485  checkHeader(req.motion_plan_request.trajectory_constraints.constraints[tc], options_.planning_frame);
486  }
487 
488  if (!options_.group_override.empty())
489  req.motion_plan_request.group_name = options_.group_override;
490  if (options_.timeout > 0.0)
491  req.motion_plan_request.allowed_planning_time = options_.timeout;
492  req.filename = options_.output + ".trajectory." + boost::lexical_cast<std::string>(i + 1) + ".log";
493 
494  ROS_INFO("Benckmarking trajectory '%s' (%d of %d)", cnames[i].c_str(), (int)i + 1, (int)cnames.size());
495  runBenchmark(req);
496  }
497  }
498  }
499  }
500 }
501 
502 bool moveit_benchmarks::BenchmarkExecution::readOptions(const std::string& filename)
503 {
504  ROS_INFO("Loading '%s'...", filename.c_str());
505 
506  std::ifstream cfg(filename.c_str());
507  if (!cfg.good())
508  {
509  ROS_ERROR_STREAM("Unable to open file '" << filename << "'");
510  return false;
511  }
512 
513  // "scene" options
514  try
515  {
516  boost::program_options::options_description desc;
517  // clang-format off
518  desc.add_options()
519  ("scene.name", boost::program_options::value<std::string>(), "Scene name")
520  ("scene.runs", boost::program_options::value<std::string>()->default_value("1"), "Number of runs")
521  ("scene.timeout", boost::program_options::value<std::string>()->default_value(""), "Timeout for planning (s)")
522  ("scene.start", boost::program_options::value<std::string>()->default_value(""), "Regex for the start states to use")
523  ("scene.query", boost::program_options::value<std::string>()->default_value(".*"), "Regex for the queries to execute")
524  ("scene.goal", boost::program_options::value<std::string>()->default_value(""), "Regex for the names of constraints to use as goals")
525  ("scene.trajectory", boost::program_options::value<std::string>()->default_value(""), "Regex for the names of constraints to use as trajectories")
526  ("scene.group", boost::program_options::value<std::string>()->default_value(""), "Override the group to plan for")
527  ("scene.planning_frame", boost::program_options::value<std::string>()->default_value(""), "Override the planning frame to use")
528  ("scene.default_constrained_link", boost::program_options::value<std::string>()->default_value(""),
529  "Specify the default link to consider as constrained when one is not specified in a moveit_msgs::Constraints message")
530  ("scene.goal_offset_x", boost::program_options::value<std::string>()->default_value("0.0"), "Goal offset in x")
531  ("scene.goal_offset_y", boost::program_options::value<std::string>()->default_value("0.0"), "Goal offset in y")
532  ("scene.goal_offset_z", boost::program_options::value<std::string>()->default_value("0.0"), "Goal offset in z")
533  ("scene.goal_offset_roll", boost::program_options::value<std::string>()->default_value("0.0"), "Goal offset in roll")
534  ("scene.goal_offset_pitch", boost::program_options::value<std::string>()->default_value("0.0"), "Goal offset in pitch")
535  ("scene.goal_offset_yaw", boost::program_options::value<std::string>()->default_value("0.0"), "Goal offset in yaw")
536  ("scene.output", boost::program_options::value<std::string>(), "Location of benchmark log file")
537  ("scene.workspace", boost::program_options::value<std::string>(), "Bounding box of workspace to plan in - min_x, min_y, min_z, max_x, max_y, max_z")
538  ("scene.workspace_frame", boost::program_options::value<std::string>(), "Frame id of bounding box of workspace to plan in");
539  // clang-format on
540 
541  boost::program_options::variables_map vm;
542  boost::program_options::parsed_options po = boost::program_options::parse_config_file(cfg, desc, true);
543 
544  cfg.close();
545  boost::program_options::store(po, vm);
546 
547  std::map<std::string, std::string> declared_options;
548  for (boost::program_options::variables_map::iterator it = vm.begin(); it != vm.end(); ++it)
549  declared_options[it->first] = boost::any_cast<std::string>(vm[it->first].value());
550 
551  options_.scene = declared_options["scene.name"];
552  options_.start_regex = declared_options["scene.start"];
553  options_.query_regex = declared_options["scene.query"];
554  options_.goal_regex = declared_options["scene.goal"];
555  options_.trajectory_regex = declared_options["scene.trajectory"];
556  options_.group_override = declared_options["scene.group"];
557  options_.default_constrained_link = declared_options["scene.default_constrained_link"];
558  options_.planning_frame = declared_options["scene.planning_frame"];
559  try
560  {
561  memset(options_.offsets, 0, 6 * sizeof(double));
562  if (!declared_options["scene.goal_offset_x"].empty())
563  options_.offsets[0] = boost::lexical_cast<double>(declared_options["scene.goal_offset_x"]);
564  if (!declared_options["scene.goal_offset_y"].empty())
565  options_.offsets[1] = boost::lexical_cast<double>(declared_options["scene.goal_offset_y"]);
566  if (!declared_options["scene.goal_offset_z"].empty())
567  options_.offsets[2] = boost::lexical_cast<double>(declared_options["scene.goal_offset_z"]);
568  if (!declared_options["scene.goal_offset_roll"].empty())
569  options_.offsets[3] = boost::lexical_cast<double>(declared_options["scene.goal_offset_roll"]);
570  if (!declared_options["scene.goal_offset_pitch"].empty())
571  options_.offsets[4] = boost::lexical_cast<double>(declared_options["scene.goal_offset_pitch"]);
572  if (!declared_options["scene.goal_offset_yaw"].empty())
573  options_.offsets[5] = boost::lexical_cast<double>(declared_options["scene.goal_offset_yaw"]);
574  }
575  catch (boost::bad_lexical_cast& ex)
576  {
577  ROS_WARN("%s", ex.what());
578  }
579 
580  // Workspace bounds
581  if (!declared_options["scene.workspace"].empty())
582  {
583  std::vector<std::string> strings;
584  boost::split(strings, declared_options["scene.workspace"], boost::is_any_of(","));
585 
586  if (strings.size() != 6)
587  {
588  ROS_WARN_STREAM("Invalid number of workspace parameters. Expected 6, received " << strings.size());
589  }
590  else if (declared_options["scene.workspace_frame"].empty())
591  {
592  ROS_WARN_STREAM("No workspace_frame parameter provided, required with the workspace frame");
593  }
594  else
595  {
596  try
597  {
598  // add workspace bounds if specified in the .cfg file
599  options_.workspace_parameters.header.frame_id = declared_options["scene.workspace_frame"];
600  options_.workspace_parameters.header.stamp = ros::Time::now();
601  options_.workspace_parameters.min_corner.x = boost::lexical_cast<double>(strings[0]);
602  options_.workspace_parameters.min_corner.y = boost::lexical_cast<double>(strings[1]);
603  options_.workspace_parameters.min_corner.z = boost::lexical_cast<double>(strings[2]);
604  options_.workspace_parameters.max_corner.x = boost::lexical_cast<double>(strings[3]);
605  options_.workspace_parameters.max_corner.y = boost::lexical_cast<double>(strings[4]);
606  options_.workspace_parameters.max_corner.z = boost::lexical_cast<double>(strings[5]);
607  }
608  catch (boost::bad_lexical_cast& ex)
609  {
610  ROS_WARN("%s", ex.what());
611  }
612  }
613  }
614 
615  // Filename
616  options_.output = declared_options["scene.output"];
617  if (options_.output.empty())
618  options_.output = filename;
619 
620  options_.plugins.clear();
621 
622  // Runs
623  std::size_t default_run_count = 1;
624  if (!declared_options["scene.runs"].empty())
625  {
626  try
627  {
628  default_run_count = boost::lexical_cast<std::size_t>(declared_options["scene.runs"]);
629  }
630  catch (boost::bad_lexical_cast& ex)
631  {
632  ROS_WARN("%s", ex.what());
633  }
634  }
635  options_.default_run_count = default_run_count;
636 
637  // Timeout
638  options_.timeout = 0.0;
639  if (!declared_options["scene.timeout"].empty())
640  {
641  try
642  {
643  options_.timeout = boost::lexical_cast<double>(declared_options["scene.timeout"]);
644  }
645  catch (boost::bad_lexical_cast& ex)
646  {
647  ROS_WARN("%s", ex.what());
648  }
649  }
650 
651  // Process non-scene options
652  std::vector<std::string> unr =
653  boost::program_options::collect_unrecognized(po.options, boost::program_options::exclude_positional);
654 
655  std::unique_ptr<PlanningPluginOptions> bpo;
656  for (std::size_t i = 0; i < unr.size() / 2; ++i)
657  {
658  std::string key = boost::to_lower_copy(unr[i * 2]);
659  std::string val = unr[i * 2 + 1];
660 
661  // "plugin" options
662  if (key.substr(0, 7) == "plugin.")
663  {
664  std::string k = key.substr(7);
665  if (k == "name")
666  {
667  if (bpo)
668  options_.plugins.push_back(*bpo);
669  bpo.reset(new PlanningPluginOptions());
670  bpo->name = val;
671  bpo->runs = default_run_count;
672  }
673  else if (k == "runs")
674  {
675  if (bpo)
676  {
677  try
678  {
679  bpo->runs = boost::lexical_cast<std::size_t>(val);
680  }
681  catch (boost::bad_lexical_cast& ex)
682  {
683  ROS_WARN("%s", ex.what());
684  }
685  }
686  else
687  ROS_WARN("Ignoring option '%s' = '%s'. Please include plugin name first.", key.c_str(), val.c_str());
688  }
689  else if (k == "planners")
690  {
691  if (bpo)
692  {
693  boost::char_separator<char> sep(" ");
694  boost::tokenizer<boost::char_separator<char>> tok(val, sep);
695  for (boost::tokenizer<boost::char_separator<char>>::iterator beg = tok.begin(); beg != tok.end(); ++beg)
696  bpo->planners.push_back(*beg);
697  }
698  else
699  ROS_WARN("Ignoring option '%s' = '%s'. Please include plugin name first.", key.c_str(), val.c_str());
700  }
701  }
702  // parameter sweeping option
703  else if (key.substr(0, 6) == "sweep.")
704  {
705  std::string sweep_var = key.substr(6);
706 
707  // Convert the string of a:b:c numbers into parsed doubles
708  std::vector<std::string> strings;
709  boost::split(strings, val, boost::is_any_of(":"));
710 
711  if (strings.size() != 3)
712  {
713  ROS_WARN_STREAM("Invalid sweep parameter for key "
714  << sweep_var << ". Expected 3 values (start, iterator, end) but only received "
715  << strings.size());
716  continue;
717  }
718 
719  ParameterOptions new_sweep;
720  new_sweep.is_sweep = true; // not a fractional factorial analaysis
721  try
722  {
723  new_sweep.start = boost::lexical_cast<double>(strings[0]);
724  new_sweep.step_size = boost::lexical_cast<double>(strings[1]);
725  new_sweep.end = boost::lexical_cast<double>(strings[2]);
726  new_sweep.key = sweep_var;
727  // for logging to file:
728  std::ostringstream ss;
729  ss << "param_" << sweep_var << " REAL";
730  new_sweep.log_key = ss.str();
731  }
732  catch (boost::bad_lexical_cast& ex)
733  {
734  ROS_WARN("%s", ex.what());
735  }
736 
737  // Error check
738  if (new_sweep.start > new_sweep.end)
739  {
740  ROS_ERROR_STREAM("Invalid sweep parameter for key " << sweep_var << ". Start is greater than end");
741  continue;
742  }
743 
744  // Insert into array of all sweeps
745  param_options_.push_back(new_sweep);
746  }
747  else
748  {
749  ROS_WARN("Unknown option: '%s' = '%s'", key.c_str(), val.c_str());
750  continue;
751  }
752  }
753  if (bpo)
754  options_.plugins.push_back(*bpo);
755  }
756 
757  catch (...)
758  {
759  ROS_ERROR_STREAM("Unable to parse '" << filename << "'");
760  return false;
761  }
762 
763  return true;
764 }
765 
766 void moveit_benchmarks::BenchmarkExecution::printOptions(std::ostream& out)
767 {
768  out << "Benchmark for scene '" << options_.scene << "' to be saved at location '" << options_.output << "'"
769  << std::endl;
770  if (!options_.query_regex.empty())
771  out << "Planning requests associated to the scene that match '" << options_.query_regex << "' will be evaluated"
772  << std::endl;
773  if (!options_.goal_regex.empty())
774  out << "Planning requests constructed from goal constraints that match '" << options_.goal_regex
775  << "' will be evaluated" << std::endl;
776  if (!options_.trajectory_regex.empty())
777  out << "Planning requests constructed from trajectory constraints that match '" << options_.trajectory_regex
778  << "' will be evaluated" << std::endl;
779  out << "Plugins:" << std::endl;
780  for (std::size_t i = 0; i < options_.plugins.size(); ++i)
781  {
782  out << " * name: " << options_.plugins[i].name << " (to be run " << options_.plugins[i].runs
783  << " times for each planner)" << std::endl;
784  out << " * planners:";
785  for (std::size_t j = 0; j < options_.plugins[i].planners.size(); ++j)
786  out << ' ' << options_.plugins[i].planners[j];
787  out << std::endl;
788  }
789 }
790 
791 void moveit_benchmarks::BenchmarkExecution::runBenchmark(BenchmarkRequest& req)
792 {
793  if (req.benchmark_type & BENCHMARK_PLANNERS)
794  runPlanningBenchmark(req);
795  if (req.benchmark_type & BENCHMARK_GOAL_EXISTANCE)
796  runGoalExistenceBenchmark(req);
797 }
798 
799 void moveit_benchmarks::BenchmarkExecution::collectMetrics(RunData& rundata,
801  bool solved, double total_time)
802 {
803  rundata["total_time REAL"] = boost::lexical_cast<std::string>(total_time);
804  rundata["solved BOOLEAN"] = boost::lexical_cast<std::string>(solved);
805  double L = 0.0;
806  double clearance = 0.0;
807  double smoothness = 0.0;
808  bool correct = true;
809  if (solved)
810  {
811  double process_time = total_time;
812  for (std::size_t j = 0; j < mp_res.trajectory_.size(); ++j)
813  {
814  correct = true;
815  L = 0.0;
816  clearance = 0.0;
817  smoothness = 0.0;
818  const robot_trajectory::RobotTrajectory& p = *mp_res.trajectory_[j];
819 
820  // compute path length
821  for (std::size_t k = 1; k < p.getWayPointCount(); ++k)
822  L += p.getWayPoint(k - 1).distance(p.getWayPoint(k));
823 
824  // compute correctness and clearance
826  for (std::size_t k = 0; k < p.getWayPointCount(); ++k)
827  {
829  planning_scene_->checkCollisionUnpadded(req, res, p.getWayPoint(k));
830  if (res.collision)
831  correct = false;
832  if (!p.getWayPoint(k).satisfiesBounds())
833  correct = false;
834  double d = planning_scene_->distanceToCollisionUnpadded(p.getWayPoint(k));
835  if (d > 0.0) // in case of collision, distance is negative
836  clearance += d;
837  }
838  clearance /= (double)p.getWayPointCount();
839 
840  // compute smoothness
841  if (p.getWayPointCount() > 2)
842  {
843  double a = p.getWayPoint(0).distance(p.getWayPoint(1));
844  for (std::size_t k = 2; k < p.getWayPointCount(); ++k)
845  {
846  // view the path as a sequence of segments, and look at the triangles it forms:
847  // s1
848  // /\ s4
849  // a / \ b |
850  // / \ |
851  // /......\_______|
852  // s0 c s2 s3
853  //
854  // use Pythagoras generalized theorem to find the cos of the angle between segments a and b
855  double b = p.getWayPoint(k - 1).distance(p.getWayPoint(k));
856  double cdist = p.getWayPoint(k - 2).distance(p.getWayPoint(k));
857  double acosValue = (a * a + b * b - cdist * cdist) / (2.0 * a * b);
858  if (acosValue > -1.0 && acosValue < 1.0)
859  {
860  // the smoothness is actually the outside angle of the one we compute
861  double angle = (boost::math::constants::pi<double>() - acos(acosValue));
862 
863  // and we normalize by the length of the segments
864  double u = 2.0 * angle;
865  smoothness += u * u;
866  }
867  a = b;
868  }
869  smoothness /= (double)p.getWayPointCount();
870  }
871  rundata["path_" + mp_res.description_[j] + "_correct BOOLEAN"] = boost::lexical_cast<std::string>(correct);
872  rundata["path_" + mp_res.description_[j] + "_length REAL"] = boost::lexical_cast<std::string>(L);
873  rundata["path_" + mp_res.description_[j] + "_clearance REAL"] = boost::lexical_cast<std::string>(clearance);
874  rundata["path_" + mp_res.description_[j] + "_smoothness REAL"] = boost::lexical_cast<std::string>(smoothness);
875  rundata["path_" + mp_res.description_[j] + "_time REAL"] =
876  boost::lexical_cast<std::string>(mp_res.processing_time_[j]);
877  process_time -= mp_res.processing_time_[j];
878  }
879  if (process_time <= 0.0)
880  process_time = 0.0;
881  rundata["process_time REAL"] = boost::lexical_cast<std::string>(process_time);
882  }
883 }
884 
885 namespace
886 {
887 bool isIKSolutionCollisionFree(const planning_scene::PlanningScene* scene, robot_state::RobotState* state,
888  const robot_model::JointModelGroup* group, const double* ik_solution, bool* reachable)
889 {
890  state->setJointGroupPositions(group, ik_solution);
891  state->update();
892  *reachable = true;
893  if (scene->isStateColliding(*state, group->getName(), false))
894  return false;
895  else
896  return true;
897 }
898 }
899 
900 void moveit_benchmarks::BenchmarkExecution::runPlanningBenchmark(BenchmarkRequest& req)
901 {
902  /* Dev Notes:
903  * planner_interface => planning plugin
904  * planner => planning algorithm within a planning plugin
905  * planner_ids => id of a planner
906  */
907 
908  // check that all requested plugins exist
909  if (!req.plugins.empty())
910  for (std::size_t i = 0; i < req.plugins.size(); ++i)
911  if (planner_interfaces_.find(req.plugins[i].name) == planner_interfaces_.end())
912  ROS_ERROR("Planning interface '%s' was not found", req.plugins[i].name.c_str());
913 
914  // pointer list of planning plugins
915  std::vector<planning_interface::PlannerManager*> planner_interfaces_to_benchmark;
916 
917  // each planning plugin has a vector of its sub algorithms (planners) that it can run
918  std::vector<std::vector<std::string>> planner_ids_to_benchmark_per_planner_interface;
919 
920  // number of runs to execute every *algorithm* per *plugin*
921  std::vector<std::size_t> runs_per_planner_interface; // average_count_per_planner_interface
922 
923  planning_interface::MotionPlanRequest motion_plan_req = req.motion_plan_request;
924 
925  // loop through each planning interface
926  for (std::map<std::string, planning_interface::PlannerManagerPtr>::const_iterator it = planner_interfaces_.begin();
927  it != planner_interfaces_.end(); ++it)
928  {
929  // find the plugin that the planning interface belongs to
930  int found = -1;
931  if (!req.plugins.empty())
932  {
933  for (std::size_t i = 0; i < req.plugins.size(); ++i)
934  {
935  if (req.plugins[i].name == it->first) // this benchmark request includes this planning plugin
936  {
937  found = i;
938  break;
939  }
940  }
941  if (found < 0)
942  continue;
943  }
944 
945  // Determine whether this plugin instance is able to represent this planning request
946  if (it->second->canServiceRequest(motion_plan_req))
947  {
948  // copy the pointer of the planner_interface
949  planner_interfaces_to_benchmark.push_back(it->second.get());
950 
951  // add new rows to the "mapper" for the new planner_interface
952  planner_ids_to_benchmark_per_planner_interface.resize(planner_ids_to_benchmark_per_planner_interface.size() + 1);
953  runs_per_planner_interface.resize(runs_per_planner_interface.size() + 1, 1); // TODO: a vector does not need to
954  // be resized, it does so
955  // automatically right??
956 
957  // get a list of all the algorithms that the planner can use
958  std::vector<std::string> known;
959  it->second->getPlanningAlgorithms(known);
960 
961  if (found < 0 || req.plugins[found].planners.empty())
962  {
963  // no algorithms found OR this benchmark request does not use this algorithm
964  planner_ids_to_benchmark_per_planner_interface.back() = known;
965  }
966  else
967  {
968  runs_per_planner_interface.back() =
969  std::max<std::size_t>(1, req.plugins[found].runs); // TODO: change it here too
970 
971  // loop through every planner(algorithm) in this plugin that the benchmark request desires
972  for (std::size_t k = 0; k < req.plugins[found].planners.size(); ++k)
973  {
974  bool planner_found = false;
975  for (std::size_t q = 0; q < known.size(); ++q)
976  {
977  // Check if the requested planner is actually in the plugin
978  if (known[q] == req.plugins[found].planners[k] ||
979  motion_plan_req.group_name + "[" + req.plugins[found].planners[k] + "]" == known[q])
980  {
981  planner_found = true;
982  break;
983  }
984  }
985  if (planner_found)
986  planner_ids_to_benchmark_per_planner_interface.back().push_back(req.plugins[found].planners[k]);
987  else
988  {
989  ROS_ERROR("The planner id '%s' is not known to the planning interface '%s'",
990  req.plugins[found].planners[k].c_str(), it->first.c_str());
991  // To help user debug, list all available planners:
992  ROS_ERROR_STREAM("Known algorithms in " << it->first.c_str() << ":");
993  for (std::size_t i = 0; i < known.size(); ++i)
994  {
995  ROS_ERROR_STREAM(" - " << known[i]);
996  }
997  }
998  }
999  }
1000 
1001  if (planner_ids_to_benchmark_per_planner_interface.back().empty())
1002  ROS_ERROR("Planning interface '%s' has no planners defined", it->first.c_str());
1003  }
1004  else
1005  ROS_WARN_STREAM("Planning interface '" << it->second->getDescription() << "' is not able to solve the specified "
1006  "benchmark problem.");
1007  }
1008 
1009  // error check
1010  if (planner_interfaces_to_benchmark.empty())
1011  {
1012  ROS_ERROR("There are no planning interfaces to benchmark");
1013  return;
1014  }
1015 
1016  // output information about planners to be tested
1017  std::stringstream sst;
1018  for (std::size_t i = 0; i < planner_interfaces_to_benchmark.size(); ++i)
1019  {
1020  if (planner_ids_to_benchmark_per_planner_interface[i].empty())
1021  continue;
1022  sst << " * " << planner_interfaces_to_benchmark[i]->getDescription() << " - Will execute interface "
1023  << runs_per_planner_interface[i] << " times:" << std::endl;
1024  for (std::size_t k = 0; k < planner_ids_to_benchmark_per_planner_interface[i].size(); ++k)
1025  sst << " - " << planner_ids_to_benchmark_per_planner_interface[i][k] << std::endl;
1026  sst << std::endl;
1027  }
1028  ROS_INFO("Benchmarking Planning Interfaces:\n%s", sst.str().c_str());
1029 
1030  // configure planning context
1031  if (req.scene.robot_model_name != planning_scene_->getRobotModel()->getName())
1032  {
1033  // if we have a different robot, use the world geometry only
1034 
1035  // clear all geometry from the scene
1036  planning_scene_->getWorldNonConst()->clearObjects();
1037  planning_scene_->getCurrentStateNonConst().clearAttachedBodies();
1038  planning_scene_->getCurrentStateNonConst().setToDefaultValues();
1039 
1040  planning_scene_->processPlanningSceneWorldMsg(req.scene.world);
1041  }
1042  else
1043  planning_scene_->usePlanningSceneMsg(req.scene);
1044 
1045  // parameter sweeping functionality
1046  std::size_t n_parameter_sets = generateParamCombinations(); // this is for parameter sweeping
1047 
1048  // get stats on how many planners and total runs will be executed
1049  std::size_t total_n_planners = 0;
1050  std::size_t total_n_runs = 0;
1051  for (std::size_t i = 0; i < planner_ids_to_benchmark_per_planner_interface.size(); ++i)
1052  {
1053  total_n_planners += planner_ids_to_benchmark_per_planner_interface[i].size();
1054 
1055  // n = algorithms * runs * parameters
1056  total_n_runs +=
1057  planner_ids_to_benchmark_per_planner_interface[i].size() * runs_per_planner_interface[i] * n_parameter_sets;
1058  }
1059 
1060  // benchmark all the planners
1061  ros::WallTime startTime = ros::WallTime::now();
1062  boost::progress_display progress(total_n_runs, std::cout);
1063  std::vector<AlgorithmRunsData> data; // holds all of the results
1064  std::vector<bool> first_solution_flag(planner_interfaces_to_benchmark.size(), true);
1065 
1066  // loop through the planning plugins
1067  for (std::size_t i = 0; i < planner_interfaces_to_benchmark.size(); ++i)
1068  {
1069  // loop through the algorithms in each plugin
1070  for (std::size_t j = 0; j < planner_ids_to_benchmark_per_planner_interface[i].size(); ++j)
1071  {
1072  motion_plan_req.planner_id = planner_ids_to_benchmark_per_planner_interface[i][j];
1073  AlgorithmRunsData runs(runs_per_planner_interface[i] * n_parameter_sets);
1074 
1075  // param tracking
1076  std::size_t param_combinations_id_ = 0;
1077 
1078  // loop through the desired parameters
1079  for (std::size_t param_count = 0; param_count < n_parameter_sets; ++param_count)
1080  {
1081  // Check if ROS is still alive
1082  if (!ros::ok())
1083  return;
1084 
1085  // Create new instance of the chosen parameters
1086  RunData parameter_data;
1087 
1088  // apply the current parameter, if we are using those
1089  if (n_parameter_sets > 1)
1090  {
1091  modifyPlannerConfiguration(*planner_interfaces_to_benchmark[i], motion_plan_req.planner_id,
1092  param_combinations_id_, parameter_data);
1093  ++param_combinations_id_;
1094  }
1095 
1096  planning_interface::PlanningContextPtr pcontext =
1097  planner_interfaces_to_benchmark[i]->getPlanningContext(planning_scene_, motion_plan_req);
1098 
1099  // loop through the desired number of runs
1100  for (unsigned int run_count = 0; run_count < runs_per_planner_interface[i]; ++run_count)
1101  {
1102  // Combine two for loops into one id
1103  std::size_t run_id = run_count * n_parameter_sets + param_count;
1104 
1105  ++progress; // this outputs asterisks
1106 
1107  // run a single benchmark
1108  ROS_DEBUG("Calling %s:%s", planner_interfaces_to_benchmark[i]->getDescription().c_str(),
1109  motion_plan_req.planner_id.c_str());
1112  bool solved = pcontext->solve(mp_res);
1113  double total_time = (ros::WallTime::now() - start).toSec();
1114 
1115  // collect data
1116  start = ros::WallTime::now();
1117  runs[run_id].insert(parameter_data.begin(), parameter_data.end()); // initalize this run's data with the
1118  // chosen parameters, if we have any
1119 
1120  collectMetrics(runs[run_id], mp_res, solved, total_time);
1121  double metrics_time = (ros::WallTime::now() - start).toSec();
1122  ROS_DEBUG("Spent %lf seconds collecting metrics", metrics_time);
1123 
1124  // record the first solution in the response
1125  if (solved && first_solution_flag[i])
1126  {
1127  first_solution_flag[i] = false;
1128  }
1129  }
1130  }
1131  // this vector of runs represents all the runs*parameters
1132  data.push_back(runs);
1133 
1134  } // end j - planning algoritms
1135  } // end i - planning plugins
1136 
1137  double duration = (ros::WallTime::now() - startTime).toSec();
1138  std::string host = moveit_benchmarks::getHostname();
1139  std::string filename = req.filename.empty() ?
1140  ("moveit_benchmarks_" + host + "_" +
1141  boost::posix_time::to_iso_extended_string(startTime.toBoost()) + ".log") :
1142  req.filename;
1143  std::ofstream out(filename.c_str());
1144  out << "Experiment " << (planning_scene_->getName().empty() ? "NO_NAME" : planning_scene_->getName()) << std::endl;
1145  out << "Running on " << (host.empty() ? "UNKNOWN" : host) << std::endl;
1146  out << "Starting at " << boost::posix_time::to_iso_extended_string(startTime.toBoost()) << std::endl;
1147  out << "Goal name " << (req.goal_name.empty() ? "UNKNOWN" : req.goal_name) << std::endl;
1148  // out << "<<<|" << std::endl << "ROS" << std::endl << req.motion_plan_request << std::endl << "|>>>" << std::endl;
1149  out << req.motion_plan_request.allowed_planning_time << " seconds per run" << std::endl;
1150  out << duration << " seconds spent to collect the data" << std::endl;
1151  out << total_n_planners << " planners" << std::endl;
1152 
1153  // tracks iteration location in data[] vector
1154  std::size_t run_id = 0;
1155 
1156  // loop through the planning *plugins*
1157  for (std::size_t q = 0; q < planner_interfaces_to_benchmark.size(); ++q)
1158  {
1159  // loop through the planning *algorithms* times the # parameter combinations
1160  for (std::size_t p = 0; p < planner_ids_to_benchmark_per_planner_interface[q].size(); ++p)
1161  {
1162  // Output name of planning algorithm
1163  out << planner_interfaces_to_benchmark[q]->getDescription() + "_" +
1164  planner_ids_to_benchmark_per_planner_interface[q][p]
1165  << std::endl;
1166 
1167  // in general, we could have properties specific for a planner;
1168  // right now, we do not include such properties
1169  out << "0 common properties" << std::endl;
1170 
1171  // construct the list of all possible properties for all runs
1172  std::set<std::string> properties_set;
1173  for (std::size_t j = 0; j < std::size_t(data[run_id].size()); ++j)
1174  {
1175  for (RunData::const_iterator mit = data[run_id][j].begin(); mit != data[run_id][j].end(); ++mit)
1176  {
1177  properties_set.insert(mit->first);
1178  }
1179  }
1180 
1181  // copy that set to a vector of properties
1182  std::vector<std::string> properties;
1183  for (std::set<std::string>::iterator it = properties_set.begin(); it != properties_set.end(); ++it)
1184  properties.push_back(*it);
1185  out << properties.size() << " properties for each run" << std::endl;
1186 
1187  // output the vector of properties to the log file
1188  for (unsigned int j = 0; j < properties.size(); ++j)
1189  out << properties[j] << std::endl;
1190  out << data[run_id].size() << " runs" << std::endl;
1191 
1192  // output all the data to the log file
1193  for (std::size_t j = 0; j < data[run_id].size(); ++j)
1194  {
1195  for (unsigned int k = 0; k < properties.size(); ++k)
1196  {
1197  // check if this current row contains each property
1198  RunData::const_iterator it = data[run_id][j].find(properties[k]);
1199  if (it != data[run_id][j].end())
1200  out << it->second;
1201  out << "; ";
1202  }
1203 
1204  // end the line
1205  out << std::endl;
1206  }
1207  out << '.' << std::endl;
1208 
1209  ++run_id;
1210  }
1211  }
1212 
1213  out.close();
1214  ROS_INFO("Results saved to '%s'", filename.c_str());
1215 }
1216 
1217 void moveit_benchmarks::BenchmarkExecution::runGoalExistenceBenchmark(BenchmarkRequest& req)
1218 {
1219  // configure planning context
1220  if (req.scene.robot_model_name != planning_scene_->getRobotModel()->getName())
1221  {
1222  // if we have a different robot, use the world geometry only
1223  // clear all geometry from the scene
1224  planning_scene_->getWorldNonConst()->clearObjects();
1225  planning_scene_->getCurrentStateNonConst().clearAttachedBodies();
1226  planning_scene_->getCurrentStateNonConst().setToDefaultValues();
1227 
1228  planning_scene_->processPlanningSceneWorldMsg(req.scene.world);
1229  planning_scene_->setName(req.scene.name);
1230  }
1231  else
1232  planning_scene_->usePlanningSceneMsg(req.scene);
1233 
1234  // \todo the code below needs to be replaced with using constraint samplers;
1235 
1236  if (req.motion_plan_request.goal_constraints.size() == 0 &&
1237  req.motion_plan_request.goal_constraints[0].position_constraints.size() == 0 &&
1238  req.motion_plan_request.goal_constraints[0].position_constraints[0].constraint_region.primitive_poses.size() ==
1239  0 &&
1240  req.motion_plan_request.goal_constraints[0].orientation_constraints.size() == 0 &&
1241  req.motion_plan_request.trajectory_constraints.constraints.size() == 0)
1242  {
1243  ROS_ERROR("Invalid goal constraints");
1244  return;
1245  }
1246 
1247  bool success = false;
1248  bool reachable = false;
1249  if (req.motion_plan_request.goal_constraints.size() > 0 &&
1250  req.motion_plan_request.goal_constraints[0].position_constraints.size() > 0 &&
1251  req.motion_plan_request.goal_constraints[0].position_constraints[0].constraint_region.primitive_poses.size() >
1252  0 &&
1253  req.motion_plan_request.goal_constraints[0].orientation_constraints.size() > 0)
1254  {
1255  // Compute IK on goal constraints
1256  geometry_msgs::Pose ik_pose;
1257  ik_pose.position.x = req.motion_plan_request.goal_constraints[0]
1258  .position_constraints[0]
1259  .constraint_region.primitive_poses[0]
1260  .position.x;
1261  ik_pose.position.y = req.motion_plan_request.goal_constraints[0]
1262  .position_constraints[0]
1263  .constraint_region.primitive_poses[0]
1264  .position.y;
1265  ik_pose.position.z = req.motion_plan_request.goal_constraints[0]
1266  .position_constraints[0]
1267  .constraint_region.primitive_poses[0]
1268  .position.z;
1269  ik_pose.orientation.x = req.motion_plan_request.goal_constraints[0].orientation_constraints[0].orientation.x;
1270  ik_pose.orientation.y = req.motion_plan_request.goal_constraints[0].orientation_constraints[0].orientation.y;
1271  ik_pose.orientation.z = req.motion_plan_request.goal_constraints[0].orientation_constraints[0].orientation.z;
1272  ik_pose.orientation.w = req.motion_plan_request.goal_constraints[0].orientation_constraints[0].orientation.w;
1273 
1274  robot_state::RobotState robot_state(planning_scene_->getCurrentState());
1275  robot_state::robotStateMsgToRobotState(req.motion_plan_request.start_state, robot_state);
1276 
1277  // Compute IK
1278  ROS_INFO_STREAM("Processing goal " << req.motion_plan_request.goal_constraints[0].name << " ...");
1279  ros::WallTime startTime = ros::WallTime::now();
1280  success = robot_state.setFromIK(
1281  robot_state.getJointModelGroup(req.motion_plan_request.group_name), ik_pose,
1282  req.motion_plan_request.num_planning_attempts, req.motion_plan_request.allowed_planning_time,
1283  boost::bind(&isIKSolutionCollisionFree, planning_scene_.get(), _1, _2, _3, &reachable));
1284  if (success)
1285  {
1286  ROS_INFO(" Success!");
1287  }
1288  else if (reachable)
1289  {
1290  ROS_INFO(" Reachable, but in collision");
1291  }
1292  else
1293  {
1294  ROS_INFO(" Not reachable");
1295  }
1296  // Log
1297  double duration = (ros::WallTime::now() - startTime).toSec();
1298  std::string host = moveit_benchmarks::getHostname();
1299  std::string filename = req.filename.empty() ?
1300  ("moveit_benchmarks_" + host + "_" +
1301  boost::posix_time::to_iso_extended_string(startTime.toBoost()) + ".log") :
1302  req.filename;
1303  std::ofstream out(filename.c_str());
1304  out << "Experiment " << (planning_scene_->getName().empty() ? "NO_NAME" : planning_scene_->getName()) << std::endl;
1305  out << "Running on " << (host.empty() ? "UNKNOWN" : host) << std::endl;
1306  out << "Starting at " << boost::posix_time::to_iso_extended_string(startTime.toBoost()) << std::endl;
1307  out << "<<<|" << std::endl << "ROS" << std::endl << req.motion_plan_request << std::endl << "|>>>" << std::endl;
1308  out << req.motion_plan_request.allowed_planning_time << " seconds per run" << std::endl;
1309  out << duration << " seconds spent to collect the data" << std::endl;
1310  out << "reachable BOOLEAN" << std::endl;
1311  out << "collision_free BOOLEAN" << std::endl;
1312  out << "total_time REAL" << std::endl;
1313  out << reachable << "; " << success << "; " << duration << std::endl;
1314  out.close();
1315  ROS_INFO("Results saved to '%s'", filename.c_str());
1316  }
1317 
1318  if (req.motion_plan_request.trajectory_constraints.constraints.size() > 0)
1319  {
1320  // Compute IK on trajectory constraints
1321  // Start Log
1322  ros::WallTime startTime = ros::WallTime::now();
1323  std::string host = moveit_benchmarks::getHostname();
1324  std::string filename = req.filename.empty() ?
1325  ("moveit_benchmarks_" + host + "_" +
1326  boost::posix_time::to_iso_extended_string(startTime.toBoost()) + ".log") :
1327  req.filename;
1328  std::ofstream out(filename.c_str());
1329  out << "Experiment " << (planning_scene_->getName().empty() ? "NO_NAME" : planning_scene_->getName()) << std::endl;
1330  out << "Running on " << (host.empty() ? "UNKNOWN" : host) << std::endl;
1331  out << "Starting at " << boost::posix_time::to_iso_extended_string(startTime.toBoost()) << std::endl;
1332  out << "<<<|" << std::endl << "ROS" << std::endl << req.motion_plan_request << std::endl << "|>>>" << std::endl;
1333  out << req.motion_plan_request.allowed_planning_time << " seconds per run" << std::endl;
1334  out << "reachable BOOLEAN" << std::endl;
1335  out << "collision_free BOOLEAN" << std::endl;
1336  out << "total_time REAL" << std::endl;
1337 
1338  for (std::size_t tc = 0; tc < req.motion_plan_request.trajectory_constraints.constraints.size(); ++tc)
1339  {
1340  geometry_msgs::Pose ik_pose;
1341  ik_pose.position.x = req.motion_plan_request.trajectory_constraints.constraints[tc]
1342  .position_constraints[0]
1343  .constraint_region.primitive_poses[0]
1344  .position.x;
1345  ik_pose.position.y = req.motion_plan_request.trajectory_constraints.constraints[tc]
1346  .position_constraints[0]
1347  .constraint_region.primitive_poses[0]
1348  .position.y;
1349  ik_pose.position.z = req.motion_plan_request.trajectory_constraints.constraints[tc]
1350  .position_constraints[0]
1351  .constraint_region.primitive_poses[0]
1352  .position.z;
1353  ik_pose.orientation.x =
1354  req.motion_plan_request.trajectory_constraints.constraints[tc].orientation_constraints[0].orientation.x;
1355  ik_pose.orientation.y =
1356  req.motion_plan_request.trajectory_constraints.constraints[tc].orientation_constraints[0].orientation.y;
1357  ik_pose.orientation.z =
1358  req.motion_plan_request.trajectory_constraints.constraints[tc].orientation_constraints[0].orientation.z;
1359  ik_pose.orientation.w =
1360  req.motion_plan_request.trajectory_constraints.constraints[tc].orientation_constraints[0].orientation.w;
1361 
1362  robot_state::RobotState robot_state(planning_scene_->getCurrentState());
1363  robot_state::robotStateMsgToRobotState(req.motion_plan_request.start_state, robot_state);
1364 
1365  // Compute IK
1366  ROS_INFO_STREAM("Processing trajectory waypoint "
1367  << req.motion_plan_request.trajectory_constraints.constraints[tc].name << " ...");
1368  startTime = ros::WallTime::now();
1369  success = robot_state.setFromIK(
1370  robot_state.getJointModelGroup(req.motion_plan_request.group_name), ik_pose,
1371  req.motion_plan_request.num_planning_attempts, req.motion_plan_request.allowed_planning_time,
1372  boost::bind(&isIKSolutionCollisionFree, planning_scene_.get(), _1, _2, _3, &reachable));
1373  double duration = (ros::WallTime::now() - startTime).toSec();
1374 
1375  if (success)
1376  {
1377  ROS_INFO(" Success!");
1378  }
1379  else if (reachable)
1380  {
1381  ROS_INFO(" Reachable, but in collision");
1382  }
1383  else
1384  {
1385  ROS_INFO(" Not reachable");
1386  }
1387 
1388  out << reachable << "; " << success << "; " << duration << std::endl;
1389  }
1390  out.close();
1391  ROS_INFO("Results saved to '%s'", filename.c_str());
1392  }
1393 }
1394 
1395 void moveit_benchmarks::BenchmarkExecution::modifyPlannerConfiguration(planning_interface::PlannerManager& planner,
1396  const std::string& planner_id,
1397  std::size_t param_combinations_id_,
1398  RunData& parameter_data)
1399 {
1400  // Get the planner's current settings
1402 
1403  // Check if this planner_id already has settings (it should)
1404  planning_interface::PlannerConfigurationMap::iterator settings_it = settings.find(planner_id);
1405  if (settings_it != settings.end())
1406  {
1407  // key exists, loop through all values in this param instance
1408  std::string str_parameter_value;
1409  for (std::size_t i = 0; i < param_options_.size(); ++i)
1410  {
1411  // convert from double to string
1412  try
1413  {
1414  double value = param_combinations_[param_combinations_id_][param_options_[i].key];
1415  str_parameter_value = boost::lexical_cast<std::string>(value);
1416  }
1417  catch (boost::bad_lexical_cast& ex)
1418  {
1419  ROS_WARN("%s", ex.what());
1420  }
1421 
1422  // record parameter values for logging
1423  parameter_data[param_options_[i].log_key] = str_parameter_value;
1424 
1425  // record parameter to planner config
1426  settings_it->second.config[param_options_[i].key] = str_parameter_value;
1427  }
1428  }
1429  else // settings for this planner_id does not exist
1430  {
1431  ROS_ERROR_STREAM("Settings for " << planner_id << " do not exist. This should not happen.");
1432  }
1433 
1434  // Apply the new settings
1435  planner.setPlannerConfigurations(settings);
1436 }
1437 
1438 std::size_t moveit_benchmarks::BenchmarkExecution::generateParamCombinations()
1439 {
1440  if (!param_options_.size())
1441  return 1; // by default there are no parameters, so the param loop runs once
1442 
1443  // Create initial param instance of all min values for the parameters
1444  ParameterInstance param_instance;
1445  for (std::size_t i = 0; i < param_options_.size(); ++i)
1446  {
1447  // start = min value
1448  param_instance[param_options_[i].key] = param_options_[i].start;
1449  }
1450 
1451  // call recusive function for every param option available
1452  int initial_options_id = 0;
1453  recursiveParamCombinations(initial_options_id, param_instance);
1454 
1455  // DEBUG
1456  /*
1457  for (std::size_t i = 0; i < param_combinations_.size(); ++i)
1458  {
1459  // Debug map
1460  for(std::map<std::string,double>::const_iterator it = param_combinations_[i].begin(); it !=
1461  param_combinations_[i].end(); ++it)
1462  std::cout << " - " << it->first << " => " << it->second << std::endl;
1463  }
1464  */
1465 
1466  // Total number of parameters
1467  return param_combinations_.size();
1468 }
1469 
1470 void moveit_benchmarks::BenchmarkExecution::recursiveParamCombinations(int options_id, ParameterInstance param_instance)
1471 {
1472  // Get a pointer to current parameter
1473  const ParameterOptions& param_option = param_options_[options_id];
1474 
1475  do
1476  {
1477  // Call the next parameter if one exists
1478  if (param_options_.size() > options_id + 1)
1479  {
1480  recursiveParamCombinations(options_id + 1, param_instance);
1481  }
1482  else // we are the end of the recursive call, so we can add this param_instance to the vector
1483  {
1484  param_combinations_.push_back(param_instance);
1485  }
1486 
1487  // Increment this value
1488  param_instance[param_option.key] += param_option.step_size;
1489 
1490  // Continue adding iteration amount until value equals end
1491  } while (param_instance[param_option.key] <= param_option.end + 0.00001); // rounding issues fudge parameter
1492 
1493  return;
1494 }
1495 
1497 void moveit_benchmarks::BenchmarkExecution::printConfigurationSettings(
1498  const planning_interface::PlannerConfigurationMap& settings, std::ostream& out)
1499 {
1500  // Debug map
1501  for (planning_interface::PlannerConfigurationMap::const_iterator it = settings.begin(); it != settings.end(); ++it)
1502  {
1503  out << " - " << it->first << " => " << it->second.name << "/" << it->second.group << std::endl;
1504  // Debug map
1505  for (std::map<std::string, std::string>::const_iterator config_it = it->second.config.begin();
1506  config_it != it->second.config.end(); ++config_it)
1507  out << " - " << config_it->first << " => " << config_it->second << std::endl;
1508  }
1509 }
d
static std::string getHostname()
filename
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
std::size_t countIndividualConstraints(const moveit_msgs::Constraints &constr)
std::map< std::string, PlannerConfigurationSettings > PlannerConfigurationMap
const PlannerConfigurationMap & getPlannerConfigurations() const
#define ROS_WARN(...)
#define ROS_FATAL_STREAM(args)
#define ROS_INFO(...)
def default_value(type)
ROSCPP_DECL bool ok()
#define ROS_WARN_STREAM(args)
virtual void setPlannerConfigurations(const PlannerConfigurationMap &pcs)
moveit_msgs::MotionPlanRequest MotionPlanRequest
const robot_state::RobotState & getWayPoint(std::size_t index) const
std::size_t getWayPointCount() const
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
#define ROS_INFO_STREAM(args)
static WallTime now()
std::vector< robot_trajectory::RobotTrajectoryPtr > trajectory_
static Time now()
boost::posix_time::ptime toBoost() const
#define ROS_ERROR_STREAM(args)
#define ROS_ERROR(...)
bool isStateColliding(const std::string &group="", bool verbose=false)
#define ROS_DEBUG(...)


benchmarks
Author(s): Ryan Luna
autogenerated on Wed Jul 10 2019 04:04:08