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


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Oct 6 2014 02:31:39