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