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_, true))
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 }
CollisionDetectorTests::robot_state_
moveit::core::RobotStatePtr robot_state_
Definition: test_multi_threaded.cpp:93
CollisionDetectorTests::SetUp
void SetUp() override
Definition: test_multi_threaded.cpp:72
SCOPED_TRACE
#define SCOPED_TRACE(message)
TRIALS
const int TRIALS
Definition: test_multi_threaded.cpp:47
planning_scene.h
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
ASSERT_EQ
#define ASSERT_EQ(val1, val2)
collision_detection::CollisionRequest
Representation of a collision checking request.
Definition: include/moveit/collision_detection/collision_common.h:216
robot_model.h
runCollisionDetection
bool runCollisionDetection(unsigned int, unsigned int trials, const planning_scene::PlanningScene &scene, const moveit::core::RobotState &state)
Definition: test_multi_threaded.cpp:50
collision_detection::CollisionPluginCache
Definition: collision_plugin_cache.h:75
collision_common.h
planning_scene::PlanningScene
This class maintains the representation of the environment as seen by a planning instance....
Definition: planning_scene.h:122
CollisionDetectorTests::robot_model_ok_
bool robot_model_ok_
Definition: test_multi_threaded.cpp:85
CollisionDetectorTests::planning_scene_
planning_scene::PlanningScenePtr planning_scene_
Definition: test_multi_threaded.cpp:95
collision_detection::CollisionPluginCache::activate
bool activate(const std::string &name, const planning_scene::PlanningScenePtr &scene, bool exclusive)
Activate a specific collision plugin for the given planning scene instance.
Definition: collision_plugin_cache.cpp:105
runCollisionDetectionAssert
void runCollisionDetectionAssert(unsigned int id, unsigned int trials, const planning_scene::PlanningScene &scene, const moveit::core::RobotState &state, bool expected_result)
Definition: test_multi_threaded.cpp:63
planning_scene::PlanningScene::checkCollision
void checkCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
Check whether the current state is in collision, and if needed, updates the collision transforms of t...
Definition: planning_scene.cpp:494
collision_detection::CollisionResult
Representation of a collision checking result.
Definition: include/moveit/collision_detection/collision_common.h:177
INSTANTIATE_TEST_SUITE_P
#define INSTANTIATE_TEST_SUITE_P(...)
Definition: test_multi_threaded.cpp:143
moveit::core::RobotState::getRobotModel
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
Definition: robot_state.h:170
RUN_ALL_TESTS
int RUN_ALL_TESTS() GTEST_MUST_USE_RESULT_
main
int main(int argc, char **argv)
Definition: test_multi_threaded.cpp:149
collision_plugin_cache.h
CollisionDetectorTests::TearDown
void TearDown() override
Definition: test_multi_threaded.cpp:81
ASSERT_TRUE
#define ASSERT_TRUE(condition)
robot_model_test_utils.h
THREADS
const int THREADS
Definition: test_multi_threaded.cpp:48
CollisionDetectorTests::cenv_
collision_detection::CollisionEnvPtr cenv_
Definition: test_multi_threaded.cpp:89
collision_detection::CollisionResult::collision
bool collision
True if collision was found, false otherwise.
Definition: include/moveit/collision_detection/collision_common.h:200
gtest.h
CollisionDetectorTests
Definition: test_multi_threaded.cpp:69
collision_detection::CollisionResult::clear
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
Clear a previously stored result.
Definition: include/moveit/collision_detection/collision_common.h:187
CollisionDetectorTests::acm_
collision_detection::AllowedCollisionMatrixPtr acm_
Definition: test_multi_threaded.cpp:91
robot_state.h
TEST_P
TEST_P(CollisionDetectorTests, Threaded)
Tests the collision detector in multiple threads.
Definition: test_multi_threaded.cpp:99
moveit::core::loadTestingRobotModel
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
Definition: robot_model_test_utils.cpp:116
CollisionDetectorTests::robot_model_
moveit::core::RobotModelPtr robot_model_
Definition: test_multi_threaded.cpp:87


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 15 2022 02:24:55