evaluate_collision_checking_speed.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, Sachin Chitta */
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       // sample a valid state
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 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jan 17 2018 03:32:07