Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00027
00030
00031 #include <cmath>
00032
00033 #include <gtest/gtest.h>
00034
00035 #include <ros/ros.h>
00036
00037 #include <geometry_msgs/Twist.h>
00038 #include <nav_msgs/Odometry.h>
00039 #include <tf/tf.h>
00040
00041 #include <std_srvs/Empty.h>
00042
00043
00044 const double EPS = 0.05;
00045 const double POSITION_TOLERANCE = 0.05;
00046 const double VELOCITY_TOLERANCE = 0.10;
00047 const double ANGULAR_VELOCITY_TOLERANCE = 0.1;
00048 const double JERK_LINEAR_VELOCITY_TOLERANCE = 0.10;
00049 const double JERK_ANGULAR_VELOCITY_TOLERANCE = 0.05;
00050 const double ORIENTATION_TOLERANCE = 0.03;
00051
00052 class SteerDriveControllerTest : public ::testing::Test
00053 {
00054 public:
00055
00056 SteerDriveControllerTest()
00057 : cmd_pub(nh.advertise<geometry_msgs::Twist>("cmd_vel", 100))
00058 , odom_sub(nh.subscribe("odom", 100, &SteerDriveControllerTest::odomCallback, this))
00059 , start_srv(nh.serviceClient<std_srvs::Empty>("start"))
00060 , stop_srv(nh.serviceClient<std_srvs::Empty>("stop"))
00061 {
00062 }
00063
00064 ~SteerDriveControllerTest()
00065 {
00066 odom_sub.shutdown();
00067 }
00068
00069 nav_msgs::Odometry getLastOdom(){ return last_odom; }
00070 void publish(geometry_msgs::Twist cmd_vel){ cmd_pub.publish(cmd_vel); }
00071 bool isControllerAlive(){ return (odom_sub.getNumPublishers() > 0) && (cmd_pub.getNumSubscribers() > 0); }
00072
00073 void start(){ std_srvs::Empty srv; start_srv.call(srv); }
00074 void stop(){ std_srvs::Empty srv; stop_srv.call(srv); }
00075
00076 private:
00077 ros::NodeHandle nh;
00078 ros::Publisher cmd_pub;
00079 ros::Subscriber odom_sub;
00080 nav_msgs::Odometry last_odom;
00081
00082 ros::ServiceClient start_srv;
00083 ros::ServiceClient stop_srv;
00084
00085 void odomCallback(const nav_msgs::Odometry& odom)
00086 {
00087 ROS_INFO_STREAM("Callback reveived: pos.x: " << odom.pose.pose.position.x
00088 << ", orient.z: " << odom.pose.pose.orientation.z
00089 << ", lin_est: " << odom.twist.twist.linear.x
00090 << ", ang_est: " << odom.twist.twist.angular.z);
00091 last_odom = odom;
00092 }
00093 };
00094
00095 inline tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion& quat)
00096 {
00097 return tf::Quaternion(quat.x, quat.y, quat.z, quat.w);
00098 }