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


benchmarks
Author(s): Ioan Sucan
autogenerated on Wed Aug 26 2015 12:43:51