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/Twist.h>
37 #include <four_wheel_steering_msgs/FourWheelSteering.h>
38 #include <nav_msgs/Odometry.h>
39 #include <tf/tf.h>
40 
41 #include <std_srvs/Empty.h>
42 
43 // Floating-point value comparison threshold
44 const double EPS = 0.01;
45 const double POSITION_TOLERANCE = 0.02; // 2 cm-s precision
46 const double VELOCITY_TOLERANCE = 0.02; // 2 cm-s-1 precision
47 const double JERK_LINEAR_VELOCITY_TOLERANCE = 0.10; // 10 cm-s-1 precision
48 const double JERK_ANGULAR_VELOCITY_TOLERANCE = 0.05; // 3 deg-s-1 precision
49 const double ORIENTATION_TOLERANCE = 0.08; // 4.58 degree precision
50 
51 class FourWheelSteeringControllerTest : public ::testing::Test
52 {
53 public:
54 
56  : received_first_odom(false)
57  , cmd_twist_pub(nh.advertise<geometry_msgs::Twist>("cmd_vel", 100))
58  , cmd_4ws_pub(nh.advertise<four_wheel_steering_msgs::FourWheelSteering>("cmd_four_wheel_steering", 100))
60  , start_srv(nh.serviceClient<std_srvs::Empty>("start"))
61  , stop_srv(nh.serviceClient<std_srvs::Empty>("stop"))
62  {
63  }
64 
66  {
68  }
69 
70  nav_msgs::Odometry getLastOdom(){ return last_odom; }
71  void publish(geometry_msgs::Twist cmd_vel)
72  {
73  cmd_twist_pub.publish(cmd_vel);
74  }
75  void publish_4ws(four_wheel_steering_msgs::FourWheelSteering cmd_vel)
76  {
77  cmd_4ws_pub.publish(cmd_vel);
78  }
79  bool isControllerAlive()const{ return (odom_sub.getNumPublishers() > 0)
81 
83 
84  void start(){ std_srvs::Empty srv; start_srv.call(srv); }
85  void stop(){ std_srvs::Empty srv; stop_srv.call(srv); }
86 
87  void waitForController() const
88  {
89  while(!isControllerAlive() && ros::ok())
90  {
91  ROS_DEBUG_STREAM_THROTTLE(0.5, "Waiting for controller.");
92  ros::Duration(0.1).sleep();
93  }
94  if (!ros::ok())
95  FAIL() << "Something went wrong while executing test.";
96  }
97 
98  void waitForOdomMsgs() const
99  {
100  while(!hasReceivedFirstOdom() && ros::ok())
101  {
102  ROS_DEBUG_STREAM_THROTTLE(0.5, "Waiting for odom messages to be published.");
103  ros::Duration(0.01).sleep();
104  }
105  if (!ros::ok())
106  FAIL() << "Something went wrong while executing test.";
107  }
108 
109 private:
114  nav_msgs::Odometry last_odom;
115 
118 
119  void odomCallback(const nav_msgs::Odometry& odom)
120  {
121  ROS_INFO_STREAM("Callback reveived: pos.x: " << odom.pose.pose.position.x
122  << ", orient.z: " << odom.pose.pose.orientation.z
123  << ", lin_est: " << odom.twist.twist.linear.x
124  << ", ang_est: " << odom.twist.twist.angular.z);
125  last_odom = odom;
126  received_first_odom = true;
127  }
128 };
129 
130 inline tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion& quat)
131 {
132  return tf::Quaternion(quat.x, quat.y, quat.z, quat.w);
133 }
134 
ros::ServiceClient stop_srv
Definition: test_common.h:117
void publish_4ws(four_wheel_steering_msgs::FourWheelSteering cmd_vel)
Definition: test_common.h:75
#define ROS_DEBUG_STREAM_THROTTLE(rate, args)
void publish(geometry_msgs::Twist cmd_vel)
Definition: test_common.h:71
void publish(const boost::shared_ptr< M > &message) const
ServiceClient serviceClient(ros::NodeHandle n, std::string name)
bool sleep() const
const double VELOCITY_TOLERANCE
Definition: test_common.h:46
bool call(MReq &req, MRes &res)
nav_msgs::Odometry last_odom
Definition: test_common.h:114
const double EPS
Definition: test_common.h:44
tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion &quat)
Definition: test_common.h:130
void subscribe()
const double JERK_ANGULAR_VELOCITY_TOLERANCE
Definition: test_common.h:48
bool hasReceivedFirstOdom() const
Definition: test_common.h:82
void odomCallback(const nav_msgs::Odometry &odom)
Definition: test_common.h:119
uint32_t getNumPublishers() const
ROSCPP_DECL bool ok()
const double JERK_LINEAR_VELOCITY_TOLERANCE
Definition: test_common.h:47
uint32_t getNumSubscribers() const
#define ROS_INFO_STREAM(args)
const double ORIENTATION_TOLERANCE
Definition: test_common.h:49
nav_msgs::Odometry getLastOdom()
Definition: test_common.h:70
ros::ServiceClient start_srv
Definition: test_common.h:116
const double POSITION_TOLERANCE
Definition: test_common.h:45


four_wheel_steering_controller
Author(s): Vincent Rousseau
autogenerated on Sat Apr 18 2020 03:58:13