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 
30 
31 #include <cmath>
32 
33 #include <gtest/gtest.h>
34 
35 #include <ros/ros.h>
36 
37 #include <geometry_msgs/Twist.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.01; // 1 cm-s precision
46 const double VELOCITY_TOLERANCE = 0.02; // 2 cm-s-1 precision
47 const double ANGULAR_VELOCITY_TOLERANCE = 0.05; // 3 deg-s-1 precision
48 const double JERK_LINEAR_VELOCITY_TOLERANCE = 0.10; // 10 cm-s-1 precision
49 const double JERK_ANGULAR_VELOCITY_TOLERANCE = 0.05; // 3 deg-s-1 precision
50 const double ORIENTATION_TOLERANCE = 0.03; // 0.57 degree precision
51 
52 class AckermannSteeringControllerTest : public ::testing::Test
53 {
54 public:
55 
57  : cmd_pub(nh.advertise<geometry_msgs::Twist>("cmd_vel", 100))
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  void publish(geometry_msgs::Twist cmd_vel){ cmd_pub.publish(cmd_vel); }
72 
73  void start(){ std_srvs::Empty srv; start_srv.call(srv); }
74  void stop(){ std_srvs::Empty srv; stop_srv.call(srv); }
75 
76 private:
80  nav_msgs::Odometry last_odom;
81 
84 
85  void odomCallback(const nav_msgs::Odometry& odom)
86  {
87  ROS_INFO_STREAM("Callback reveived: pos.x: " << odom.pose.pose.position.x
88  << ", orient.z: " << odom.pose.pose.orientation.z
89  << ", lin_est: " << odom.twist.twist.linear.x
90  << ", ang_est: " << odom.twist.twist.angular.z);
91  last_odom = odom;
92  }
93 };
94 
95 inline tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion& quat)
96 {
97  return tf::Quaternion(quat.x, quat.y, quat.z, quat.w);
98 }
void odomCallback(const nav_msgs::Odometry &odom)
Definition: test_common.h:85
nav_msgs::Odometry getLastOdom()
Definition: test_common.h:69
void publish(const boost::shared_ptr< M > &message) const
nav_msgs::Odometry last_odom
Definition: test_common.h:80
ServiceClient serviceClient(ros::NodeHandle n, std::string name)
const double VELOCITY_TOLERANCE
Definition: test_common.h:46
bool call(MReq &req, MRes &res)
const double EPS
Definition: test_common.h:44
void publish(geometry_msgs::Twist cmd_vel)
Definition: test_common.h:70
tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion &quat)
Definition: test_common.h:95
ros::ServiceClient start_srv
Definition: test_common.h:82
ros::ServiceClient stop_srv
Definition: test_common.h:83
void subscribe()
const double JERK_ANGULAR_VELOCITY_TOLERANCE
Definition: test_common.h:49
uint32_t getNumPublishers() const
const double JERK_LINEAR_VELOCITY_TOLERANCE
Definition: test_common.h:48
uint32_t getNumSubscribers() const
#define ROS_INFO_STREAM(args)
const double ANGULAR_VELOCITY_TOLERANCE
Definition: test_common.h:47
const double ORIENTATION_TOLERANCE
Definition: test_common.h:50
const double POSITION_TOLERANCE
Definition: test_common.h:45


ackermann_steering_controller
Author(s): Masaru Morita
autogenerated on Thu Apr 11 2019 03:08:13