encoders_monitor_tests.cpp
Go to the documentation of this file.
00001 #include "gtest/gtest.h"
00002 #include "grizzly_motion/encoders_monitor.h"
00003 
00004 #include "ros/console.h"
00005 
00006 class EncodersMonitorFixture : public ::testing::Test 
00007 {
00008 public:
00009   virtual void SetUp() {
00010     ros::Time::init();
00011     start = ros::Time::now();
00012     em.encoders_timeout = ros::Duration(0.05);
00013     em.encoder_fault_time_to_failure = ros::Duration(0.5);
00014     em.encoder_speed_error_diff_threshold = 0.5;
00015 
00016     drive.reset(new grizzly_msgs::Drive);
00017     enc.reset(new grizzly_msgs::Drive);
00018   }
00019 
00020   void setStamps(ros::Time t) {
00021     drive->header.stamp = t;
00022     enc->header.stamp = t;
00023   }
00024 
00025   ros::Time start;
00026   EncodersMonitor em;
00027 
00028   grizzly_msgs::DrivePtr drive;
00029   grizzly_msgs::DrivePtr enc;
00030 };
00031 
00032 TEST_F(EncodersMonitorFixture, no_fault_when_encoders_match_commanded)
00033 {
00034   // 100 seconds in ROS time.
00035   for (int step = 0; step < 2000; step++)
00036   {
00037     setStamps(start + ros::Duration(0.05) * step);
00038     VectorDrive vec(step, 2000 - step, step, 2000 - step);
00039     grizzly_msgs::fillDriveMsgFromVector(vec, drive.get());
00040     em.encodersCallback(drive);
00041     em.driveCallback(drive);
00042     EXPECT_TRUE(em.ok());
00043   } 
00044 }
00045 
00046 TEST_F(EncodersMonitorFixture, no_fault_when_two_encoders_stall)
00047 {
00048   // 100 seconds in ROS time.
00049   for (int step = 0; step < 2000; step++)
00050   {
00051     setStamps(start + ros::Duration(0.05) * step);
00052     VectorDrive vec(step, step, step, step);
00053     grizzly_msgs::fillDriveMsgFromVector(vec, drive.get());
00054     if (step > 1500) {
00055       // Simulate two motors stalling under load.
00056       vec[2] = 0;
00057       vec[3] = 0;
00058     }
00059     grizzly_msgs::fillDriveMsgFromVector(vec, enc.get());
00060     em.driveCallback(drive);
00061     em.encodersCallback(enc);
00062     EXPECT_TRUE(em.ok());
00063   } 
00064 }
00065 
00066 TEST_F(EncodersMonitorFixture, no_fault_in_partial_stall)
00067 {
00068   // 100 seconds in ROS time.
00069   for (int step = 0; step < 2000; step++)
00070   {
00071     setStamps(start + ros::Duration(0.05) * step);
00072     VectorDrive vec(step, -step, step, -step);
00073     grizzly_msgs::fillDriveMsgFromVector(vec, drive.get());
00074     if (step > 1500) {
00075       // Simulate partial stall on one wheel.
00076       vec[2] = 10;
00077     }
00078     grizzly_msgs::fillDriveMsgFromVector(vec, enc.get());
00079     em.driveCallback(drive);
00080     em.encodersCallback(enc);
00081     EXPECT_TRUE(em.ok());
00082   } 
00083 }
00084 
00085 TEST_F(EncodersMonitorFixture, no_fault_brief_dropout)
00086 {
00087   // 100 seconds in ROS time.
00088   for (int step = 0; step < 2000; step++)
00089   {
00090     setStamps(start + ros::Duration(0.05) * step);
00091     VectorDrive vec(step, -step, step, -step);
00092     grizzly_msgs::fillDriveMsgFromVector(vec, drive.get());
00093     if (step > 1500 && step < 1508) {
00094       // Momentary encoder loss.
00095       vec[2] = 0;
00096     }
00097     grizzly_msgs::fillDriveMsgFromVector(vec, enc.get());
00098     em.driveCallback(drive);
00099     em.encodersCallback(enc);
00100     EXPECT_TRUE(em.ok());
00101   } 
00102 }
00103 
00104 TEST_F(EncodersMonitorFixture, fault_when_one_encoder_fails)
00105 {
00106   // 100 seconds in ROS time.
00107   for (int step = 0; step < 2000; step++)
00108   {
00109     setStamps(start + ros::Duration(0.05) * step);
00110     VectorDrive vec(step, -step, step, -step);
00111     grizzly_msgs::fillDriveMsgFromVector(vec, drive.get());
00112     if (step > 1500) {
00113       // Simulate encoder failure on one wheel.
00114       vec[2] = 0.0001;
00115     }
00116     grizzly_msgs::fillDriveMsgFromVector(vec, enc.get());
00117     em.driveCallback(drive);
00118     em.encodersCallback(enc);
00119     bool res = em.ok();
00120     if (step < 1510) EXPECT_TRUE(res);
00121     if (step > 1510) EXPECT_FALSE(res);
00122   } 
00123 }
00124 
00125 TEST_F(EncodersMonitorFixture, timeout)
00126 {
00127   // Before the first encoders is received, should definitely be not-ok.
00128   EXPECT_FALSE(em.ok());
00129 
00130   // Receive one which is clearly out of date, still not ok.
00131   grizzly_msgs::DrivePtr msg(new grizzly_msgs::Drive);
00132   msg->header.stamp = ros::Time::now() - ros::Duration(1.0);
00133   em.encodersCallback(msg);
00134   EXPECT_FALSE(em.ok());
00135 
00136   // Receive an up-to-date one, should now be okay.
00137   msg->header.stamp = ros::Time::now();
00138   em.encodersCallback(msg);
00139   EXPECT_TRUE(em.ok());
00140 
00141   // Out of date again.
00142   msg->header.stamp = ros::Time::now() - ros::Duration(0.5);
00143   em.encodersCallback(msg);
00144   EXPECT_FALSE(em.ok());
00145 }
00146 
00147 int main(int argc, char **argv)
00148 {
00149   ::testing::InitGoogleTest(&argc, argv);
00150   return RUN_ALL_TESTS();
00151 }


grizzly_motion
Author(s):
autogenerated on Thu Jun 6 2019 21:44:02