benchmark_execution.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, Mario Prats, Dave Coleman */
00036 
00037 #include <moveit/benchmarks/benchmark_execution.h>
00038 #include <moveit/benchmarks/benchmarks_utils.h>
00039 #include <moveit/kinematic_constraints/utils.h>
00040 #include <moveit/planning_scene/planning_scene.h>
00041 #include <moveit/robot_state/conversions.h>
00042 
00043 #include <boost/regex.hpp>
00044 #include <boost/tokenizer.hpp>
00045 #include <boost/lexical_cast.hpp>
00046 #include <boost/program_options/parsers.hpp>
00047 #include <boost/program_options/variables_map.hpp>
00048 #include <boost/algorithm/string/case_conv.hpp>
00049 #include <boost/algorithm/string.hpp>
00050 #include <boost/math/constants/constants.hpp>
00051 #include <boost/progress.hpp>
00052 #include <boost/date_time/posix_time/posix_time.hpp>
00053 
00054 #include <eigen_conversions/eigen_msg.h>
00055 #include <Eigen/Core>
00056 #include <Eigen/Geometry>
00057 
00058 #include <fstream>
00059 
00060 namespace moveit_benchmarks
00061 {
00062 namespace
00063 {
00064 // update the constrained link for Position and Orientation constraints, if that link is empty
00065 void checkConstrainedLink(moveit_msgs::Constraints& c, const std::string& link_name)
00066 {
00067   for (std::size_t i = 0; i < c.position_constraints.size(); ++i)
00068     if (c.position_constraints[i].link_name.empty())
00069       c.position_constraints[i].link_name = link_name;
00070   for (std::size_t i = 0; i < c.orientation_constraints.size(); ++i)
00071     if (c.orientation_constraints[i].link_name.empty())
00072       c.orientation_constraints[i].link_name = link_name;
00073 }
00074 
00075 void checkHeader(moveit_msgs::Constraints& c, const std::string& header_frame)
00076 {
00077   for (std::size_t i = 0; i < c.position_constraints.size(); ++i)
00078     if (c.position_constraints[i].header.frame_id.empty())
00079     {
00080       c.position_constraints[i].header.frame_id = header_frame;
00081       c.position_constraints[i].header.stamp = ros::Time::now();
00082     }
00083   for (std::size_t i = 0; i < c.orientation_constraints.size(); ++i)
00084     if (c.orientation_constraints[i].header.frame_id.empty())
00085     {
00086       c.orientation_constraints[i].header.frame_id = header_frame;
00087       c.orientation_constraints[i].header.stamp = ros::Time::now();
00088     }
00089 }
00090 }
00091 }
00092 
00093 moveit_benchmarks::BenchmarkExecution::BenchmarkExecution(const planning_scene::PlanningScenePtr& scene,
00094                                                           const std::string& host, std::size_t port)
00095   : planning_scene_(scene), pss_(host, port), psws_(host, port), cs_(host, port), rs_(host, port)
00096 {
00097   // load the pluginlib class loader
00098   try
00099   {
00100     planner_plugin_loader_.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
00101         "moveit_core", "planning_interface::PlannerManager"));
00102   }
00103   catch (pluginlib::PluginlibException& ex)
00104   {
00105     ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
00106   }
00107 
00108   // load the planning plugins
00109   const std::vector<std::string>& classes = planner_plugin_loader_->getDeclaredClasses();
00110   for (std::size_t i = 0; i < classes.size(); ++i)
00111   {
00112     ROS_INFO("Attempting to load and configure %s", classes[i].c_str());
00113     try
00114     {
00115       planning_interface::PlannerManagerPtr p = planner_plugin_loader_->createInstance(classes[i]);
00116       p->initialize(planning_scene_->getRobotModel(), "");
00117       planner_interfaces_[classes[i]] = p;
00118     }
00119     catch (pluginlib::PluginlibException& ex)
00120     {
00121       ROS_ERROR_STREAM("Exception while loading planner '" << classes[i] << "': " << ex.what());
00122     }
00123   }
00124 
00125   // error check
00126   if (planner_interfaces_.empty())
00127     ROS_ERROR("No planning plugins have been loaded. Nothing to do for the benchmarking service.");
00128   else
00129   {
00130     std::stringstream ss;
00131     for (std::map<std::string, planning_interface::PlannerManagerPtr>::const_iterator it = planner_interfaces_.begin();
00132          it != planner_interfaces_.end(); ++it)
00133       ss << it->first << " ";
00134     ROS_INFO("Available planner instances: %s", ss.str().c_str());
00135   }
00136 }
00137 
00138 void moveit_benchmarks::BenchmarkExecution::runAllBenchmarks(BenchmarkType type)
00139 {
00140   moveit_warehouse::PlanningSceneWithMetadata pswm;
00141   moveit_warehouse::PlanningSceneWorldWithMetadata pswwm;
00142   bool world_only = false;
00143 
00144   // read the environment geometry
00145   if (!pss_.hasPlanningScene(options_.scene))
00146   {
00147     if (psws_.hasPlanningSceneWorld(options_.scene))
00148     {
00149       bool ok = false;
00150       try
00151       {
00152         ok = psws_.getPlanningSceneWorld(pswwm, options_.scene);
00153       }
00154       catch (std::runtime_error& ex)
00155       {
00156         ROS_ERROR("%s", ex.what());
00157       }
00158       if (!ok)
00159         return;
00160       world_only = true;
00161     }
00162     else
00163     {
00164       std::stringstream ss;
00165       std::vector<std::string> names;
00166       pss_.getPlanningSceneNames(names);
00167       for (std::size_t i = 0; i < names.size(); ++i)
00168         ss << names[i] << " ";
00169       ROS_ERROR("Scene '%s' not found in warehouse. Available names: %s", options_.scene.c_str(), ss.str().c_str());
00170       return;
00171     }
00172   }
00173   else
00174   {
00175     bool ok = false;
00176     try
00177     {
00178       ok = pss_.getPlanningScene(pswm, options_.scene);
00179     }
00180     catch (std::runtime_error& ex)
00181     {
00182       ROS_ERROR("%s", ex.what());
00183     }
00184 
00185     if (!ok)
00186     {
00187       ROS_ERROR("Scene '%s' not found in warehouse", options_.scene.c_str());
00188       return;
00189     }
00190   }
00191 
00192   // fill in the environment geometry in the benchmark request
00193   BenchmarkRequest req;
00194   req.benchmark_type = type;
00195 
00196   if (world_only)
00197   {
00198     req.scene.world = static_cast<const moveit_msgs::PlanningSceneWorld&>(*pswwm);
00199     req.scene.robot_model_name =
00200         "NO ROBOT INFORMATION. ONLY WORLD GEOMETRY";  // so that run_benchmark sees a different robot name
00201   }
00202   else
00203     req.scene = static_cast<const moveit_msgs::PlanningScene&>(*pswm);
00204 
00205   // check if this scene has associated queries
00206   req.scene.name = options_.scene;
00207   std::vector<std::string> planning_queries_names;
00208   try
00209   {
00210     pss_.getPlanningQueriesNames(options_.query_regex, planning_queries_names, options_.scene);
00211   }
00212   catch (std::runtime_error& ex)
00213   {
00214     ROS_ERROR("%s", ex.what());
00215   }
00216   if (planning_queries_names.empty())
00217     ROS_DEBUG("Scene '%s' has no associated queries", options_.scene.c_str());
00218 
00219   // fill in the plugins option
00220   req.plugins = options_.plugins;
00221 
00222   // see if we have any start states specified; if we do, we run the benchmark for
00223   // each start state; if not, we run it for one start state only: the current one saved in the loaded scene
00224   std::vector<std::string> start_states;
00225   if (!options_.start_regex.empty())
00226   {
00227     boost::regex start_regex(options_.start_regex);
00228     std::vector<std::string> state_names;
00229     rs_.getKnownRobotStates(state_names);
00230     for (std::size_t i = 0; i < state_names.size(); ++i)
00231     {
00232       boost::cmatch match;
00233       if (boost::regex_match(state_names[i].c_str(), match, start_regex))
00234         start_states.push_back(state_names[i]);
00235     }
00236     if (start_states.empty())
00237     {
00238       ROS_WARN("No stored states matched the provided regex: '%s'", options_.start_regex.c_str());
00239       return;
00240     }
00241     else
00242       ROS_INFO("Running benchmark using %u start states.", (unsigned int)start_states.size());
00243   }
00244   else
00245     ROS_INFO("No specified start state. Running benchmark once with the default start state.");
00246 
00247   unsigned int n_call = 0;
00248   bool have_more_start_states = true;
00249   boost::scoped_ptr<moveit_msgs::RobotState> start_state_to_use;
00250   while (have_more_start_states)
00251   {
00252     start_state_to_use.reset();
00253 
00254     // if no set of start states provided, run once for the current one
00255     if (options_.start_regex.empty())
00256       have_more_start_states = false;
00257     else
00258     {
00259       // otherwise, extract the start states one by one
00260       std::string state_name = start_states.back();
00261       start_states.pop_back();
00262       have_more_start_states = !start_states.empty();
00263       moveit_warehouse::RobotStateWithMetadata robot_state;
00264       bool got_robot_state = false;
00265       try
00266       {
00267         got_robot_state = rs_.getRobotState(robot_state, state_name);
00268       }
00269       catch (std::runtime_error& ex)
00270       {
00271         ROS_ERROR("%s", ex.what());
00272       }
00273 
00274       if (got_robot_state)
00275       {
00276         start_state_to_use.reset(new moveit_msgs::RobotState(*robot_state));
00277         ROS_INFO("Loaded start state '%s'", state_name.c_str());
00278       }
00279       else
00280         continue;
00281     }
00282 
00283     // run benchmarks for specified queries
00284     // ************************************
00285 
00286     if (!options_.query_regex.empty())
00287     {
00288       for (std::size_t i = 0; i < planning_queries_names.size(); ++i)
00289       {
00290         moveit_warehouse::MotionPlanRequestWithMetadata planning_query;
00291         pss_.getPlanningQuery(planning_query, options_.scene, planning_queries_names[i]);
00292 
00293         // Save name of goal - only used for later analysis
00294         req.goal_name = planning_queries_names[i];
00295 
00296         // read request from db
00297         req.motion_plan_request = static_cast<const moveit_msgs::MotionPlanRequest&>(*planning_query);
00298 
00299         // set the workspace bounds
00300         req.motion_plan_request.workspace_parameters = options_.workspace_parameters;
00301 
00302         // update request given .cfg options
00303         if (start_state_to_use)
00304           req.motion_plan_request.start_state = *start_state_to_use;
00305         req.filename = options_.output + "." + boost::lexical_cast<std::string>(++n_call) + ".log";
00306         if (!options_.group_override.empty())
00307           req.motion_plan_request.group_name = options_.group_override;
00308 
00309         if (options_.timeout > 0.0)
00310           req.motion_plan_request.allowed_planning_time = options_.timeout;
00311 
00312         if (!options_.default_constrained_link.empty())
00313         {
00314           checkConstrainedLink(req.motion_plan_request.path_constraints, options_.default_constrained_link);
00315           for (std::size_t j = 0; j < req.motion_plan_request.goal_constraints.size(); ++j)
00316             checkConstrainedLink(req.motion_plan_request.goal_constraints[j], options_.default_constrained_link);
00317         }
00318         if (!options_.planning_frame.empty())
00319         {
00320           checkHeader(req.motion_plan_request.path_constraints, options_.planning_frame);
00321           for (std::size_t j = 0; j < req.motion_plan_request.goal_constraints.size(); ++j)
00322             checkHeader(req.motion_plan_request.goal_constraints[j], options_.planning_frame);
00323         }
00324 
00325         ROS_INFO("Benckmarking query '%s' (%d of %d)", planning_queries_names[i].c_str(), (int)i + 1,
00326                  (int)planning_queries_names.size());
00327         runBenchmark(req);
00328       }
00329     }
00330 
00331     // if we have any goals specified as constraints, run benchmarks for those as well
00332     // *******************************************************************************
00333 
00334     if (!options_.goal_regex.empty())
00335     {
00336       std::vector<std::string> cnames;
00337       cs_.getKnownConstraints(options_.goal_regex, cnames);
00338       for (std::size_t i = 0; i < cnames.size(); ++i)
00339       {
00340         moveit_warehouse::ConstraintsWithMetadata constr;
00341         bool got_constraints = false;
00342         try
00343         {
00344           got_constraints = cs_.getConstraints(constr, cnames[i]);
00345         }
00346         catch (std::runtime_error& ex)
00347         {
00348           ROS_ERROR("%s", ex.what());
00349         }
00350 
00351         if (got_constraints)
00352         {
00353           // Save name of goal - only used for later analysis
00354           req.goal_name = cnames[i];
00355 
00356           // construct a planning request from the constraints message
00357           req.motion_plan_request = moveit_msgs::MotionPlanRequest();
00358           req.motion_plan_request.goal_constraints.resize(1);
00359           if (start_state_to_use)
00360             req.motion_plan_request.start_state = *start_state_to_use;
00361           req.motion_plan_request.goal_constraints[0] = *constr;
00362 
00363           // set the workspace bounds
00364           req.motion_plan_request.workspace_parameters = options_.workspace_parameters;
00365 
00366           // Apply the goal offset for constraints that appear to specify IK poses
00367           if (constr->position_constraints.size() == 1 && constr->orientation_constraints.size() == 1 &&
00368               kinematic_constraints::countIndividualConstraints(*constr) == 2 &&
00369               constr->position_constraints[0].constraint_region.primitive_poses.size() == 1 &&
00370               constr->position_constraints[0].constraint_region.mesh_poses.empty())
00371           {
00372             geometry_msgs::Pose wMc_msg;
00373             wMc_msg.position = constr->position_constraints[0].constraint_region.primitive_poses[0].position;
00374             wMc_msg.orientation = constr->orientation_constraints[0].orientation;
00375             Eigen::Affine3d wMc;
00376             tf::poseMsgToEigen(wMc_msg, wMc);
00377 
00378             Eigen::Affine3d offset_tf(Eigen::AngleAxis<double>(options_.offsets[3], Eigen::Vector3d::UnitX()) *
00379                                       Eigen::AngleAxis<double>(options_.offsets[4], Eigen::Vector3d::UnitY()) *
00380                                       Eigen::AngleAxis<double>(options_.offsets[5], Eigen::Vector3d::UnitZ()));
00381             offset_tf.translation() = Eigen::Vector3d(options_.offsets[0], options_.offsets[1], options_.offsets[2]);
00382 
00383             Eigen::Affine3d wMnc = wMc * offset_tf;
00384             geometry_msgs::Pose wMnc_msg;
00385             tf::poseEigenToMsg(wMnc, wMnc_msg);
00386 
00387             req.motion_plan_request.goal_constraints[0]
00388                 .position_constraints[0]
00389                 .constraint_region.primitive_poses[0]
00390                 .position = wMnc_msg.position;
00391             req.motion_plan_request.goal_constraints[0].orientation_constraints[0].orientation = wMnc_msg.orientation;
00392           }
00393 
00394           if (!options_.group_override.empty())
00395             req.motion_plan_request.group_name = options_.group_override;
00396           if (options_.timeout > 0.0)
00397             req.motion_plan_request.allowed_planning_time = options_.timeout;
00398           if (!options_.default_constrained_link.empty())
00399             checkConstrainedLink(req.motion_plan_request.goal_constraints[0], options_.default_constrained_link);
00400           if (!options_.planning_frame.empty())
00401             checkHeader(req.motion_plan_request.goal_constraints[0], options_.planning_frame);
00402           req.filename = options_.output + "." + boost::lexical_cast<std::string>(++n_call) + ".log";
00403 
00404           ROS_INFO("Benckmarking goal '%s' (%d of %d)", cnames[i].c_str(), (int)i + 1, (int)cnames.size());
00405           runBenchmark(req);
00406         }
00407       }
00408     }
00409 
00410     // if we have any goals specified as trajectory constraints, run benchmarks for those as well
00411     // ******************************************************************************************
00412 
00413     if (!options_.trajectory_regex.empty())
00414     {
00415       std::vector<std::string> cnames;
00416       tcs_.getKnownTrajectoryConstraints(options_.trajectory_regex, cnames);
00417       for (std::size_t i = 0; i < cnames.size(); ++i)
00418       {
00419         moveit_warehouse::TrajectoryConstraintsWithMetadata constr;
00420         bool got_constraints = false;
00421         try
00422         {
00423           got_constraints = tcs_.getTrajectoryConstraints(constr, cnames[i]);
00424         }
00425         catch (std::runtime_error& ex)
00426         {
00427           ROS_ERROR("%s", ex.what());
00428         }
00429 
00430         // Save name of goal - only used for later analysis
00431         req.goal_name = cnames[i];
00432 
00433         if (got_constraints)
00434         {
00435           // construct a planning request from the trajectory constraints message
00436           req.motion_plan_request = moveit_msgs::MotionPlanRequest();
00437           if (start_state_to_use)
00438             req.motion_plan_request.start_state = *start_state_to_use;
00439           req.motion_plan_request.trajectory_constraints = *constr;
00440 
00441           // set the workspace bounds
00442           req.motion_plan_request.workspace_parameters = options_.workspace_parameters;
00443 
00444           Eigen::Affine3d offset_tf(Eigen::AngleAxis<double>(options_.offsets[3], Eigen::Vector3d::UnitX()) *
00445                                     Eigen::AngleAxis<double>(options_.offsets[4], Eigen::Vector3d::UnitY()) *
00446                                     Eigen::AngleAxis<double>(options_.offsets[5], Eigen::Vector3d::UnitZ()));
00447           offset_tf.translation() = Eigen::Vector3d(options_.offsets[0], options_.offsets[1], options_.offsets[2]);
00448 
00449           // Apply waypoint offsets, check fields
00450           for (std::size_t tc = 0; tc < constr->constraints.size(); ++tc)
00451           {
00452             // Apply the goal offset for constraints that appear to specify IK poses
00453             if (constr->constraints[tc].position_constraints.size() == 1 &&
00454                 constr->constraints[tc].orientation_constraints.size() == 1 &&
00455                 kinematic_constraints::countIndividualConstraints(constr->constraints[tc]) == 2 &&
00456                 constr->constraints[tc].position_constraints[0].constraint_region.primitive_poses.size() == 1 &&
00457                 constr->constraints[tc].position_constraints[0].constraint_region.mesh_poses.empty())
00458             {
00459               geometry_msgs::Pose wMc_msg;
00460               wMc_msg.position = req.motion_plan_request.trajectory_constraints.constraints[tc]
00461                                      .position_constraints[0]
00462                                      .constraint_region.primitive_poses[0]
00463                                      .position;
00464               wMc_msg.orientation =
00465                   req.motion_plan_request.trajectory_constraints.constraints[tc].orientation_constraints[0].orientation;
00466               Eigen::Affine3d wMc;
00467               tf::poseMsgToEigen(wMc_msg, wMc);
00468 
00469               Eigen::Affine3d wMnc = wMc * offset_tf;
00470               geometry_msgs::Pose wMnc_msg;
00471               tf::poseEigenToMsg(wMnc, wMnc_msg);
00472 
00473               req.motion_plan_request.trajectory_constraints.constraints[tc]
00474                   .position_constraints[0]
00475                   .constraint_region.primitive_poses[0]
00476                   .position = wMnc_msg.position;
00477               req.motion_plan_request.trajectory_constraints.constraints[tc].orientation_constraints[0].orientation =
00478                   wMnc_msg.orientation;
00479             }
00480             if (!options_.default_constrained_link.empty())
00481               checkConstrainedLink(req.motion_plan_request.trajectory_constraints.constraints[tc],
00482                                    options_.default_constrained_link);
00483             if (!options_.planning_frame.empty())
00484               checkHeader(req.motion_plan_request.trajectory_constraints.constraints[tc], options_.planning_frame);
00485           }
00486 
00487           if (!options_.group_override.empty())
00488             req.motion_plan_request.group_name = options_.group_override;
00489           if (options_.timeout > 0.0)
00490             req.motion_plan_request.allowed_planning_time = options_.timeout;
00491           req.filename = options_.output + ".trajectory." + boost::lexical_cast<std::string>(i + 1) + ".log";
00492 
00493           ROS_INFO("Benckmarking trajectory '%s' (%d of %d)", cnames[i].c_str(), (int)i + 1, (int)cnames.size());
00494           runBenchmark(req);
00495         }
00496       }
00497     }
00498   }
00499 }
00500 
00501 bool moveit_benchmarks::BenchmarkExecution::readOptions(const std::string& filename)
00502 {
00503   ROS_INFO("Loading '%s'...", filename.c_str());
00504 
00505   std::ifstream cfg(filename.c_str());
00506   if (!cfg.good())
00507   {
00508     ROS_ERROR_STREAM("Unable to open file '" << filename << "'");
00509     return false;
00510   }
00511 
00512   // "scene" options
00513   try
00514   {
00515     boost::program_options::options_description desc;
00516     desc.add_options()("scene.name", boost::program_options::value<std::string>(), "Scene name")(
00517         "scene.runs", boost::program_options::value<std::string>()->default_value("1"), "Number of runs")(
00518         "scene.timeout", boost::program_options::value<std::string>()->default_value(""),
00519         "Timeout for planning (s)")("scene.start", boost::program_options::value<std::string>()->default_value(""),
00520                                     "Regex for the start states to use")(
00521         "scene.query", boost::program_options::value<std::string>()->default_value(".*"),
00522         "Regex for the queries to execute")("scene.goal",
00523                                             boost::program_options::value<std::string>()->default_value(""),
00524                                             "Regex for the names of constraints to use as goals")(
00525         "scene.trajectory", boost::program_options::value<std::string>()->default_value(""),
00526         "Regex for the names of constraints to use as trajectories")(
00527         "scene.group", boost::program_options::value<std::string>()->default_value(""),
00528         "Override the group to plan for")("scene.planning_frame",
00529                                           boost::program_options::value<std::string>()->default_value(""),
00530                                           "Override the planning frame to use")(
00531         "scene.default_constrained_link", boost::program_options::value<std::string>()->default_value(""),
00532         "Specify the default link to consider as constrained when one is not specified in a moveit_msgs::Constraints "
00533         "message")("scene.goal_offset_x", boost::program_options::value<std::string>()->default_value("0.0"),
00534                    "Goal offset in x")(
00535         "scene.goal_offset_y", boost::program_options::value<std::string>()->default_value("0.0"), "Goal offset in y")(
00536         "scene.goal_offset_z", boost::program_options::value<std::string>()->default_value("0.0"),
00537         "Goal offset in z")("scene.goal_offset_roll",
00538                             boost::program_options::value<std::string>()->default_value("0.0"), "Goal offset in roll")(
00539         "scene.goal_offset_pitch", boost::program_options::value<std::string>()->default_value("0.0"),
00540         "Goal offset in pitch")("scene.goal_offset_yaw",
00541                                 boost::program_options::value<std::string>()->default_value("0.0"),
00542                                 "Goal offset in yaw")("scene.output", boost::program_options::value<std::string>(),
00543                                                       "Location of benchmark log file")(
00544         "scene.workspace", boost::program_options::value<std::string>(), "Bounding box of workspace to plan in - "
00545                                                                          "min_x, min_y, min_z, max_x, max_y, max_z")(
00546         "scene.workspace_frame", boost::program_options::value<std::string>(), "Frame id of bounding box of workspace "
00547                                                                                "to plan in");
00548 
00549     boost::program_options::variables_map vm;
00550     boost::program_options::parsed_options po = boost::program_options::parse_config_file(cfg, desc, true);
00551 
00552     cfg.close();
00553     boost::program_options::store(po, vm);
00554 
00555     std::map<std::string, std::string> declared_options;
00556     for (boost::program_options::variables_map::iterator it = vm.begin(); it != vm.end(); ++it)
00557       declared_options[it->first] = boost::any_cast<std::string>(vm[it->first].value());
00558 
00559     options_.scene = declared_options["scene.name"];
00560     options_.start_regex = declared_options["scene.start"];
00561     options_.query_regex = declared_options["scene.query"];
00562     options_.goal_regex = declared_options["scene.goal"];
00563     options_.trajectory_regex = declared_options["scene.trajectory"];
00564     options_.group_override = declared_options["scene.group"];
00565     options_.default_constrained_link = declared_options["scene.default_constrained_link"];
00566     options_.planning_frame = declared_options["scene.planning_frame"];
00567     try
00568     {
00569       memset(options_.offsets, 0, 6 * sizeof(double));
00570       if (!declared_options["scene.goal_offset_x"].empty())
00571         options_.offsets[0] = boost::lexical_cast<double>(declared_options["scene.goal_offset_x"]);
00572       if (!declared_options["scene.goal_offset_y"].empty())
00573         options_.offsets[1] = boost::lexical_cast<double>(declared_options["scene.goal_offset_y"]);
00574       if (!declared_options["scene.goal_offset_z"].empty())
00575         options_.offsets[2] = boost::lexical_cast<double>(declared_options["scene.goal_offset_z"]);
00576       if (!declared_options["scene.goal_offset_roll"].empty())
00577         options_.offsets[3] = boost::lexical_cast<double>(declared_options["scene.goal_offset_roll"]);
00578       if (!declared_options["scene.goal_offset_pitch"].empty())
00579         options_.offsets[4] = boost::lexical_cast<double>(declared_options["scene.goal_offset_pitch"]);
00580       if (!declared_options["scene.goal_offset_yaw"].empty())
00581         options_.offsets[5] = boost::lexical_cast<double>(declared_options["scene.goal_offset_yaw"]);
00582     }
00583     catch (boost::bad_lexical_cast& ex)
00584     {
00585       ROS_WARN("%s", ex.what());
00586     }
00587 
00588     // Workspace bounds
00589     if (!declared_options["scene.workspace"].empty())
00590     {
00591       std::vector<std::string> strings;
00592       boost::split(strings, declared_options["scene.workspace"], boost::is_any_of(","));
00593 
00594       if (strings.size() != 6)
00595       {
00596         ROS_WARN_STREAM("Invalid number of workspace parameters. Expected 6, recieved " << strings.size());
00597       }
00598       else if (declared_options["scene.workspace_frame"].empty())
00599       {
00600         ROS_WARN_STREAM("No workspace_frame parameter provided, required with the workspace frame");
00601       }
00602       else
00603       {
00604         try
00605         {
00606           // add workspace bounds if specified in the .cfg file
00607           options_.workspace_parameters.header.frame_id = declared_options["scene.workspace_frame"];
00608           options_.workspace_parameters.header.stamp = ros::Time::now();
00609           options_.workspace_parameters.min_corner.x = boost::lexical_cast<double>(strings[0]);
00610           options_.workspace_parameters.min_corner.y = boost::lexical_cast<double>(strings[1]);
00611           options_.workspace_parameters.min_corner.z = boost::lexical_cast<double>(strings[2]);
00612           options_.workspace_parameters.max_corner.x = boost::lexical_cast<double>(strings[3]);
00613           options_.workspace_parameters.max_corner.y = boost::lexical_cast<double>(strings[4]);
00614           options_.workspace_parameters.max_corner.z = boost::lexical_cast<double>(strings[5]);
00615         }
00616         catch (boost::bad_lexical_cast& ex)
00617         {
00618           ROS_WARN("%s", ex.what());
00619         }
00620       }
00621     }
00622 
00623     // Filename
00624     options_.output = declared_options["scene.output"];
00625     if (options_.output.empty())
00626       options_.output = filename;
00627 
00628     options_.plugins.clear();
00629 
00630     // Runs
00631     std::size_t default_run_count = 1;
00632     if (!declared_options["scene.runs"].empty())
00633     {
00634       try
00635       {
00636         default_run_count = boost::lexical_cast<std::size_t>(declared_options["scene.runs"]);
00637       }
00638       catch (boost::bad_lexical_cast& ex)
00639       {
00640         ROS_WARN("%s", ex.what());
00641       }
00642     }
00643     options_.default_run_count = default_run_count;
00644 
00645     // Timeout
00646     options_.timeout = 0.0;
00647     if (!declared_options["scene.timeout"].empty())
00648     {
00649       try
00650       {
00651         options_.timeout = boost::lexical_cast<double>(declared_options["scene.timeout"]);
00652       }
00653       catch (boost::bad_lexical_cast& ex)
00654       {
00655         ROS_WARN("%s", ex.what());
00656       }
00657     }
00658 
00659     // Process non-scene options
00660     std::vector<std::string> unr =
00661         boost::program_options::collect_unrecognized(po.options, boost::program_options::exclude_positional);
00662 
00663     boost::scoped_ptr<PlanningPluginOptions> bpo;
00664     for (std::size_t i = 0; i < unr.size() / 2; ++i)
00665     {
00666       std::string key = boost::to_lower_copy(unr[i * 2]);
00667       std::string val = unr[i * 2 + 1];
00668 
00669       // "plugin" options
00670       if (key.substr(0, 7) == "plugin.")
00671       {
00672         std::string k = key.substr(7);
00673         if (k == "name")
00674         {
00675           if (bpo)
00676             options_.plugins.push_back(*bpo);
00677           bpo.reset(new PlanningPluginOptions());
00678           bpo->name = val;
00679           bpo->runs = default_run_count;
00680         }
00681         else if (k == "runs")
00682         {
00683           if (bpo)
00684           {
00685             try
00686             {
00687               bpo->runs = boost::lexical_cast<std::size_t>(val);
00688             }
00689             catch (boost::bad_lexical_cast& ex)
00690             {
00691               ROS_WARN("%s", ex.what());
00692             }
00693           }
00694           else
00695             ROS_WARN("Ignoring option '%s' = '%s'. Please include plugin name first.", key.c_str(), val.c_str());
00696         }
00697         else if (k == "planners")
00698         {
00699           if (bpo)
00700           {
00701             boost::char_separator<char> sep(" ");
00702             boost::tokenizer<boost::char_separator<char> > tok(val, sep);
00703             for (boost::tokenizer<boost::char_separator<char> >::iterator beg = tok.begin(); beg != tok.end(); ++beg)
00704               bpo->planners.push_back(*beg);
00705           }
00706           else
00707             ROS_WARN("Ignoring option '%s' = '%s'. Please include plugin name first.", key.c_str(), val.c_str());
00708         }
00709       }
00710       // parameter sweeping option
00711       else if (key.substr(0, 6) == "sweep.")
00712       {
00713         std::string sweep_var = key.substr(6);
00714 
00715         // Convert the string of a:b:c numbers into parsed doubles
00716         std::vector<std::string> strings;
00717         boost::split(strings, val, boost::is_any_of(":"));
00718 
00719         if (strings.size() != 3)
00720         {
00721           ROS_WARN_STREAM("Invalid sweep parameter for key "
00722                           << sweep_var << ". Expected 3 values (start, iterator, end) but only recieved "
00723                           << strings.size());
00724           continue;
00725         }
00726 
00727         ParameterOptions new_sweep;
00728         new_sweep.is_sweep = true;  // not a fractional factorial analaysis
00729         try
00730         {
00731           new_sweep.start = boost::lexical_cast<double>(strings[0]);
00732           new_sweep.step_size = boost::lexical_cast<double>(strings[1]);
00733           new_sweep.end = boost::lexical_cast<double>(strings[2]);
00734           new_sweep.key = sweep_var;
00735           // for logging to file:
00736           std::ostringstream ss;
00737           ss << "param_" << sweep_var << " REAL";
00738           new_sweep.log_key = ss.str();
00739         }
00740         catch (boost::bad_lexical_cast& ex)
00741         {
00742           ROS_WARN("%s", ex.what());
00743         }
00744 
00745         // Error check
00746         if (new_sweep.start > new_sweep.end)
00747         {
00748           ROS_ERROR_STREAM("Invalid sweep parameter for key " << sweep_var << ". Start is greater than end");
00749           continue;
00750         }
00751 
00752         // Insert into array of all sweeps
00753         param_options_.push_back(new_sweep);
00754       }
00755       else
00756       {
00757         ROS_WARN("Unknown option: '%s' = '%s'", key.c_str(), val.c_str());
00758         continue;
00759       }
00760     }
00761     if (bpo)
00762       options_.plugins.push_back(*bpo);
00763   }
00764 
00765   catch (...)
00766   {
00767     ROS_ERROR_STREAM("Unable to parse '" << filename << "'");
00768     return false;
00769   }
00770 
00771   return true;
00772 }
00773 
00774 void moveit_benchmarks::BenchmarkExecution::printOptions(std::ostream& out)
00775 {
00776   out << "Benchmark for scene '" << options_.scene << "' to be saved at location '" << options_.output << "'"
00777       << std::endl;
00778   if (!options_.query_regex.empty())
00779     out << "Planning requests associated to the scene that match '" << options_.query_regex << "' will be evaluated"
00780         << std::endl;
00781   if (!options_.goal_regex.empty())
00782     out << "Planning requests constructed from goal constraints that match '" << options_.goal_regex
00783         << "' will be evaluated" << std::endl;
00784   if (!options_.trajectory_regex.empty())
00785     out << "Planning requests constructed from trajectory constraints that match '" << options_.trajectory_regex
00786         << "' will be evaluated" << std::endl;
00787   out << "Plugins:" << std::endl;
00788   for (std::size_t i = 0; i < options_.plugins.size(); ++i)
00789   {
00790     out << "   * name: " << options_.plugins[i].name << " (to be run " << options_.plugins[i].runs
00791         << " times for each planner)" << std::endl;
00792     out << "   * planners:";
00793     for (std::size_t j = 0; j < options_.plugins[i].planners.size(); ++j)
00794       out << ' ' << options_.plugins[i].planners[j];
00795     out << std::endl;
00796   }
00797 }
00798 
00799 void moveit_benchmarks::BenchmarkExecution::runBenchmark(BenchmarkRequest& req)
00800 {
00801   if (req.benchmark_type & BENCHMARK_PLANNERS)
00802     runPlanningBenchmark(req);
00803   if (req.benchmark_type & BENCHMARK_GOAL_EXISTANCE)
00804     runGoalExistenceBenchmark(req);
00805 }
00806 
00807 void moveit_benchmarks::BenchmarkExecution::collectMetrics(RunData& rundata,
00808                                                            const planning_interface::MotionPlanDetailedResponse& mp_res,
00809                                                            bool solved, double total_time)
00810 {
00811   rundata["total_time REAL"] = boost::lexical_cast<std::string>(total_time);
00812   rundata["solved BOOLEAN"] = boost::lexical_cast<std::string>(solved);
00813   double L = 0.0;
00814   double clearance = 0.0;
00815   double smoothness = 0.0;
00816   bool correct = true;
00817   if (solved)
00818   {
00819     double process_time = total_time;
00820     for (std::size_t j = 0; j < mp_res.trajectory_.size(); ++j)
00821     {
00822       correct = true;
00823       L = 0.0;
00824       clearance = 0.0;
00825       smoothness = 0.0;
00826       const robot_trajectory::RobotTrajectory& p = *mp_res.trajectory_[j];
00827 
00828       // compute path length
00829       for (std::size_t k = 1; k < p.getWayPointCount(); ++k)
00830         L += p.getWayPoint(k - 1).distance(p.getWayPoint(k));
00831 
00832       // compute correctness and clearance
00833       collision_detection::CollisionRequest req;
00834       for (std::size_t k = 0; k < p.getWayPointCount(); ++k)
00835       {
00836         collision_detection::CollisionResult res;
00837         planning_scene_->checkCollisionUnpadded(req, res, p.getWayPoint(k));
00838         if (res.collision)
00839           correct = false;
00840         if (!p.getWayPoint(k).satisfiesBounds())
00841           correct = false;
00842         double d = planning_scene_->distanceToCollisionUnpadded(p.getWayPoint(k));
00843         if (d > 0.0)  // in case of collision, distance is negative
00844           clearance += d;
00845       }
00846       clearance /= (double)p.getWayPointCount();
00847 
00848       // compute smoothness
00849       if (p.getWayPointCount() > 2)
00850       {
00851         double a = p.getWayPoint(0).distance(p.getWayPoint(1));
00852         for (std::size_t k = 2; k < p.getWayPointCount(); ++k)
00853         {
00854           // view the path as a sequence of segments, and look at the triangles it forms:
00855           //          s1
00856           //          /\          s4
00857           //      a  /  \ b       |
00858           //        /    \        |
00859           //       /......\_______|
00860           //     s0    c   s2     s3
00861           //
00862           // use Pythagoras generalized theorem to find the cos of the angle between segments a and b
00863           double b = p.getWayPoint(k - 1).distance(p.getWayPoint(k));
00864           double cdist = p.getWayPoint(k - 2).distance(p.getWayPoint(k));
00865           double acosValue = (a * a + b * b - cdist * cdist) / (2.0 * a * b);
00866           if (acosValue > -1.0 && acosValue < 1.0)
00867           {
00868             // the smoothness is actually the outside angle of the one we compute
00869             double angle = (boost::math::constants::pi<double>() - acos(acosValue));
00870 
00871             // and we normalize by the length of the segments
00872             double u = 2.0 * angle;  
00873             smoothness += u * u;
00874           }
00875           a = b;
00876         }
00877         smoothness /= (double)p.getWayPointCount();
00878       }
00879       rundata["path_" + mp_res.description_[j] + "_correct BOOLEAN"] = boost::lexical_cast<std::string>(correct);
00880       rundata["path_" + mp_res.description_[j] + "_length REAL"] = boost::lexical_cast<std::string>(L);
00881       rundata["path_" + mp_res.description_[j] + "_clearance REAL"] = boost::lexical_cast<std::string>(clearance);
00882       rundata["path_" + mp_res.description_[j] + "_smoothness REAL"] = boost::lexical_cast<std::string>(smoothness);
00883       rundata["path_" + mp_res.description_[j] + "_time REAL"] =
00884           boost::lexical_cast<std::string>(mp_res.processing_time_[j]);
00885       process_time -= mp_res.processing_time_[j];
00886     }
00887     if (process_time <= 0.0)
00888       process_time = 0.0;
00889     rundata["process_time REAL"] = boost::lexical_cast<std::string>(process_time);
00890   }
00891 }
00892 
00893 namespace
00894 {
00895 bool isIKSolutionCollisionFree(const planning_scene::PlanningScene* scene, robot_state::RobotState* state,
00896                                const robot_model::JointModelGroup* group, const double* ik_solution, bool* reachable)
00897 {
00898   state->setJointGroupPositions(group, ik_solution);
00899   state->update();
00900   *reachable = true;
00901   if (scene->isStateColliding(*state, group->getName(), false))
00902     return false;
00903   else
00904     return true;
00905 }
00906 }
00907 
00908 void moveit_benchmarks::BenchmarkExecution::runPlanningBenchmark(BenchmarkRequest& req)
00909 {
00910   /* Dev Notes:
00911    *   planner_interface => planning plugin
00912    *   planner => planning algorithm within a planning plugin
00913    *   planner_ids => id of a planner
00914    */
00915 
00916   // check that all requested plugins exist
00917   if (!req.plugins.empty())
00918     for (std::size_t i = 0; i < req.plugins.size(); ++i)
00919       if (planner_interfaces_.find(req.plugins[i].name) == planner_interfaces_.end())
00920         ROS_ERROR("Planning interface '%s' was not found", req.plugins[i].name.c_str());
00921 
00922   // pointer list of planning plugins
00923   std::vector<planning_interface::PlannerManager*> planner_interfaces_to_benchmark;
00924 
00925   // each planning plugin has a vector of its sub algorithms (planners) that it can run
00926   std::vector<std::vector<std::string> > planner_ids_to_benchmark_per_planner_interface;
00927 
00928   // number of runs to execute every *algorithm* per *plugin*
00929   std::vector<std::size_t> runs_per_planner_interface;  // average_count_per_planner_interface
00930 
00931   planning_interface::MotionPlanRequest motion_plan_req = req.motion_plan_request;
00932 
00933   // loop through each planning interface
00934   for (std::map<std::string, planning_interface::PlannerManagerPtr>::const_iterator it = planner_interfaces_.begin();
00935        it != planner_interfaces_.end(); ++it)
00936   {
00937     // find the plugin that the planning interface belongs to
00938     int found = -1;
00939     if (!req.plugins.empty())
00940     {
00941       for (std::size_t i = 0; i < req.plugins.size(); ++i)
00942       {
00943         if (req.plugins[i].name == it->first)  // this benchmark request includes this planning plugin
00944         {
00945           found = i;
00946           break;
00947         }
00948       }
00949       if (found < 0)
00950         continue;
00951     }
00952 
00953     // Determine whether this plugin instance is able to represent this planning request
00954     if (it->second->canServiceRequest(motion_plan_req))
00955     {
00956       // copy the pointer of the planner_interface
00957       planner_interfaces_to_benchmark.push_back(it->second.get());
00958 
00959       // add new rows to the "mapper" for the new planner_interface
00960       planner_ids_to_benchmark_per_planner_interface.resize(planner_ids_to_benchmark_per_planner_interface.size() + 1);
00961       runs_per_planner_interface.resize(runs_per_planner_interface.size() + 1, 1);  // TODO: a vector does not need to
00962                                                                                     // be resized, it does so
00963                                                                                     // automatically right??
00964 
00965       // get a list of all the algorithms that the planner can use
00966       std::vector<std::string> known;
00967       it->second->getPlanningAlgorithms(known);
00968 
00969       if (found < 0 || req.plugins[found].planners.empty())
00970       {
00971         // no algorithms found OR this benchmark request does not use this algorithm
00972         planner_ids_to_benchmark_per_planner_interface.back() = known;
00973       }
00974       else
00975       {
00976         runs_per_planner_interface.back() =
00977             std::max<std::size_t>(1, req.plugins[found].runs);  // TODO: change it here too
00978 
00979         // loop through every planner(algorithm) in this plugin that the benchmark request desires
00980         for (std::size_t k = 0; k < req.plugins[found].planners.size(); ++k)
00981         {
00982           bool planner_found = false;
00983           for (std::size_t q = 0; q < known.size(); ++q)
00984           {
00985             // Check if the requested planner is actually in the plugin
00986             if (known[q] == req.plugins[found].planners[k] ||
00987                 motion_plan_req.group_name + "[" + req.plugins[found].planners[k] + "]" == known[q])
00988             {
00989               planner_found = true;
00990               break;
00991             }
00992           }
00993           if (planner_found)
00994             planner_ids_to_benchmark_per_planner_interface.back().push_back(req.plugins[found].planners[k]);
00995           else
00996           {
00997             ROS_ERROR("The planner id '%s' is not known to the planning interface '%s'",
00998                       req.plugins[found].planners[k].c_str(), it->first.c_str());
00999             // To help user debug, list all available planners:
01000             ROS_ERROR_STREAM("Known algorithms in " << it->first.c_str() << ":");
01001             for (std::size_t i = 0; i < known.size(); ++i)
01002             {
01003               ROS_ERROR_STREAM(" - " << known[i]);
01004             }
01005           }
01006         }
01007       }
01008 
01009       if (planner_ids_to_benchmark_per_planner_interface.back().empty())
01010         ROS_ERROR("Planning interface '%s' has no planners defined", it->first.c_str());
01011     }
01012     else
01013       ROS_WARN_STREAM("Planning interface '" << it->second->getDescription() << "' is not able to solve the specified "
01014                                                                                 "benchmark problem.");
01015   }
01016 
01017   // error check
01018   if (planner_interfaces_to_benchmark.empty())
01019   {
01020     ROS_ERROR("There are no planning interfaces to benchmark");
01021     return;
01022   }
01023 
01024   // output information about planners to be tested
01025   std::stringstream sst;
01026   for (std::size_t i = 0; i < planner_interfaces_to_benchmark.size(); ++i)
01027   {
01028     if (planner_ids_to_benchmark_per_planner_interface[i].empty())
01029       continue;
01030     sst << "  * " << planner_interfaces_to_benchmark[i]->getDescription() << " - Will execute interface "
01031         << runs_per_planner_interface[i] << " times:" << std::endl;
01032     for (std::size_t k = 0; k < planner_ids_to_benchmark_per_planner_interface[i].size(); ++k)
01033       sst << "    - " << planner_ids_to_benchmark_per_planner_interface[i][k] << std::endl;
01034     sst << std::endl;
01035   }
01036   ROS_INFO("Benchmarking Planning Interfaces:\n%s", sst.str().c_str());
01037 
01038   // configure planning context
01039   if (req.scene.robot_model_name != planning_scene_->getRobotModel()->getName())
01040   {
01041     // if we have a different robot, use the world geometry only
01042 
01043     // clear all geometry from the scene
01044     planning_scene_->getWorldNonConst()->clearObjects();
01045     planning_scene_->getCurrentStateNonConst().clearAttachedBodies();
01046     planning_scene_->getCurrentStateNonConst().setToDefaultValues();
01047 
01048     planning_scene_->processPlanningSceneWorldMsg(req.scene.world);
01049   }
01050   else
01051     planning_scene_->usePlanningSceneMsg(req.scene);
01052 
01053   // parameter sweeping functionality
01054   std::size_t n_parameter_sets = generateParamCombinations();  // this is for parameter sweeping
01055 
01056   // get stats on how many planners and total runs will be executed
01057   std::size_t total_n_planners = 0;
01058   std::size_t total_n_runs = 0;
01059   for (std::size_t i = 0; i < planner_ids_to_benchmark_per_planner_interface.size(); ++i)
01060   {
01061     total_n_planners += planner_ids_to_benchmark_per_planner_interface[i].size();
01062 
01063     // n = algorithms * runs * parameters
01064     total_n_runs +=
01065         planner_ids_to_benchmark_per_planner_interface[i].size() * runs_per_planner_interface[i] * n_parameter_sets;
01066   }
01067 
01068   // benchmark all the planners
01069   ros::WallTime startTime = ros::WallTime::now();
01070   boost::progress_display progress(total_n_runs, std::cout);
01071   std::vector<AlgorithmRunsData> data;  // holds all of the results
01072   std::vector<bool> first_solution_flag(planner_interfaces_to_benchmark.size(), true);
01073 
01074   // loop through the planning plugins
01075   for (std::size_t i = 0; i < planner_interfaces_to_benchmark.size(); ++i)
01076   {
01077     // loop through the algorithms in each plugin
01078     for (std::size_t j = 0; j < planner_ids_to_benchmark_per_planner_interface[i].size(); ++j)
01079     {
01080       motion_plan_req.planner_id = planner_ids_to_benchmark_per_planner_interface[i][j];
01081       AlgorithmRunsData runs(runs_per_planner_interface[i] * n_parameter_sets);
01082 
01083       // param tracking
01084       std::size_t param_combinations_id_ = 0;
01085 
01086       // loop through the desired parameters
01087       for (std::size_t param_count = 0; param_count < n_parameter_sets; ++param_count)
01088       {
01089         // Check if ROS is still alive
01090         if (!ros::ok())
01091           return;
01092 
01093         // Create new instance of the chosen parameters
01094         RunData parameter_data;
01095 
01096         // apply the current parameter, if we are using those
01097         if (n_parameter_sets > 1)
01098         {
01099           modifyPlannerConfiguration(*planner_interfaces_to_benchmark[i], motion_plan_req.planner_id,
01100                                      param_combinations_id_, parameter_data);
01101           ++param_combinations_id_;
01102         }
01103 
01104         planning_interface::PlanningContextPtr pcontext =
01105             planner_interfaces_to_benchmark[i]->getPlanningContext(planning_scene_, motion_plan_req);
01106 
01107         // loop through the desired number of runs
01108         for (unsigned int run_count = 0; run_count < runs_per_planner_interface[i]; ++run_count)
01109         {
01110           // Combine two for loops into one id
01111           std::size_t run_id = run_count * n_parameter_sets + param_count;
01112 
01113           ++progress;  // this outputs asterisks
01114 
01115           // run a single benchmark
01116           ROS_DEBUG("Calling %s:%s", planner_interfaces_to_benchmark[i]->getDescription().c_str(),
01117                     motion_plan_req.planner_id.c_str());
01118           planning_interface::MotionPlanDetailedResponse mp_res;
01119           ros::WallTime start = ros::WallTime::now();
01120           bool solved = pcontext->solve(mp_res);
01121           double total_time = (ros::WallTime::now() - start).toSec();
01122 
01123           // collect data
01124           start = ros::WallTime::now();
01125           runs[run_id].insert(parameter_data.begin(), parameter_data.end());  // initalize this run's data with the
01126                                                                               // chosen parameters, if we have any
01127 
01128           collectMetrics(runs[run_id], mp_res, solved, total_time);
01129           double metrics_time = (ros::WallTime::now() - start).toSec();
01130           ROS_DEBUG("Spent %lf seconds collecting metrics", metrics_time);
01131 
01132           // record the first solution in the response
01133           if (solved && first_solution_flag[i])
01134           {
01135             first_solution_flag[i] = false;
01136           }
01137         }
01138       }
01139       // this vector of runs represents all the runs*parameters
01140       data.push_back(runs);
01141 
01142     }  // end j - planning algoritms
01143   }    // end i - planning plugins
01144 
01145   double duration = (ros::WallTime::now() - startTime).toSec();
01146   std::string host = moveit_benchmarks::getHostname();
01147   std::string filename = req.filename.empty() ?
01148                              ("moveit_benchmarks_" + host + "_" +
01149                               boost::posix_time::to_iso_extended_string(startTime.toBoost()) + ".log") :
01150                              req.filename;
01151   std::ofstream out(filename.c_str());
01152   out << "Experiment " << (planning_scene_->getName().empty() ? "NO_NAME" : planning_scene_->getName()) << std::endl;
01153   out << "Running on " << (host.empty() ? "UNKNOWN" : host) << std::endl;
01154   out << "Starting at " << boost::posix_time::to_iso_extended_string(startTime.toBoost()) << std::endl;
01155   out << "Goal name " << (req.goal_name.empty() ? "UNKNOWN" : req.goal_name) << std::endl;
01156   // out << "<<<|" << std::endl << "ROS" << std::endl << req.motion_plan_request << std::endl << "|>>>" << std::endl;
01157   out << req.motion_plan_request.allowed_planning_time << " seconds per run" << std::endl;
01158   out << duration << " seconds spent to collect the data" << std::endl;
01159   out << total_n_planners << " planners" << std::endl;
01160 
01161   // tracks iteration location in data[] vector
01162   std::size_t run_id = 0;
01163 
01164   // loop through the planning *plugins*
01165   for (std::size_t q = 0; q < planner_interfaces_to_benchmark.size(); ++q)
01166   {
01167     // loop through the planning *algorithms* times the # parameter combinations
01168     for (std::size_t p = 0; p < planner_ids_to_benchmark_per_planner_interface[q].size(); ++p)
01169     {
01170       // Output name of planning algorithm
01171       out << planner_interfaces_to_benchmark[q]->getDescription() + "_" +
01172                  planner_ids_to_benchmark_per_planner_interface[q][p]
01173           << std::endl;
01174 
01175       // in general, we could have properties specific for a planner;
01176       // right now, we do not include such properties
01177       out << "0 common properties" << std::endl;
01178 
01179       // construct the list of all possible properties for all runs
01180       std::set<std::string> properties_set;
01181       for (std::size_t j = 0; j < std::size_t(data[run_id].size()); ++j)
01182       {
01183         for (RunData::const_iterator mit = data[run_id][j].begin(); mit != data[run_id][j].end(); ++mit)
01184         {
01185           properties_set.insert(mit->first);
01186         }
01187       }
01188 
01189       // copy that set to a vector of properties
01190       std::vector<std::string> properties;
01191       for (std::set<std::string>::iterator it = properties_set.begin(); it != properties_set.end(); ++it)
01192         properties.push_back(*it);
01193       out << properties.size() << " properties for each run" << std::endl;
01194 
01195       // output the vector of properties to the log file
01196       for (unsigned int j = 0; j < properties.size(); ++j)
01197         out << properties[j] << std::endl;
01198       out << data[run_id].size() << " runs" << std::endl;
01199 
01200       // output all the data to the log file
01201       for (std::size_t j = 0; j < data[run_id].size(); ++j)
01202       {
01203         for (unsigned int k = 0; k < properties.size(); ++k)
01204         {
01205           // check if this current row contains each property
01206           RunData::const_iterator it = data[run_id][j].find(properties[k]);
01207           if (it != data[run_id][j].end())
01208             out << it->second;
01209           out << "; ";
01210         }
01211 
01212         // end the line
01213         out << std::endl;
01214       }
01215       out << '.' << std::endl;
01216 
01217       ++run_id;
01218     }
01219   }
01220 
01221   out.close();
01222   ROS_INFO("Results saved to '%s'", filename.c_str());
01223 }
01224 
01225 void moveit_benchmarks::BenchmarkExecution::runGoalExistenceBenchmark(BenchmarkRequest& req)
01226 {
01227   // configure planning context
01228   if (req.scene.robot_model_name != planning_scene_->getRobotModel()->getName())
01229   {
01230     // if we have a different robot, use the world geometry only
01231     // clear all geometry from the scene
01232     planning_scene_->getWorldNonConst()->clearObjects();
01233     planning_scene_->getCurrentStateNonConst().clearAttachedBodies();
01234     planning_scene_->getCurrentStateNonConst().setToDefaultValues();
01235 
01236     planning_scene_->processPlanningSceneWorldMsg(req.scene.world);
01237     planning_scene_->setName(req.scene.name);
01238   }
01239   else
01240     planning_scene_->usePlanningSceneMsg(req.scene);
01241 
01242   // \todo the code below needs to be replaced with using constraint samplers;
01243 
01244   if (req.motion_plan_request.goal_constraints.size() == 0 &&
01245       req.motion_plan_request.goal_constraints[0].position_constraints.size() == 0 &&
01246       req.motion_plan_request.goal_constraints[0].position_constraints[0].constraint_region.primitive_poses.size() ==
01247           0 &&
01248       req.motion_plan_request.goal_constraints[0].orientation_constraints.size() == 0 &&
01249       req.motion_plan_request.trajectory_constraints.constraints.size() == 0)
01250   {
01251     ROS_ERROR("Invalid goal constraints");
01252     return;
01253   }
01254 
01255   bool success = false;
01256   bool reachable = false;
01257   if (req.motion_plan_request.goal_constraints.size() > 0 &&
01258       req.motion_plan_request.goal_constraints[0].position_constraints.size() > 0 &&
01259       req.motion_plan_request.goal_constraints[0].position_constraints[0].constraint_region.primitive_poses.size() >
01260           0 &&
01261       req.motion_plan_request.goal_constraints[0].orientation_constraints.size() > 0)
01262   {
01263     // Compute IK on goal constraints
01264     geometry_msgs::Pose ik_pose;
01265     ik_pose.position.x = req.motion_plan_request.goal_constraints[0]
01266                              .position_constraints[0]
01267                              .constraint_region.primitive_poses[0]
01268                              .position.x;
01269     ik_pose.position.y = req.motion_plan_request.goal_constraints[0]
01270                              .position_constraints[0]
01271                              .constraint_region.primitive_poses[0]
01272                              .position.y;
01273     ik_pose.position.z = req.motion_plan_request.goal_constraints[0]
01274                              .position_constraints[0]
01275                              .constraint_region.primitive_poses[0]
01276                              .position.z;
01277     ik_pose.orientation.x = req.motion_plan_request.goal_constraints[0].orientation_constraints[0].orientation.x;
01278     ik_pose.orientation.y = req.motion_plan_request.goal_constraints[0].orientation_constraints[0].orientation.y;
01279     ik_pose.orientation.z = req.motion_plan_request.goal_constraints[0].orientation_constraints[0].orientation.z;
01280     ik_pose.orientation.w = req.motion_plan_request.goal_constraints[0].orientation_constraints[0].orientation.w;
01281 
01282     robot_state::RobotState robot_state(planning_scene_->getCurrentState());
01283     robot_state::robotStateMsgToRobotState(req.motion_plan_request.start_state, robot_state);
01284 
01285     // Compute IK
01286     ROS_INFO_STREAM("Processing goal " << req.motion_plan_request.goal_constraints[0].name << " ...");
01287     ros::WallTime startTime = ros::WallTime::now();
01288     success = robot_state.setFromIK(
01289         robot_state.getJointModelGroup(req.motion_plan_request.group_name), ik_pose,
01290         req.motion_plan_request.num_planning_attempts, req.motion_plan_request.allowed_planning_time,
01291         boost::bind(&isIKSolutionCollisionFree, planning_scene_.get(), _1, _2, _3, &reachable));
01292     if (success)
01293     {
01294       ROS_INFO("  Success!");
01295     }
01296     else if (reachable)
01297     {
01298       ROS_INFO("  Reachable, but in collision");
01299     }
01300     else
01301     {
01302       ROS_INFO("  Not reachable");
01303     }
01304     // Log
01305     double duration = (ros::WallTime::now() - startTime).toSec();
01306     std::string host = moveit_benchmarks::getHostname();
01307     std::string filename = req.filename.empty() ?
01308                                ("moveit_benchmarks_" + host + "_" +
01309                                 boost::posix_time::to_iso_extended_string(startTime.toBoost()) + ".log") :
01310                                req.filename;
01311     std::ofstream out(filename.c_str());
01312     out << "Experiment " << (planning_scene_->getName().empty() ? "NO_NAME" : planning_scene_->getName()) << std::endl;
01313     out << "Running on " << (host.empty() ? "UNKNOWN" : host) << std::endl;
01314     out << "Starting at " << boost::posix_time::to_iso_extended_string(startTime.toBoost()) << std::endl;
01315     out << "<<<|" << std::endl << "ROS" << std::endl << req.motion_plan_request << std::endl << "|>>>" << std::endl;
01316     out << req.motion_plan_request.allowed_planning_time << " seconds per run" << std::endl;
01317     out << duration << " seconds spent to collect the data" << std::endl;
01318     out << "reachable BOOLEAN" << std::endl;
01319     out << "collision_free BOOLEAN" << std::endl;
01320     out << "total_time REAL" << std::endl;
01321     out << reachable << "; " << success << "; " << duration << std::endl;
01322     out.close();
01323     ROS_INFO("Results saved to '%s'", filename.c_str());
01324   }
01325 
01326   if (req.motion_plan_request.trajectory_constraints.constraints.size() > 0)
01327   {
01328     // Compute IK on trajectory constraints
01329     // Start Log
01330     ros::WallTime startTime = ros::WallTime::now();
01331     std::string host = moveit_benchmarks::getHostname();
01332     std::string filename = req.filename.empty() ?
01333                                ("moveit_benchmarks_" + host + "_" +
01334                                 boost::posix_time::to_iso_extended_string(startTime.toBoost()) + ".log") :
01335                                req.filename;
01336     std::ofstream out(filename.c_str());
01337     out << "Experiment " << (planning_scene_->getName().empty() ? "NO_NAME" : planning_scene_->getName()) << std::endl;
01338     out << "Running on " << (host.empty() ? "UNKNOWN" : host) << std::endl;
01339     out << "Starting at " << boost::posix_time::to_iso_extended_string(startTime.toBoost()) << std::endl;
01340     out << "<<<|" << std::endl << "ROS" << std::endl << req.motion_plan_request << std::endl << "|>>>" << std::endl;
01341     out << req.motion_plan_request.allowed_planning_time << " seconds per run" << std::endl;
01342     out << "reachable BOOLEAN" << std::endl;
01343     out << "collision_free BOOLEAN" << std::endl;
01344     out << "total_time REAL" << std::endl;
01345 
01346     for (std::size_t tc = 0; tc < req.motion_plan_request.trajectory_constraints.constraints.size(); ++tc)
01347     {
01348       geometry_msgs::Pose ik_pose;
01349       ik_pose.position.x = req.motion_plan_request.trajectory_constraints.constraints[tc]
01350                                .position_constraints[0]
01351                                .constraint_region.primitive_poses[0]
01352                                .position.x;
01353       ik_pose.position.y = req.motion_plan_request.trajectory_constraints.constraints[tc]
01354                                .position_constraints[0]
01355                                .constraint_region.primitive_poses[0]
01356                                .position.y;
01357       ik_pose.position.z = req.motion_plan_request.trajectory_constraints.constraints[tc]
01358                                .position_constraints[0]
01359                                .constraint_region.primitive_poses[0]
01360                                .position.z;
01361       ik_pose.orientation.x =
01362           req.motion_plan_request.trajectory_constraints.constraints[tc].orientation_constraints[0].orientation.x;
01363       ik_pose.orientation.y =
01364           req.motion_plan_request.trajectory_constraints.constraints[tc].orientation_constraints[0].orientation.y;
01365       ik_pose.orientation.z =
01366           req.motion_plan_request.trajectory_constraints.constraints[tc].orientation_constraints[0].orientation.z;
01367       ik_pose.orientation.w =
01368           req.motion_plan_request.trajectory_constraints.constraints[tc].orientation_constraints[0].orientation.w;
01369 
01370       robot_state::RobotState robot_state(planning_scene_->getCurrentState());
01371       robot_state::robotStateMsgToRobotState(req.motion_plan_request.start_state, robot_state);
01372 
01373       // Compute IK
01374       ROS_INFO_STREAM("Processing trajectory waypoint "
01375                       << req.motion_plan_request.trajectory_constraints.constraints[tc].name << " ...");
01376       startTime = ros::WallTime::now();
01377       success = robot_state.setFromIK(
01378           robot_state.getJointModelGroup(req.motion_plan_request.group_name), ik_pose,
01379           req.motion_plan_request.num_planning_attempts, req.motion_plan_request.allowed_planning_time,
01380           boost::bind(&isIKSolutionCollisionFree, planning_scene_.get(), _1, _2, _3, &reachable));
01381       double duration = (ros::WallTime::now() - startTime).toSec();
01382 
01383       if (success)
01384       {
01385         ROS_INFO("  Success!");
01386       }
01387       else if (reachable)
01388       {
01389         ROS_INFO("  Reachable, but in collision");
01390       }
01391       else
01392       {
01393         ROS_INFO("  Not reachable");
01394       }
01395 
01396       out << reachable << "; " << success << "; " << duration << std::endl;
01397     }
01398     out.close();
01399     ROS_INFO("Results saved to '%s'", filename.c_str());
01400   }
01401 }
01402 
01403 void moveit_benchmarks::BenchmarkExecution::modifyPlannerConfiguration(planning_interface::PlannerManager& planner,
01404                                                                        const std::string& planner_id,
01405                                                                        std::size_t param_combinations_id_,
01406                                                                        RunData& parameter_data)
01407 {
01408   // Get the planner's current settings
01409   planning_interface::PlannerConfigurationMap settings = planner.getPlannerConfigurations();
01410 
01411   // Check if this planner_id already has settings (it should)
01412   planning_interface::PlannerConfigurationMap::iterator settings_it = settings.find(planner_id);
01413   if (settings_it != settings.end())
01414   {
01415     // key exists, loop through all values in this param instance
01416     std::string str_parameter_value;
01417     for (std::size_t i = 0; i < param_options_.size(); ++i)
01418     {
01419       // convert from double to string
01420       try
01421       {
01422         double value = param_combinations_[param_combinations_id_][param_options_[i].key];
01423         str_parameter_value = boost::lexical_cast<std::string>(value);
01424       }
01425       catch (boost::bad_lexical_cast& ex)
01426       {
01427         ROS_WARN("%s", ex.what());
01428       }
01429 
01430       // record parameter values for logging
01431       parameter_data[param_options_[i].log_key] = str_parameter_value;
01432 
01433       // record parameter to planner config
01434       settings_it->second.config[param_options_[i].key] = str_parameter_value;
01435     }
01436   }
01437   else  // settings for this planner_id does not exist
01438   {
01439     ROS_ERROR_STREAM("Settings for " << planner_id << " do not exist. This should not happen.");
01440   }
01441 
01442   // Apply the new settings
01443   planner.setPlannerConfigurations(settings);
01444 }
01445 
01446 std::size_t moveit_benchmarks::BenchmarkExecution::generateParamCombinations()
01447 {
01448   if (!param_options_.size())
01449     return 1;  // by default there are no parameters, so the param loop runs once
01450 
01451   // Create initial param instance of all min values for the parameters
01452   ParameterInstance param_instance;
01453   for (std::size_t i = 0; i < param_options_.size(); ++i)
01454   {
01455     // start = min value
01456     param_instance[param_options_[i].key] = param_options_[i].start;
01457   }
01458 
01459   // call recusive function for every param option available
01460   int initial_options_id = 0;
01461   recursiveParamCombinations(initial_options_id, param_instance);
01462 
01463   // DEBUG
01464   /*
01465   for (std::size_t i = 0; i < param_combinations_.size(); ++i)
01466   {
01467     // Debug map
01468     for(std::map<std::string,double>::const_iterator it = param_combinations_[i].begin(); it !=
01469   param_combinations_[i].end(); ++it)
01470       std::cout << "  - " << it->first << " => " << it->second << std::endl;
01471   }
01472   */
01473 
01474   // Total number of parameters
01475   return param_combinations_.size();
01476 }
01477 
01478 void moveit_benchmarks::BenchmarkExecution::recursiveParamCombinations(int options_id, ParameterInstance param_instance)
01479 {
01480   // Get a pointer to current parameter
01481   const ParameterOptions& param_option = param_options_[options_id];
01482 
01483   do
01484   {
01485     // Call the next parameter if one exists
01486     if (param_options_.size() > options_id + 1)
01487     {
01488       recursiveParamCombinations(options_id + 1, param_instance);
01489     }
01490     else  // we are the end of the recursive call, so we can add this param_instance to the vector
01491     {
01492       param_combinations_.push_back(param_instance);
01493     }
01494 
01495     // Increment this value
01496     param_instance[param_option.key] += param_option.step_size;
01497 
01498     // Continue adding iteration amount until value equals end
01499   } while (param_instance[param_option.key] <= param_option.end + 0.00001);  // rounding issues fudge parameter
01500 
01501   return;
01502 }
01503 
01505 void moveit_benchmarks::BenchmarkExecution::printConfigurationSettings(
01506     const planning_interface::PlannerConfigurationMap& settings, std::ostream& out)
01507 {
01508   // Debug map
01509   for (planning_interface::PlannerConfigurationMap::const_iterator it = settings.begin(); it != settings.end(); ++it)
01510   {
01511     out << "  - " << it->first << " => " << it->second.name << "/" << it->second.group << std::endl;
01512     // Debug map
01513     for (std::map<std::string, std::string>::const_iterator config_it = it->second.config.begin();
01514          config_it != it->second.config.end(); ++config_it)
01515       out << "      - " << config_it->first << " => " << config_it->second << std::endl;
01516   }
01517 }


benchmarks
Author(s): Ryan Luna
autogenerated on Wed Jun 19 2019 19:24:50