evaluate_collision_checking_speed.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan, Sachin Chitta */
36 
38 #include <boost/program_options/parsers.hpp>
39 #include <boost/program_options/variables_map.hpp>
40 
41 static const std::string ROBOT_DESCRIPTION = "robot_description";
42 
43 void runCollisionDetection(unsigned int id, unsigned int trials, const planning_scene::PlanningScene* scene,
44  const robot_state::RobotState* state)
45 {
46  ROS_INFO("Starting thread %u", id);
49  for (unsigned int i = 0; i < trials; ++i)
50  {
52  scene->checkCollision(req, res, *state);
53  }
54  double duration = (ros::WallTime::now() - start).toSec();
55  ROS_INFO("Thread %u performed %lf collision checks per second", id, (double)trials / duration);
56 }
57 
58 int main(int argc, char** argv)
59 {
60  ros::init(argc, argv, "evaluate_collision_checking_speed");
61 
62  unsigned int nthreads = 2;
63  unsigned int trials = 10000;
64  boost::program_options::options_description desc;
65  desc.add_options()("nthreads", boost::program_options::value<unsigned int>(&nthreads)->default_value(nthreads),
66  "Number of threads to use")(
67  "trials", boost::program_options::value<unsigned int>(&trials)->default_value(trials),
68  "Number of collision checks to perform with each thread")("wait",
69  "Wait for a user command (so the planning scene can be "
70  "updated in thre background)")("help", "this screen");
71  boost::program_options::variables_map vm;
72  boost::program_options::parsed_options po = boost::program_options::parse_command_line(argc, argv, desc);
73  boost::program_options::store(po, vm);
74  boost::program_options::notify(vm);
75 
76  if (vm.count("help"))
77  {
78  std::cout << desc << std::endl;
79  return 0;
80  }
81 
83  spinner.start();
84 
86  if (psm.getPlanningScene())
87  {
88  if (vm.count("wait"))
89  {
91  psm.startSceneMonitor();
92  std::cout << "Listening to planning scene updates. Press Enter to continue ..." << std::endl;
93  std::cin.get();
94  }
95  else
96  ros::Duration(0.5).sleep();
97 
98  std::vector<robot_state::RobotStatePtr> states;
99  ROS_INFO("Sampling %u valid states...", nthreads);
100  for (unsigned int i = 0; i < nthreads; ++i)
101  {
102  // sample a valid state
103  robot_state::RobotState* state = new robot_state::RobotState(psm.getPlanningScene()->getRobotModel());
105  do
106  {
107  state->setToRandomPositions();
108  state->update();
110  psm.getPlanningScene()->checkCollision(req, res);
111  if (!res.collision)
112  break;
113  } while (true);
114  states.push_back(robot_state::RobotStatePtr(state));
115  }
116 
117  std::vector<boost::thread*> threads;
118 
119  for (unsigned int i = 0; i < states.size(); ++i)
120  threads.push_back(new boost::thread(
121  boost::bind(&runCollisionDetection, i, trials, psm.getPlanningScene().get(), states[i].get())));
122 
123  for (unsigned int i = 0; i < states.size(); ++i)
124  {
125  threads[i]->join();
126  delete threads[i];
127  }
128  }
129  else
130  ROS_ERROR("Planning scene not configured");
131 
132  return 0;
133 }
void startSceneMonitor(const std::string &scene_topic=DEFAULT_PLANNING_SCENE_TOPIC)
Start the scene monitor.
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void spinner()
const planning_scene::PlanningScenePtr & getPlanningScene()
Avoid this function! Returns an unsafe pointer to the current planning scene.
static const std::string ROBOT_DESCRIPTION
void startWorldGeometryMonitor(const std::string &collision_objects_topic=DEFAULT_COLLISION_OBJECT_TOPIC, const std::string &planning_scene_world_topic=DEFAULT_PLANNING_SCENE_WORLD_TOPIC, const bool load_octomap_monitor=true)
Start listening for objects in the world, the collision map and attached collision objects...
#define ROS_INFO(...)
PlanningSceneMonitor Subscribes to the topic planning_scene.
def default_value(type)
void checkCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
int main(int argc, char **argv)
void runCollisionDetection(unsigned int id, unsigned int trials, const planning_scene::PlanningScene *scene, const robot_state::RobotState *state)
static WallTime now()
#define ROS_ERROR(...)


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jul 10 2019 04:03:32