18 #ifndef PRBT_HARDWARE_SUPPORT_BRAKE_TEST_UTILS_H 19 #define PRBT_HARDWARE_SUPPORT_BRAKE_TEST_UTILS_H 25 #include <sensor_msgs/JointState.h> 29 static const std::string
TOPIC_NAME{
"/prbt/joint_states"};
60 const sensor_msgs::JointStateConstPtr &msg2,
94 const sensor_msgs::JointStateConstPtr &msg2,
97 return std::equal(msg1->position.begin(), msg1->position.end(), msg2->position.begin(),
98 [tol](
double pos1,
double pos2) {
return (fabs(pos1 - pos2) < tol); });
103 #endif // PRBT_HARDWARE_SUPPORT_BRAKE_TEST_UTILS_H
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
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 const std::string TOPIC_NAME