brake_test_utils.h
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 #ifndef PRBT_HARDWARE_SUPPORT_BRAKE_TEST_UTILS_H
19 #define PRBT_HARDWARE_SUPPORT_BRAKE_TEST_UTILS_H
20 
21 #include <math.h>
22 #include <algorithm>
23 
24 #include <ros/ros.h>
25 #include <sensor_msgs/JointState.h>
26 
28 
29 static const std::string TOPIC_NAME{ "/prbt/joint_states" };
30 static constexpr double JOINT_STATES_COMPARISON_FREQUENCY{ 10.0 };
31 static constexpr double JOINT_STATES_TOPIC_TIMEOUT{ 1.0 };
32 static constexpr double DEFAULT_JOINT_STATES_COMPARISON_TOLERANCE{ 0.001 };
33 static constexpr double DEFAULT_ROBOT_MOTION_TIMEOUT_S{ 1.0 };
34 
35 namespace prbt_hardware_support
36 {
38 {
39 public:
46  static bool detectRobotMotion(double timeout_s = DEFAULT_ROBOT_MOTION_TIMEOUT_S);
47 
53  inline static sensor_msgs::JointStateConstPtr getCurrentJointStates();
54 
58  inline static bool compareJointStatePositions(const sensor_msgs::JointStateConstPtr& msg1,
59  const sensor_msgs::JointStateConstPtr& msg2,
61 };
62 
63 bool BrakeTestUtils::detectRobotMotion(double timeout_s)
64 {
65  auto msg_start{ getCurrentJointStates() };
66 
69  while ((ros::Time::now() - start).toSec() < timeout_s && ros::ok())
70  {
71  auto msg_current{ getCurrentJointStates() };
72  if (!compareJointStatePositions(msg_start, msg_current))
73  {
74  return true;
75  }
76  r.sleep();
77  }
78 
79  return false;
80 }
81 
82 sensor_msgs::JointStateConstPtr BrakeTestUtils::getCurrentJointStates()
83 {
84  auto msg{ ros::topic::waitForMessage<sensor_msgs::JointState>(TOPIC_NAME,
86  if (msg == nullptr)
87  {
88  throw GetCurrentJointStatesException("Could not obtain message from joint_states topic.");
89  }
90  return msg;
91 }
92 
93 bool BrakeTestUtils::compareJointStatePositions(const sensor_msgs::JointStateConstPtr& msg1,
94  const sensor_msgs::JointStateConstPtr& msg2, const double tol)
95 {
96  return std::equal(msg1->position.begin(), msg1->position.end(), msg2->position.begin(),
97  [tol](double pos1, double pos2) { return (fabs(pos1 - pos2) < tol); });
98 }
99 
100 } // namespace prbt_hardware_support
101 
102 #endif // PRBT_HARDWARE_SUPPORT_BRAKE_TEST_UTILS_H
msg
ROSCPP_DECL void start()
static bool detectRobotMotion(double timeout_s=DEFAULT_ROBOT_MOTION_TIMEOUT_S)
return true if a robot motion was detected, false otherwise.
static sensor_msgs::JointStateConstPtr getCurrentJointStates()
wait for a single message on the joint_states topic and return it.
static constexpr double DEFAULT_ROBOT_MOTION_TIMEOUT_S
Expection thrown by BrakeTestUtils::getCurrentJointStates().
static constexpr double DEFAULT_JOINT_STATES_COMPARISON_TOLERANCE
static constexpr double JOINT_STATES_COMPARISON_FREQUENCY
static constexpr double JOINT_STATES_TOPIC_TIMEOUT
ROSCPP_DECL bool ok()
bool sleep()
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...
static Time now()
static const std::string TOPIC_NAME


prbt_hardware_support
Author(s):
autogenerated on Thu Nov 19 2020 03:11:50