moveit2
The MoveIt Motion Planning Framework for ROS 2.
unittest_planning_context_loaders.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018 Pilz GmbH & Co. KG
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 Pilz GmbH & Co. KG 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 #include <gtest/gtest.h>
36 
37 #include <pluginlib/class_loader.hpp>
38 
41 
43 
44 #include "test_utils.h"
45 
46 #include <rclcpp/rclcpp.hpp>
47 
48 static const rclcpp::Logger LOGGER = rclcpp::get_logger("unittest_planning_context_loader");
49 
50 class PlanningContextLoadersTest : public ::testing::TestWithParam<std::vector<std::string>>
51 {
52 protected:
59  void SetUp() override
60  {
61  rclcpp::NodeOptions node_options;
62  node_options.automatically_declare_parameters_from_overrides(true);
63  node_ = rclcpp::Node::make_shared("unittest_planning_context_loader", node_options);
64 
65  // load robot model
66  rm_loader_ = std::make_unique<robot_model_loader::RobotModelLoader>(node_);
67  robot_model_ = rm_loader_->getModel();
68  ASSERT_FALSE(robot_model_ == nullptr) << "There is no robot model!";
69 
70  // Load the plugin
71  try
72  {
74  std::make_unique<pluginlib::ClassLoader<pilz_industrial_motion_planner::PlanningContextLoader>>(
75  "pilz_industrial_motion_planner", "pilz_industrial_motion_planner::PlanningContextLoader");
76  }
77  catch (pluginlib::PluginlibException& ex)
78  {
79  RCLCPP_FATAL_STREAM(LOGGER, "Exception while creating planning context loader " << ex.what());
80  FAIL();
81  }
82 
83  // Create planning context loader ptp
84  try
85  {
86  planning_context_loader_.reset(planning_context_loader_class_loader_->createUnmanagedInstance(GetParam().front()));
87  }
88  catch (pluginlib::PluginlibException& ex)
89  {
90  FAIL() << ex.what();
91  }
92  return;
93  }
94 
95  void TearDown() override
96  {
98  {
99  planning_context_loader_class_loader_->unloadLibraryForClass(GetParam().front());
100  }
101  robot_model_.reset();
102  }
103 
104 protected:
105  rclcpp::Node::SharedPtr node_;
106  moveit::core::RobotModelConstPtr robot_model_;
107  std::unique_ptr<robot_model_loader::RobotModelLoader> rm_loader_;
108 
109  // Load the plugin
110  std::unique_ptr<pluginlib::ClassLoader<pilz_industrial_motion_planner::PlanningContextLoader>>
112 
114 };
115 
116 // Instantiate the test cases for all loaders, extend here if you added a new
117 // ContextLoader you want to test
119  InstantiationName, PlanningContextLoadersTest,
120  ::testing::Values(
121  std::vector<std::string>{ "pilz_industrial_motion_planner/PlanningContextLoaderPTP", "PTP" }, // Test for PTP
122  std::vector<std::string>{ "pilz_industrial_motion_planner/PlanningContextLoaderLIN", "LIN" }, // Test for LIN
123  std::vector<std::string>{ "pilz_industrial_motion_planner/PlanningContextLoaderCIRC", "CIRC" } // Test for CIRC
124  ));
125 
130 {
131  std::string alg = planning_context_loader_->getAlgorithm();
132  EXPECT_EQ(alg, GetParam().back());
133 }
134 
139 {
140  planning_interface::PlanningContextPtr planning_context;
141  const std::string& group_name = "manipulator";
142 
143  // Without limits should return false
144  bool res = planning_context_loader_->loadContext(planning_context, "test", group_name);
145  EXPECT_EQ(false, res) << "Context returned even when no limits where set";
146 
147  // After setting the limits this should work
149  testutils::createFakeLimits(robot_model_->getVariableNames());
153  cart_limits.setMaxRotationalVelocity(1 * M_PI);
154  cart_limits.setMaxTranslationalAcceleration(2);
155  cart_limits.setMaxTranslationalDeceleration(2);
156  cart_limits.setMaxTranslationalVelocity(1);
157  limits.setCartesianLimits(cart_limits);
158 
159  planning_context_loader_->setLimits(limits);
160  planning_context_loader_->setModel(robot_model_);
161 
162  try
163  {
164  res = planning_context_loader_->loadContext(planning_context, "test", group_name);
165  }
166  catch (std::exception& ex)
167  {
168  FAIL() << "Exception!" << ex.what() << " " << typeid(ex).name();
169  }
170 
171  EXPECT_EQ(true, res) << "Context could not be loaded!";
172 }
173 
174 int main(int argc, char** argv)
175 {
176  rclcpp::init(argc, argv);
177  testing::InitGoogleTest(&argc, argv);
178  return RUN_ALL_TESTS();
179 }
std::unique_ptr< pluginlib::ClassLoader< pilz_industrial_motion_planner::PlanningContextLoader > > planning_context_loader_class_loader_
std::unique_ptr< robot_model_loader::RobotModelLoader > rm_loader_
pilz_industrial_motion_planner::PlanningContextLoaderPtr planning_context_loader_
void SetUp() override
To initialize the planning context loader The planning context loader is loaded using the pluginlib....
moveit::core::RobotModelConstPtr robot_model_
Set of cartesian limits, has values for velocity, acceleration and deceleration of both the translati...
void setMaxRotationalVelocity(double max_rot_vel)
Set the maximum rotational velocity.
void setMaxTranslationalVelocity(double max_trans_vel)
Set the maximal translational velocity.
void setMaxTranslationalAcceleration(double max_trans_acc)
Set the maximum translational acceleration.
void setMaxTranslationalDeceleration(double max_trans_dec)
Set the maximum translational deceleration.
Container for JointLimits, essentially a map with convenience functions. Adds the ability to as for l...
This class combines CartesianLimit and JointLimits into on single class.
void setCartesianLimits(CartesianLimit &cartesian_limit)
Set cartesian limits.
void setJointLimits(JointLimitsContainer &joint_limits)
Set joint limits.
std::shared_ptr< PlanningContextLoader > PlanningContextLoaderPtr
name
Definition: setup.py:7
pilz_industrial_motion_planner::JointLimitsContainer createFakeLimits(const std::vector< std::string > &joint_names)
Create limits for tests to avoid the need to get the limits from the node parameter.
Definition: test_utils.cpp:48
TEST_P(PlanningContextLoadersTest, GetAlgorithm)
Test getAlgorithm returns PTP.
int main(int argc, char **argv)
INSTANTIATE_TEST_SUITE_P(InstantiationName, PlanningContextLoadersTest, ::testing::Values(std::vector< std::string >{ "pilz_industrial_motion_planner/PlanningContextLoaderPTP", "PTP" }, std::vector< std::string >{ "pilz_industrial_motion_planner/PlanningContextLoaderLIN", "LIN" }, std::vector< std::string >{ "pilz_industrial_motion_planner/PlanningContextLoaderCIRC", "CIRC" }))