moveit2
The MoveIt Motion Planning Framework for ROS 2.
test_multi_threaded.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, Jens Petit
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 the copyright holder 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: Jens Petit */
36 
41 #include <gtest/gtest.h>
42 #include <thread>
43 
46 
47 const int TRIALS = 1000;
48 const int THREADS = 2;
49 
50 bool runCollisionDetection(unsigned int /*id*/, unsigned int trials, const planning_scene::PlanningScene& scene,
51  const moveit::core::RobotState& state)
52 {
55  for (unsigned int i = 0; i < trials; ++i)
56  {
57  res.clear();
58  scene.checkCollision(req, res, state);
59  }
60  return res.collision;
61 }
62 
63 void runCollisionDetectionAssert(unsigned int id, unsigned int trials, const planning_scene::PlanningScene& scene,
64  const moveit::core::RobotState& state, bool expected_result)
65 {
66  ASSERT_EQ(expected_result, runCollisionDetection(id, trials, scene, state));
67 }
68 
69 class CollisionDetectorTests : public testing::TestWithParam<const char*>
70 {
71 protected:
72  void SetUp() override
73  {
75  ASSERT_TRUE(static_cast<bool>(robot_model_));
76 
77  robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
78  planning_scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
79  }
80 
81  void TearDown() override
82  {
83  }
84 
86 
87  moveit::core::RobotModelPtr robot_model_;
88 
89  collision_detection::CollisionEnvPtr cenv_;
90 
91  collision_detection::AllowedCollisionMatrixPtr acm_;
92 
93  moveit::core::RobotStatePtr robot_state_;
94 
95  planning_scene::PlanningScenePtr planning_scene_;
96 };
97 
100 {
101  const std::string plugin_name = GetParam();
102  SCOPED_TRACE(plugin_name);
103 
104  std::vector<moveit::core::RobotState> states;
105  std::vector<std::thread> threads;
106  std::vector<bool> collisions;
107 
109  if (!loader.activate(plugin_name, planning_scene_))
110  {
111 #if defined(GTEST_SKIP_)
112  GTEST_SKIP_("Failed to load collision plugin");
113 #else
114  return;
115 #endif
116  }
117 
118  for (unsigned int i = 0; i < THREADS; ++i)
119  {
120  moveit::core::RobotState state{ planning_scene_->getRobotModel() };
122  state.setToRandomPositions();
123  state.update();
124  collisions.push_back(runCollisionDetection(0, 1, *planning_scene_, state));
125  states.push_back(std::move(state));
126  }
127 
128  for (unsigned int i = 0; i < THREADS; ++i)
129  threads.emplace_back(std::thread([i, &scene = *planning_scene_, &state = states[i], expected = collisions[i]] {
130  return runCollisionDetectionAssert(i, TRIALS, scene, state, expected);
131  }));
132 
133  for (unsigned int i = 0; i < states.size(); ++i)
134  {
135  threads[i].join();
136  }
137  threads.clear();
138 
139  planning_scene_.reset();
140 }
141 
142 #ifndef INSTANTIATE_TEST_SUITE_P // prior to gtest 1.10
143 #define INSTANTIATE_TEST_SUITE_P(...) INSTANTIATE_TEST_CASE_P(__VA_ARGS__)
144 #endif
145 
146 // instantiate parameterized tests for common collision plugins
147 INSTANTIATE_TEST_SUITE_P(PluginTests, CollisionDetectorTests, testing::Values("FCL", "Bullet"));
148 
149 int main(int argc, char** argv)
150 {
151  ::testing::InitGoogleTest(&argc, argv);
152  return RUN_ALL_TESTS();
153 }
collision_detection::CollisionEnvPtr cenv_
moveit::core::RobotModelPtr robot_model_
planning_scene::PlanningScenePtr planning_scene_
moveit::core::RobotStatePtr robot_state_
collision_detection::AllowedCollisionMatrixPtr acm_
bool activate(const std::string &name, const planning_scene::PlanningScenePtr &scene)
Activate a specific collision plugin for the given planning scene instance.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
Definition: robot_state.h:104
This class maintains the representation of the environment as seen by a planning instance....
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
scene
Definition: pick.py:52
Representation of a collision checking request.
Representation of a collision checking result.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
Clear a previously stored result.
bool collision
True if collision was found, false otherwise.
int main(int argc, char **argv)
#define INSTANTIATE_TEST_SUITE_P(...)
bool runCollisionDetection(unsigned int, unsigned int trials, const planning_scene::PlanningScene &scene, const moveit::core::RobotState &state)
void runCollisionDetectionAssert(unsigned int id, unsigned int trials, const planning_scene::PlanningScene &scene, const moveit::core::RobotState &state, bool expected_result)
const int THREADS
TEST_P(CollisionDetectorTests, Threaded)
Tests the collision detector in multiple threads.
const int TRIALS