unittest_brake_test_utils.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2019 Pilz GmbH & Co. KG
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8 
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13 
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
18 #include <atomic>
19 #include <algorithm>
20 #include <mutex>
21 #include <thread>
22 #include <memory>
23 
24 #include <gtest/gtest.h>
25 
26 #include <ros/ros.h>
27 #include <sensor_msgs/JointState.h>
28 
29 #include <pilz_testutils/joint_state_publisher_mock.h>
31 
34 
36 {
37 using namespace prbt_hardware_support;
38 using sensor_msgs::JointState;
39 using sensor_msgs::JointStateConstPtr;
40 using sensor_msgs::JointStatePtr;
41 
42 static const std::string JOINT_STATES_TOPIC_NAME{ "/prbt/joint_states" };
43 
47 static ::testing::AssertionResult compareJointStateMessages(const JointStateConstPtr& msg1,
48  const JointStateConstPtr& msg2)
49 {
50  if (msg1->name.size() != msg2->name.size())
51  {
52  return ::testing::AssertionFailure() << "Joint numbers in joint state messages do not match.";
53  }
54  if (!std::equal(msg1->name.begin(), msg1->name.end(), msg2->name.begin(),
55  [](std::string name1, std::string name2) { return name1 == name2; }))
56  {
57  return ::testing::AssertionFailure() << "Joint names in joint state messages do not match.";
58  }
59  if (msg1->position.size() != msg2->position.size())
60  {
61  return ::testing::AssertionFailure() << "Joint numbers in joint state messages do not match.";
62  }
64  {
65  return ::testing::AssertionFailure() << "Joint positions in joint state messages do not match.";
66  }
67 
68  return ::testing::AssertionSuccess();
69 }
70 
75 TEST(BrakeTestUtilsTest, testExceptionDtor)
76 {
77  {
78  std::shared_ptr<BrakeTestUtilsException> ex{ new BrakeTestUtilsException("Test msg") };
79  }
80 
81  {
82  std::shared_ptr<GetCurrentJointStatesException> ex{ new GetCurrentJointStatesException("Test msg") };
83  }
84 }
85 
100 TEST(BrakeTestUtilsTest, testCompareJointStatePositions)
101 {
102  /**********
103  * Step 1 *
104  **********/
105  JointStatePtr msg1{ boost::make_shared<JointState>() };
106  msg1->name = { "joint1", "joint2" };
107  msg1->position = { 0.1, 0.11 };
108 
109  JointStatePtr msg2{ boost::make_shared<JointState>() };
110  msg2->name = { "joint1", "joint2" };
111  msg2->position = { 0.1, 0.11 };
112 
113  EXPECT_TRUE(BrakeTestUtils::compareJointStatePositions(msg1, msg2));
114 
115  /**********
116  * Step 2 *
117  **********/
118  double tolerance = 0.0001;
119  for (double& pos : msg2->position)
120  {
121  pos += 0.9 * tolerance;
122  }
123 
124  EXPECT_TRUE(BrakeTestUtils::compareJointStatePositions(msg1, msg2, tolerance));
125 
126  /**********
127  * Step 3 *
128  **********/
129  for (size_t i = 0; i < msg2->position.size(); ++i)
130  {
131  for (size_t j = 0; j < msg2->position.size(); ++j)
132  {
133  msg2->position[j] = msg1->position[j];
134  }
135  msg2->position[i] += 1.1 * tolerance;
136 
137  EXPECT_FALSE(BrakeTestUtils::compareJointStatePositions(msg1, msg2, tolerance));
138  }
139 }
140 
152 TEST(BrakeTestUtilsTest, testGetCurrentJointStates)
153 {
154  /**********
155  * Step 1 *
156  **********/
158 
159  /**********
160  * Step 2 *
161  **********/
162  pilz_testutils::JointStatePublisherMock joint_states_pub;
163  const double joint1_start_position{ 0.92 };
164  joint_states_pub.startPublishingAsync(joint1_start_position);
165 
166  pilz_utils::waitForMessage<JointState>(JOINT_STATES_TOPIC_NAME);
167 
168  auto expected_msg = joint_states_pub.getNextMessage();
169 
170  try
171  {
173  EXPECT_TRUE(compareJointStateMessages(msg, expected_msg));
174  }
175  catch (const std::exception& e)
176  {
177  ADD_FAILURE() << e.what();
178  }
179 
180  joint_states_pub.stopPublishing();
181 }
182 
194 TEST(BrakeTestUtilsTest, testDetectRobotMotion)
195 {
196  /**********
197  * Step 1 *
198  **********/
199  pilz_testutils::JointStatePublisherMock joint_states_pub;
200  joint_states_pub.startPublishingAsync();
201 
202  pilz_utils::waitForMessage<JointState>(JOINT_STATES_TOPIC_NAME);
203 
204  EXPECT_FALSE(BrakeTestUtils::detectRobotMotion());
205 
206  /**********
207  * Step 2 *
208  **********/
209  joint_states_pub.setJoint1Velocity(0.1);
210 
211  EXPECT_TRUE(BrakeTestUtils::detectRobotMotion());
212 
213  joint_states_pub.stopPublishing();
214 }
215 
216 } // namespace brake_test_utils_test
217 
218 int main(int argc, char* argv[])
219 {
220  ros::init(argc, argv, "unittest_brake_test_utils");
221  ros::NodeHandle nh;
222 
223  testing::InitGoogleTest(&argc, argv);
224  return RUN_ALL_TESTS();
225 }
msg
int main(int argc, char *argv[])
static bool detectRobotMotion(double timeout_s=DEFAULT_ROBOT_MOTION_TIMEOUT_S)
return true if a robot motion was detected, false otherwise.
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
::testing::AssertionResult compareJointStateMessages(const JointStateConstPtr &msg1, const JointStateConstPtr &msg2)
Checks for identical names and positions in joint state messages.
static sensor_msgs::JointStateConstPtr getCurrentJointStates()
wait for a single message on the joint_states topic and return it.
Expection thrown by BrakeTestUtils::getCurrentJointStates().
TEST(BrakeTestUtilsTest, testExceptionDtor)
Test increases function coverage by ensuring that all Dtor variants are called.
Exception thrown by a BrakeTestUtils function.
static const std::string JOINT_STATES_TOPIC_NAME
static bool compareJointStatePositions(const sensor_msgs::JointStateConstPtr &msg1, const sensor_msgs::JointStateConstPtr &msg2, const double tol=DEFAULT_JOINT_STATES_COMPARISON_TOLERANCE)
return true if the joint state positions are equal up to a given tolerance, false otherwise...


prbt_hardware_support
Author(s):
autogenerated on Mon Feb 28 2022 23:14:34