24 #include <gtest/gtest.h> 27 #include <sensor_msgs/JointState.h> 29 #include <pilz_testutils/joint_state_publisher_mock.h> 38 using sensor_msgs::JointState;
39 using sensor_msgs::JointStateConstPtr;
40 using sensor_msgs::JointStatePtr;
48 const JointStateConstPtr& msg2)
50 if (msg1->name.size() != msg2->name.size())
52 return ::testing::AssertionFailure() <<
"Joint numbers in joint state messages do not match.";
54 if (!std::equal(msg1->name.begin(), msg1->name.end(), msg2->name.begin(),
55 [](std::string name1, std::string name2) {
return name1 == name2; }))
57 return ::testing::AssertionFailure() <<
"Joint names in joint state messages do not match.";
59 if (msg1->position.size() != msg2->position.size())
61 return ::testing::AssertionFailure() <<
"Joint numbers in joint state messages do not match.";
65 return ::testing::AssertionFailure() <<
"Joint positions in joint state messages do not match.";
68 return ::testing::AssertionSuccess();
75 TEST(BrakeTestUtilsTest, testExceptionDtor)
100 TEST(BrakeTestUtilsTest, testCompareJointStatePositions)
105 JointStatePtr msg1{ boost::make_shared<JointState>() };
106 msg1->name = {
"joint1",
"joint2" };
107 msg1->position = { 0.1, 0.11 };
109 JointStatePtr msg2{ boost::make_shared<JointState>() };
110 msg2->name = {
"joint1",
"joint2" };
111 msg2->position = { 0.1, 0.11 };
118 double tolerance = 0.0001;
119 for (
double& pos : msg2->position)
121 pos += 0.9 * tolerance;
129 for (
size_t i = 0; i < msg2->position.size(); ++i)
131 for (
size_t j = 0; j < msg2->position.size(); ++j)
133 msg2->position[j] = msg1->position[j];
135 msg2->position[i] += 1.1 * tolerance;
152 TEST(BrakeTestUtilsTest, testGetCurrentJointStates)
162 pilz_testutils::JointStatePublisherMock joint_states_pub;
163 const double joint1_start_position{ 0.92 };
164 joint_states_pub.startPublishingAsync(joint1_start_position);
168 auto expected_msg = joint_states_pub.getNextMessage();
175 catch (
const std::exception& e)
177 ADD_FAILURE() << e.what();
180 joint_states_pub.stopPublishing();
194 TEST(BrakeTestUtilsTest, testDetectRobotMotion)
199 pilz_testutils::JointStatePublisherMock joint_states_pub;
200 joint_states_pub.startPublishingAsync();
209 joint_states_pub.setJoint1Velocity(0.1);
213 joint_states_pub.stopPublishing();
218 int main(
int argc,
char* argv[])
220 ros::init(argc, argv,
"unittest_brake_test_utils");
223 testing::InitGoogleTest(&argc, argv);
224 return RUN_ALL_TESTS();
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...