Go to the documentation of this file.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/planning_scene_monitor/planning_scene_monitor.h>
00038 #include <boost/program_options/parsers.hpp>
00039 #include <boost/program_options/variables_map.hpp>
00040
00041 static const std::string ROBOT_DESCRIPTION = "robot_description";
00042
00043 void runCollisionDetection(unsigned int id, unsigned int trials, const planning_scene::PlanningScene* scene,
00044 const robot_state::RobotState* state)
00045 {
00046 ROS_INFO("Starting thread %u", id);
00047 collision_detection::CollisionRequest req;
00048 ros::WallTime start = ros::WallTime::now();
00049 for (unsigned int i = 0; i < trials; ++i)
00050 {
00051 collision_detection::CollisionResult res;
00052 scene->checkCollision(req, res, *state);
00053 }
00054 double duration = (ros::WallTime::now() - start).toSec();
00055 ROS_INFO("Thread %u performed %lf collision checks per second", id, (double)trials / duration);
00056 }
00057
00058 int main(int argc, char** argv)
00059 {
00060 ros::init(argc, argv, "evaluate_collision_checking_speed");
00061
00062 unsigned int nthreads = 2;
00063 unsigned int trials = 10000;
00064 boost::program_options::options_description desc;
00065 desc.add_options()("nthreads", boost::program_options::value<unsigned int>(&nthreads)->default_value(nthreads),
00066 "Number of threads to use")(
00067 "trials", boost::program_options::value<unsigned int>(&trials)->default_value(trials),
00068 "Number of collision checks to perform with each thread")("wait",
00069 "Wait for a user command (so the planning scene can be "
00070 "updated in thre background)")("help", "this screen");
00071 boost::program_options::variables_map vm;
00072 boost::program_options::parsed_options po = boost::program_options::parse_command_line(argc, argv, desc);
00073 boost::program_options::store(po, vm);
00074 boost::program_options::notify(vm);
00075
00076 if (vm.count("help"))
00077 {
00078 std::cout << desc << std::endl;
00079 return 0;
00080 }
00081
00082 ros::AsyncSpinner spinner(1);
00083 spinner.start();
00084
00085 planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION);
00086 if (psm.getPlanningScene())
00087 {
00088 if (vm.count("wait"))
00089 {
00090 psm.startWorldGeometryMonitor();
00091 psm.startSceneMonitor();
00092 std::cout << "Listening to planning scene updates. Press Enter to continue ..." << std::endl;
00093 std::cin.get();
00094 }
00095 else
00096 ros::Duration(0.5).sleep();
00097
00098 std::vector<robot_state::RobotStatePtr> states;
00099 ROS_INFO("Sampling %u valid states...", nthreads);
00100 for (unsigned int i = 0; i < nthreads; ++i)
00101 {
00102
00103 robot_state::RobotState* state = new robot_state::RobotState(psm.getPlanningScene()->getRobotModel());
00104 collision_detection::CollisionRequest req;
00105 do
00106 {
00107 state->setToRandomPositions();
00108 state->update();
00109 collision_detection::CollisionResult res;
00110 psm.getPlanningScene()->checkCollision(req, res);
00111 if (!res.collision)
00112 break;
00113 } while (true);
00114 states.push_back(robot_state::RobotStatePtr(state));
00115 }
00116
00117 std::vector<boost::thread*> threads;
00118
00119 for (unsigned int i = 0; i < states.size(); ++i)
00120 threads.push_back(new boost::thread(
00121 boost::bind(&runCollisionDetection, i, trials, psm.getPlanningScene().get(), states[i].get())));
00122
00123 for (unsigned int i = 0; i < states.size(); ++i)
00124 {
00125 threads[i]->join();
00126 delete threads[i];
00127 }
00128 }
00129 else
00130 ROS_ERROR("Planning scene not configured");
00131
00132 return 0;
00133 }