moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 
37 #include <chrono>
39 #include <boost/program_options/parsers.hpp>
40 #include <boost/program_options/variables_map.hpp>
41 #include <thread>
42 
43 using namespace std::chrono_literals;
44 
45 static const std::string ROBOT_DESCRIPTION = "robot_description";
46 
47 static const rclcpp::Logger LOGGER = rclcpp::get_logger("evaluate_collision_checking_speed");
48 
49 void runCollisionDetection(unsigned int id, unsigned int trials, const planning_scene::PlanningScene& scene,
50  const moveit::core::RobotState& state)
51 {
52  RCLCPP_INFO(LOGGER, "Starting thread %u", id);
53  rclcpp::Clock clock(RCL_ROS_TIME);
55  rclcpp::Time start = clock.now();
56  for (unsigned int i = 0; i < trials; ++i)
57  {
59  scene.checkCollision(req, res, state);
60  }
61  double duration = (clock.now() - start).seconds();
62  RCLCPP_INFO(LOGGER, "Thread %u performed %lf collision checks per second", id, (double)trials / duration);
63 }
64 
65 int main(int argc, char** argv)
66 {
67  rclcpp::init(argc, argv);
68  auto node = rclcpp::Node::make_shared("evaluate_collision_checking_speed");
69 
70  unsigned int nthreads = 2;
71  unsigned int trials = 10000;
72  boost::program_options::options_description desc;
73  desc.add_options()("nthreads", boost::program_options::value<unsigned int>(&nthreads)->default_value(nthreads),
74  "Number of threads to use")(
75  "trials", boost::program_options::value<unsigned int>(&trials)->default_value(trials),
76  "Number of collision checks to perform with each thread")("wait",
77  "Wait for a user command (so the planning scene can be "
78  "updated in the background)")("help", "this screen");
79  boost::program_options::variables_map vm;
80  boost::program_options::parsed_options po = boost::program_options::parse_command_line(argc, argv, desc);
81  boost::program_options::store(po, vm);
82  boost::program_options::notify(vm);
83 
84  if (vm.count("help"))
85  {
86  std::cout << desc << '\n';
87  return 0;
88  }
89 
90  rclcpp::executors::MultiThreadedExecutor executor;
91  executor.add_node(node);
92 
93  planning_scene_monitor::PlanningSceneMonitor psm(node, ROBOT_DESCRIPTION);
94  if (psm.getPlanningScene())
95  {
96  if (vm.count("wait"))
97  {
99  psm.startSceneMonitor();
100  std::cout << "Listening to planning scene updates. Press Enter to continue ..." << '\n';
101  std::cin.get();
102  }
103  else
104  rclcpp::sleep_for(500ms);
105 
106  std::vector<moveit::core::RobotStatePtr> states;
107  RCLCPP_INFO(LOGGER, "Sampling %u valid states...", nthreads);
108  for (unsigned int i = 0; i < nthreads; ++i)
109  {
110  // sample a valid state
111  moveit::core::RobotState* state = new moveit::core::RobotState(psm.getPlanningScene()->getRobotModel());
113  do
114  {
115  state->setToRandomPositions();
116  state->update();
118  psm.getPlanningScene()->checkCollision(req, res);
119  if (!res.collision)
120  break;
121  } while (true);
122  states.push_back(moveit::core::RobotStatePtr(state));
123  }
124 
125  std::vector<std::thread*> threads;
126  runCollisionDetection(10, trials, *psm.getPlanningScene(), *states[0]);
127  for (unsigned int i = 0; i < states.size(); ++i)
128  threads.push_back(new std::thread([i, trials, &scene = *psm.getPlanningScene(), &state = *states[i]] {
129  return runCollisionDetection(i, trials, scene, state);
130  }));
131 
132  for (unsigned int i = 0; i < states.size(); ++i)
133  {
134  threads[i]->join();
135  delete threads[i];
136  }
137  }
138  else
139  RCLCPP_ERROR(LOGGER, "Planning scene not configured");
140 
141  return 0;
142 }
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
void update(bool force=false)
Update all transforms.
This class maintains the representation of the environment as seen by a planning instance....
PlanningSceneMonitor Subscribes to the topic planning_scene.
const planning_scene::PlanningScenePtr & getPlanningScene()
Avoid this function! Returns an unsafe pointer to the current planning scene.
void startSceneMonitor(const std::string &scene_topic=DEFAULT_PLANNING_SCENE_TOPIC)
Start the scene monitor (ROS topic-based)
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 the OccupancyMapMonitor and listening for:
void runCollisionDetection(unsigned int id, unsigned int trials, const planning_scene::PlanningScene &scene, const moveit::core::RobotState &state)
int main(int argc, char **argv)
scene
Definition: pick.py:52
const rclcpp::Logger LOGGER
Definition: async_test.h:31
Representation of a collision checking request.
Representation of a collision checking result.
bool collision
True if collision was found, false otherwise.