test_common.h
Go to the documentation of this file.
1 // Copyright (C) 2013, PAL Robotics S.L.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright notice,
7 // this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of PAL Robotics, Inc. nor the names of its
12 // contributors may be used to endorse or promote products derived from
13 // this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 // POSSIBILITY OF SUCH DAMAGE.
27 
29 
30 #include <cmath>
31 
32 #include <gtest/gtest.h>
33 
34 #include <ros/ros.h>
35 
36 #include <geometry_msgs/TwistStamped.h>
37 #include <nav_msgs/Odometry.h>
38 #include <tf/tf.h>
39 
40 #include <std_srvs/Empty.h>
41 
42 // Floating-point value comparison threshold
43 const double EPS = 0.01;
44 const double POSITION_TOLERANCE = 0.01; // 1 cm-s precision
45 const double VELOCITY_TOLERANCE = 0.02; // 2 cm-s-1 precision
46 const double JERK_LINEAR_VELOCITY_TOLERANCE = 0.10; // 10 cm-s-1 precision
47 const double JERK_ANGULAR_VELOCITY_TOLERANCE = 0.05; // 3 deg-s-1 precision
48 const double ORIENTATION_TOLERANCE = 0.03; // 0.57 degree precision
49 
50 class DiffDriveControllerTest : public ::testing::Test
51 {
52 public:
53 
55  : received_first_odom(false)
56  , cmd_pub(nh.advertise<geometry_msgs::Twist>("cmd_vel", 100))
58  , vel_out_sub(nh.subscribe("cmd_vel_out", 100, &DiffDriveControllerTest::cmdVelOutCallback, this))
59  , start_srv(nh.serviceClient<std_srvs::Empty>("start"))
60  , stop_srv(nh.serviceClient<std_srvs::Empty>("stop"))
61  {
62  }
63 
65  {
67  }
68 
69  nav_msgs::Odometry getLastOdom(){ return last_odom; }
70  geometry_msgs::TwistStamped getLastCmdVelOut(){ return last_cmd_vel_out; }
71  void publish(geometry_msgs::Twist cmd_vel){ cmd_pub.publish(cmd_vel); }
72  bool isControllerAlive()const{ return (odom_sub.getNumPublishers() > 0) && (cmd_pub.getNumSubscribers() > 0); }
73  bool isPublishingCmdVelOut(const ros::Duration &timeout=ros::Duration(1)) const
74  {
76  int get_num_publishers = vel_out_sub.getNumPublishers();
77  while ( (get_num_publishers == 0) && (ros::Time::now() < start + timeout) ) {
78  ros::Duration(0.1).sleep();
79  get_num_publishers = vel_out_sub.getNumPublishers();
80  }
81  return (get_num_publishers > 0);
82  }
84 
85  void start(){ std_srvs::Empty srv; start_srv.call(srv); }
86  void stop(){ std_srvs::Empty srv; stop_srv.call(srv); }
87 
88  void waitForController() const
89  {
90  while(!isControllerAlive() && ros::ok())
91  {
92  ROS_DEBUG_STREAM_THROTTLE(0.5, "Waiting for controller.");
93  ros::Duration(0.1).sleep();
94  }
95  if (!ros::ok())
96  FAIL() << "Something went wrong while executing test.";
97  }
98 
99  void waitForOdomMsgs() const
100  {
101  while(!hasReceivedFirstOdom() && ros::ok())
102  {
103  ROS_DEBUG_STREAM_THROTTLE(0.5, "Waiting for odom messages to be published.");
104  ros::Duration(0.01).sleep();
105  }
106  if (!ros::ok())
107  FAIL() << "Something went wrong while executing test.";
108  }
109 
110 private:
116  nav_msgs::Odometry last_odom;
117  geometry_msgs::TwistStamped last_cmd_vel_out;
118 
121 
122  void odomCallback(const nav_msgs::Odometry& odom)
123  {
124  ROS_INFO_STREAM("Callback received: pos.x: " << odom.pose.pose.position.x
125  << ", orient.z: " << odom.pose.pose.orientation.z
126  << ", lin_est: " << odom.twist.twist.linear.x
127  << ", ang_est: " << odom.twist.twist.angular.z);
128  last_odom = odom;
129  received_first_odom = true;
130  }
131 
132  void cmdVelOutCallback(const geometry_msgs::TwistStamped& cmd_vel_out)
133  {
134  ROS_INFO_STREAM("Callback received: lin: " << cmd_vel_out.twist.linear.x
135  << ", ang: " << cmd_vel_out.twist.angular.z);
136  last_cmd_vel_out = cmd_vel_out;
137  }
138 };
139 
140 inline tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion& quat)
141 {
142  return tf::Quaternion(quat.x, quat.y, quat.z, quat.w);
143 }
144 
ros::NodeHandle nh
Definition: test_common.h:112
#define ROS_DEBUG_STREAM_THROTTLE(rate, args)
geometry_msgs::TwistStamped getLastCmdVelOut()
Definition: test_common.h:70
bool hasReceivedFirstOdom() const
Definition: test_common.h:83
void cmdVelOutCallback(const geometry_msgs::TwistStamped &cmd_vel_out)
Definition: test_common.h:132
void publish(const boost::shared_ptr< M > &message) const
ServiceClient serviceClient(ros::NodeHandle n, std::string name)
nav_msgs::Odometry getLastOdom()
Definition: test_common.h:69
void waitForOdomMsgs() const
Definition: test_common.h:99
bool sleep() const
nav_msgs::Odometry last_odom
Definition: test_common.h:116
ros::ServiceClient start_srv
Definition: test_common.h:119
const double VELOCITY_TOLERANCE
Definition: test_common.h:45
bool call(MReq &req, MRes &res)
void publish(geometry_msgs::Twist cmd_vel)
Definition: test_common.h:71
const double EPS
Definition: test_common.h:43
ros::Subscriber vel_out_sub
Definition: test_common.h:115
tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion &quat)
Definition: test_common.h:140
void subscribe()
const double JERK_ANGULAR_VELOCITY_TOLERANCE
Definition: test_common.h:47
uint32_t getNumPublishers() const
ROSCPP_DECL bool ok()
bool isPublishingCmdVelOut(const ros::Duration &timeout=ros::Duration(1)) const
Definition: test_common.h:73
ros::Publisher cmd_pub
Definition: test_common.h:113
const double JERK_LINEAR_VELOCITY_TOLERANCE
Definition: test_common.h:46
bool isControllerAlive() const
Definition: test_common.h:72
ros::ServiceClient stop_srv
Definition: test_common.h:120
uint32_t getNumSubscribers() const
#define ROS_INFO_STREAM(args)
void waitForController() const
Definition: test_common.h:88
static Time now()
const double ORIENTATION_TOLERANCE
Definition: test_common.h:48
ros::Subscriber odom_sub
Definition: test_common.h:114
geometry_msgs::TwistStamped last_cmd_vel_out
Definition: test_common.h:117
void odomCallback(const nav_msgs::Odometry &odom)
Definition: test_common.h:122
const double POSITION_TOLERANCE
Definition: test_common.h:44


diff_drive_controller
Author(s): Bence Magyar
autogenerated on Sat Apr 18 2020 03:58:05