GripperPositionStateMachine_Test.cpp
Go to the documentation of this file.
00001 #include <gtest/gtest.h>
00002 #include <boost/shared_ptr.hpp>
00003 #include <boost/make_shared.hpp>
00004 #include "robodyn_mechanisms/GripperPositionStateMachine.h"
00005 
00006 using namespace std;
00007 
00008 namespace sc = boost::statechart;
00009 namespace mpl = boost::mpl;
00010 namespace scu = StatechartUtilities;
00011 
00012 std::map<std::string, uint16_t> testUInt16s;
00013 
00014 uint16_t getUInt16(const std::string& name)
00015 {
00016     return testUInt16s[name];
00017 }
00018 
00019 void setUInt16(const std::string& name, uint16_t value)
00020 {
00021     testUInt16s[name] = value;
00022 }
00023 
00024 std::map<std::string, float> testFloats;
00025 
00026 float getFloat(const std::string& name)
00027 {
00028     return testFloats[name];
00029 }
00030 
00031 void setFloat(const std::string& name, float value)
00032 {
00033     testFloats[name] = value;
00034 }
00035 
00036 void sendCommand(const r2_msgs::JointCommand& msg)
00037 {
00038     //cout << msg << endl;
00039 }
00040 
00041 class GripperPositionStateMachineTest : public ::testing::Test
00042 {
00043 protected:
00044     virtual void SetUp()
00045     {
00046         gp                            = boost::make_shared<GripperPosition>();
00047         gp->mechanism                 = "r2/left_leg/gripper";
00048         gp->getFloat                  = getFloat;
00049         gp->setFloat                  = setFloat;
00050         gp->getUInt16                 = getUInt16;
00051         gp->setUInt16                 = setUInt16;
00052         gp->dutyCycleLimitName        = "r2/left_leg/gripper/joint0/MotComLimit";
00053         gp->measuredCurrentName       = "r2/left_leg/gripper/joint0/CurrentMeasured";
00054         gp->currentLimitName          = "r2/left_leg/gripper/joint0/VLCurLimit";
00055         gp->jointCommand.name.assign(1, "r2/left_leg/gripper/joint0");
00056         gp->sendCommand               = sendCommand;
00057         gp->jawOpenPosition           = 0.5;
00058         gp->encoderOpenVelocity       = 15;
00059         gp->encoderCloseVelocity      = -15;
00060         gp->encoderClosedPosition     = 0;
00061         gp->encoderState.position.assign(1, 0);
00062         gp->jawLeftState.position.assign(1, 0);
00063         gp->jawRightState.position.assign(1, 0);
00064         gp->encoderState.velocity.assign(1, 0);
00065         gp->encoderState.effort.assign(1, 0);
00066         gp->jawLeftState.effort.assign(1, 0);
00067         gp->jawRightState.effort.assign(1, 0);
00068         gp->jawPositionNoise          = 0.005;
00069         gp->encoderPositionNoise      = 0.1;
00070         gp->encoderVelocityNoise      = 0.1;
00071         gp->currentNoise              = 0.1;
00072         gp->currentMax                = 4.0;
00073         gp->dutyCycleLimit            = 255;
00074         gp->weakForce                 = 30;
00075         gp->strongForce               = 30;
00076         gp->jawPositionForceThreshold = 0.0;
00077         gp->dutyCycleLimitFilter.setRates(1, 255);
00078         gp->outputCurrentFilter.setRates(0.05, 1000);
00079         gp->initiate();
00080     }
00081 
00082     virtual void TearDown()
00083     {
00084     }
00085 
00086     boost::shared_ptr<GripperPosition> gp;
00087     std::vector<std::string> states;
00088 };
00089 
00090 TEST_F(GripperPositionStateMachineTest, InitialStates)
00091 {
00092     states = scu::getCurrentStates(gp);
00093     EXPECT_EQ(12, states.size());
00094 
00095     EXPECT_TRUE(scu::isActiveState(gp, "Container"));
00096     EXPECT_TRUE(scu::isActiveState(gp, "Inactive"));
00097     EXPECT_TRUE(scu::isActiveState(gp, "Unready"));
00098     EXPECT_TRUE(scu::isActiveState(gp, "NotFaulted"));
00099     EXPECT_TRUE(scu::isActiveState(gp, "Unloaded"));
00100     EXPECT_TRUE(scu::isActiveState(gp, "Locked"));
00101     EXPECT_TRUE(scu::isActiveState(gp, "LockedStatus"));
00102     EXPECT_TRUE(scu::isActiveState(gp, "LessThanDesiredJawPosition"));
00103     EXPECT_TRUE(scu::isActiveState(gp, "NotStalled"));
00104     EXPECT_TRUE(scu::isActiveState(gp, "Still"));
00105     EXPECT_TRUE(scu::isActiveState(gp, "NotOpen"));
00106     EXPECT_TRUE(scu::isActiveState(gp, "LessThanForceThreshold"));
00107 
00108     EXPECT_STREQ("r2/left_leg/gripper/joint0", gp->jointCommand.name[0].c_str());
00109 }
00110 
00111 TEST_F(GripperPositionStateMachineTest, Loaded)
00112 {
00114     EXPECT_TRUE(scu::isActiveState(gp, "Unloaded"));
00115     gp->process_event(e_loaded());
00116     EXPECT_TRUE(scu::isActiveState(gp, "Loaded"));
00117     gp->process_event(e_unloaded());
00118     EXPECT_TRUE(scu::isActiveState(gp, "Unloaded"));
00119 
00121     gp->setPoint.expectedLoad = 10;
00122     gp->jawLeftState.effort.assign(1, 10);
00123     gp->jawRightState.effort.assign(1, 10);
00124     gp->process_event(e_do());
00125     EXPECT_TRUE(scu::isActiveState(gp, "Loaded"));
00126     gp->jawLeftState.effort.assign(1, 9);
00127     gp->jawRightState.effort.assign(1, 9);
00128     gp->process_event(e_do());
00129     EXPECT_TRUE(scu::isActiveState(gp, "Unloaded"));
00130     gp->jawLeftState.effort.assign(1, 9);
00131     gp->jawRightState.effort.assign(1, 11);
00132     gp->process_event(e_do());
00133     EXPECT_TRUE(scu::isActiveState(gp, "Loaded"));
00134 }
00135 
00136 TEST_F(GripperPositionStateMachineTest, Locked)
00137 {
00139     EXPECT_TRUE(scu::isActiveState(gp, "Locked"));
00140     gp->process_event(e_unlocked());
00141     EXPECT_TRUE(scu::isActiveState(gp, "Unlocked"));
00142     gp->process_event(e_locked());
00143     EXPECT_TRUE(scu::isActiveState(gp, "Locked"));
00144 
00147     // double baseEncoderAngle = GripperKinematics::getEncoderAngle(0);
00148     // gp->encoderState.position.assign(1, baseEncoderAngle-2);
00149     // gp->jawLeftState.position.assign(1, 0);
00150     // gp->jawRightState.position.assign(1, 0);
00151     // gp->process_event(e_do());
00152     // EXPECT_TRUE(scu::isActiveState(gp, "Locked"));
00153     // gp->encoderState.position.assign(1, baseEncoderAngle+5);
00154     // gp->process_event(e_do());
00155     // EXPECT_TRUE(scu::isActiveState(gp, "Unlocked"));
00156 }
00157 
00158 TEST_F(GripperPositionStateMachineTest, LockedStatus)
00159 {
00161     EXPECT_TRUE(scu::isActiveState(gp, "LockedStatus"));
00162     gp->process_event(e_unlockedStatus());
00163     EXPECT_TRUE(scu::isActiveState(gp, "UnlockedStatus"));
00164     gp->process_event(e_lockedStatus());
00165     EXPECT_TRUE(scu::isActiveState(gp, "LockedStatus"));
00166 
00169     // double baseEncoderAngle = GripperKinematics::getEncoderAngle(0);
00170     // gp->encoderState.position.assign(1, baseEncoderAngle-2);
00171     // gp->jawLeftState.position.assign(1, 0);
00172     // gp->jawRightState.position.assign(1, 0);
00173     // gp->process_event(e_do());
00174     // EXPECT_TRUE(scu::isActiveState(gp, "Locked"));
00175     // gp->encoderState.position.assign(1, baseEncoderAngle+5);
00176     // gp->process_event(e_do());
00177     // EXPECT_TRUE(scu::isActiveState(gp, "Unlocked"));
00178 }
00179 
00180 TEST_F(GripperPositionStateMachineTest, Ready)
00181 {
00183     EXPECT_TRUE(scu::isActiveState(gp, "Unready"));
00184     gp->process_event(e_ready());
00185     EXPECT_TRUE(scu::isActiveState(gp, "Ready"));
00186     gp->process_event(e_unready());
00187     EXPECT_TRUE(scu::isActiveState(gp, "Unready"));
00188 
00190     gp->jointControlData.controlMode.state = r2_msgs::JointControlMode::DRIVE;
00191     gp->process_event(e_do());
00192     EXPECT_TRUE(scu::isActiveState(gp, "Ready"));
00193     gp->jointControlData.controlMode.state = r2_msgs::JointControlMode::IGNORE;
00194     gp->process_event(e_do());
00195     EXPECT_TRUE(scu::isActiveState(gp, "Unready"));
00196 }
00197 
00198 TEST_F(GripperPositionStateMachineTest, DesiredJawPosition)
00199 {
00201     EXPECT_TRUE(scu::isActiveState(gp, "LessThanDesiredJawPosition"));
00202     gp->process_event(e_atDesiredJawPosition());
00203     EXPECT_TRUE(scu::isActiveState(gp, "AtDesiredJawPosition"));
00204     gp->process_event(e_lessThanDesiredJawPosition());
00205     EXPECT_TRUE(scu::isActiveState(gp, "LessThanDesiredJawPosition"));
00206     gp->process_event(e_moreThanDesiredJawPosition());
00207     EXPECT_TRUE(scu::isActiveState(gp, "MoreThanDesiredJawPosition"));
00208     gp->process_event(e_atDesiredJawPosition());
00209     EXPECT_TRUE(scu::isActiveState(gp, "AtDesiredJawPosition"));
00210     gp->process_event(e_moreThanDesiredJawPosition());
00211     EXPECT_TRUE(scu::isActiveState(gp, "MoreThanDesiredJawPosition"));
00212     gp->process_event(e_lessThanDesiredJawPosition());
00213     EXPECT_TRUE(scu::isActiveState(gp, "LessThanDesiredJawPosition"));
00214 
00216     gp->setPoint.jawPosition = 1;
00217     gp->setPoint.jawPositionDelta = 0.100001;
00218     gp->jawLeftState.position.assign(1, 1);
00219     gp->jawRightState.position.assign(1, 1);
00220     gp->process_event(e_do());
00221     EXPECT_TRUE(scu::isActiveState(gp, "AtDesiredJawPosition"));
00222     gp->jawLeftState.position.assign(1, 0.8);
00223     gp->jawRightState.position.assign(1, 0.8);
00224     gp->process_event(e_do());
00225     EXPECT_TRUE(scu::isActiveState(gp, "LessThanDesiredJawPosition"));
00226     gp->jawLeftState.position.assign(1, 1.2);
00227     gp->jawRightState.position.assign(1, 1.2);
00228     gp->process_event(e_do());
00229     EXPECT_TRUE(scu::isActiveState(gp, "MoreThanDesiredJawPosition"));
00230 }
00231 
00232 TEST_F(GripperPositionStateMachineTest, Stalled)
00233 {
00235     EXPECT_TRUE(scu::isActiveState(gp, "NotStalled"));
00236     gp->process_event(e_stalled());
00237     EXPECT_TRUE(scu::isActiveState(gp, "Stalled"));
00238     gp->process_event(e_notStalled());
00239     EXPECT_TRUE(scu::isActiveState(gp, "NotStalled"));
00240 
00242     gp->currentLimit    = 0.5;
00243     gp->currentNoise    = 0.1;
00244     gp->measuredCurrent = 0.45;
00245     gp->process_event(e_do());
00246     EXPECT_TRUE(scu::isActiveState(gp, "Stalled"));
00248     gp->measuredCurrent = 0.35;
00249     gp->process_event(e_do());
00250     EXPECT_FALSE(scu::isActiveState(gp, "Stalled"));
00251 }
00252 
00253 TEST_F(GripperPositionStateMachineTest, Faulted)
00254 {
00256     EXPECT_TRUE(scu::isActiveState(gp, "NotFaulted"));
00257     gp->process_event(e_faulted());
00258     EXPECT_TRUE(scu::isActiveState(gp, "Faulted"));
00259     gp->process_event(e_notFaulted());
00260     EXPECT_TRUE(scu::isActiveState(gp, "NotFaulted"));
00261 }
00262 
00263 TEST_F(GripperPositionStateMachineTest, Moving)
00264 {
00266     EXPECT_TRUE(scu::isActiveState(gp, "Still"));
00267     gp->process_event(e_moving());
00268     EXPECT_TRUE(scu::isActiveState(gp, "Moving"));
00269     gp->process_event(e_still());
00270     EXPECT_TRUE(scu::isActiveState(gp, "Still"));
00271 
00273     gp->encoderState.velocity.assign(1, 42);
00274     gp->process_event(e_do());
00275     EXPECT_TRUE(scu::isActiveState(gp, "Moving"));
00276     gp->encoderState.velocity.assign(1, 0);
00277     gp->process_event(e_do());
00278     EXPECT_TRUE(scu::isActiveState(gp, "Still"));
00279 }
00280 
00281 TEST_F(GripperPositionStateMachineTest, Open)
00282 {
00284     EXPECT_TRUE(scu::isActiveState(gp, "NotOpen"));
00285     gp->process_event(e_isOpen());
00286     EXPECT_TRUE(scu::isActiveState(gp, "Open"));
00287     gp->process_event(e_isNotOpen());
00288     EXPECT_TRUE(scu::isActiveState(gp, "NotOpen"));
00289 
00291     gp->jawLeftState.position.assign(1, gp->jawOpenPosition);
00292     gp->jawRightState.position.assign(1, gp->jawOpenPosition);
00293     gp->process_event(e_do());
00294     EXPECT_TRUE(scu::isActiveState(gp, "Open"));
00295     gp->jawLeftState.position.assign(1, gp->jawOpenPosition - 0.1);
00296     gp->jawRightState.position.assign(1, gp->jawOpenPosition - 0.1);
00297     gp->process_event(e_do());
00298     EXPECT_TRUE(scu::isActiveState(gp, "NotOpen"));
00299     gp->jawLeftState.position.assign(1, gp->jawOpenPosition + 0.1);
00300     gp->jawRightState.position.assign(1, gp->jawOpenPosition + 0.1);
00301     gp->process_event(e_do());
00302     EXPECT_TRUE(scu::isActiveState(gp, "Open"));
00303 }
00304 
00305 TEST_F(GripperPositionStateMachineTest, Force)
00306 {
00308     EXPECT_TRUE(scu::isActiveState(gp, "LessThanForceThreshold"));
00309     EXPECT_FLOAT_EQ(gp->strongForce, gp->desiredForce);
00310     gp->process_event(e_moreThanForceThreshold());
00311     EXPECT_TRUE(scu::isActiveState(gp, "MoreThanForceThreshold"));
00312     EXPECT_FLOAT_EQ(gp->weakForce, gp->desiredForce);
00313     gp->process_event(e_lessThanForceThreshold());
00314     EXPECT_TRUE(scu::isActiveState(gp, "LessThanForceThreshold"));
00315 
00317     gp->jawLeftState.position.assign(1, gp->jawPositionForceThreshold + 0.1);
00318     gp->jawRightState.position.assign(1, gp->jawPositionForceThreshold + 0.1);
00319     gp->process_event(e_do());
00320     EXPECT_TRUE(scu::isActiveState(gp, "MoreThanForceThreshold"));
00321     gp->jawLeftState.position.assign(1, gp->jawPositionForceThreshold - 0.1);
00322     gp->jawRightState.position.assign(1, gp->jawPositionForceThreshold - 0.1);
00323     gp->process_event(e_do());
00324     EXPECT_TRUE(scu::isActiveState(gp, "LessThanForceThreshold"));
00325     gp->jawLeftState.position.assign(1, gp->jawPositionForceThreshold + 0.1);
00326     gp->jawRightState.position.assign(1, gp->jawPositionForceThreshold + 0.1);
00327     gp->process_event(e_do());
00328     EXPECT_TRUE(scu::isActiveState(gp, "MoreThanForceThreshold"));
00329 }
00330 
00331 TEST_F(GripperPositionStateMachineTest, SetParameters)
00332 {
00333     GripperSetpoint setPoint;
00334     setPoint.jawPosition       = 10;
00335     setPoint.jawPositionDelta  = 10;
00336     setPoint.expectedLoad      = 10;
00337     setPoint.expectedLoadDelta = 10;
00338 
00339     EXPECT_TRUE(scu::isActiveState(gp, "Inactive"));
00340     gp->process_event(c_setParameters(setPoint));
00341 
00342     EXPECT_FLOAT_EQ(10, gp->setPoint.jawPosition);
00343     EXPECT_FLOAT_EQ(10, gp->setPoint.jawPositionDelta);
00344     EXPECT_FLOAT_EQ(10, gp->setPoint.expectedLoad);
00345     EXPECT_FLOAT_EQ(10, gp->setPoint.expectedLoad);
00346 
00347     setPoint.jawPosition       = 15;
00348     setPoint.jawPositionDelta  = 15;
00349     setPoint.expectedLoad      = 15;
00350     setPoint.expectedLoadDelta = 15;
00351 
00352     gp->process_event(e_ready());
00353     EXPECT_TRUE(scu::isActiveState(gp, "Listening"));
00354     gp->process_event(c_setParameters(setPoint));
00355 
00356     EXPECT_FLOAT_EQ(15, gp->setPoint.jawPosition);
00357     EXPECT_FLOAT_EQ(15, gp->setPoint.jawPositionDelta);
00358     EXPECT_FLOAT_EQ(15, gp->setPoint.expectedLoad);
00359     EXPECT_FLOAT_EQ(15, gp->setPoint.expectedLoad);
00360 
00362     setPoint.jawPosition       = 20;
00363     setPoint.jawPositionDelta  = 20;
00364     setPoint.expectedLoad      = 20;
00365     setPoint.expectedLoadDelta = 20;
00366 
00367     gp->process_event(c_set());
00368     EXPECT_FALSE(scu::isActiveState(gp, "Inactive"));
00369     EXPECT_FALSE(scu::isActiveState(gp, "Listening"));
00370     gp->process_event(c_setParameters(setPoint));
00371 
00372     EXPECT_FLOAT_EQ(15, gp->setPoint.jawPosition);
00373     EXPECT_FLOAT_EQ(15, gp->setPoint.jawPositionDelta);
00374     EXPECT_FLOAT_EQ(15, gp->setPoint.expectedLoad);
00375     EXPECT_FLOAT_EQ(15, gp->setPoint.expectedLoad);
00376 }
00377 
00378 TEST_F(GripperPositionStateMachineTest, Command)
00379 {
00381     EXPECT_TRUE(scu::isActiveState(gp, "Inactive"));
00382     gp->process_event(e_ready());
00383     EXPECT_TRUE(scu::isActiveState(gp, "Active"));
00384     EXPECT_TRUE(scu::isActiveState(gp, "Stopped"));
00385     EXPECT_TRUE(scu::isActiveState(gp, "Listening"));
00386     EXPECT_FLOAT_EQ(0, gp->jointCommand.desiredPositionVelocityLimit[0]);
00387     gp->process_event(e_unready());
00388     EXPECT_TRUE(scu::isActiveState(gp, "Inactive"));
00389     EXPECT_FLOAT_EQ(0, gp->jointCommand.desiredPositionVelocityLimit[0]);
00390     gp->process_event(e_ready());
00391     EXPECT_TRUE(scu::isActiveState(gp, "Active"));
00392     gp->process_event(e_faulted());
00393     EXPECT_TRUE(scu::isActiveState(gp, "Inactive"));
00395     gp->process_event(e_ready());
00396     EXPECT_FALSE(scu::isActiveState(gp, "Active"));
00397 
00398     gp->process_event(e_unready());
00399     gp->process_event(e_notFaulted());
00400 
00402     gp->jointControlData.controlMode.state = r2_msgs::JointControlMode::DRIVE;
00403     gp->process_event(e_do());
00404     EXPECT_TRUE(scu::isActiveState(gp, "Active"));
00405     gp->jointControlData.controlMode.state = r2_msgs::JointControlMode::IGNORE;
00406     gp->process_event(e_do());
00407     EXPECT_TRUE(scu::isActiveState(gp, "Inactive"));
00410     gp->process_event(e_faulted());
00411     gp->jointControlData.controlMode.state = r2_msgs::JointControlMode::DRIVE;
00412     gp->process_event(e_do());
00413     EXPECT_FALSE(scu::isActiveState(gp, "Active"));
00414 }
00415 
00416 TEST_F(GripperPositionStateMachineTest, Movement)
00417 {
00419     gp->jointControlData.controlMode.state = r2_msgs::JointControlMode::DRIVE;
00420     gp->process_event(e_ready());
00421     EXPECT_TRUE(scu::isActiveState(gp, "Stopped"));
00422     EXPECT_FLOAT_EQ(0, gp->jointCommand.desiredPositionVelocityLimit[0]);
00423 
00424     gp->process_event(e_open());
00425     EXPECT_TRUE(scu::isActiveState(gp, "Opening"));
00426     gp->process_event(e_do());
00427     EXPECT_FLOAT_EQ(gp->encoderOpenVelocity, gp->jointCommand.desiredPositionVelocityLimit[0]);
00428     gp->process_event(e_stop());
00429     EXPECT_TRUE(scu::isActiveState(gp, "Stopped"));
00430     gp->process_event(e_do());
00431     EXPECT_FLOAT_EQ(0, gp->jointCommand.desiredPositionVelocityLimit[0]);
00432     gp->process_event(e_open());
00433     EXPECT_TRUE(scu::isActiveState(gp, "Opening"));
00434     gp->process_event(e_isOpen());
00435     EXPECT_TRUE(scu::isActiveState(gp, "Stopped"));
00436 
00437     gp->process_event(e_close());
00438     EXPECT_TRUE(scu::isActiveState(gp, "Closing"));
00439     gp->process_event(e_do());
00440     EXPECT_FLOAT_EQ(gp->encoderCloseVelocity, gp->jointCommand.desiredPositionVelocityLimit[0]);
00441     gp->process_event(e_stop());
00442     EXPECT_TRUE(scu::isActiveState(gp, "Stopped"));
00443     gp->process_event(e_do());
00444     EXPECT_FLOAT_EQ(0, gp->jointCommand.desiredPositionVelocityLimit[0]);
00445     gp->process_event(e_close());
00446     EXPECT_TRUE(scu::isActiveState(gp, "Closing"));
00447     gp->process_event(e_isClosed());
00448     EXPECT_TRUE(scu::isActiveState(gp, "Stopped"));
00449 
00450     gp->process_event(e_open());
00451     EXPECT_TRUE(scu::isActiveState(gp, "Opening"));
00452     gp->process_event(e_close());
00453     EXPECT_TRUE(scu::isActiveState(gp, "Closing"));
00454     gp->process_event(e_open());
00455     EXPECT_TRUE(scu::isActiveState(gp, "Opening"));
00456 
00459     gp->process_event(e_stop());
00460     gp->process_event(e_isNotOpen());
00461     gp->jointControlData.controlMode.state = r2_msgs::JointControlMode::DRIVE;
00462     gp->process_event(e_open());
00463     EXPECT_TRUE(scu::isActiveState(gp, "Opening"));
00464     gp->encoderState.position.assign(1, 16);
00465     gp->jawLeftState.position.assign(1, 0.2);
00466     gp->jawRightState.position.assign(1, 0.2);
00467     gp->process_event(e_do());
00468     gp->process_event(e_do());
00469     gp->process_event(e_do());
00470     gp->process_event(e_do());
00471     gp->jawLeftState.position.assign(1, 0.55);
00472     gp->jawRightState.position.assign(1, 0.55);
00473     gp->process_event(e_do());
00474     gp->process_event(e_do());
00475     gp->process_event(e_do());
00476     gp->process_event(e_do());
00477     EXPECT_TRUE(scu::isActiveState(gp, "Stopped"));
00478 }
00479 
00480 TEST_F(GripperPositionStateMachineTest, ActionSet)
00481 {
00483     gp->process_event(e_ready());
00484     gp->process_event(c_set());
00485     EXPECT_TRUE(scu::isActiveState(gp, "OpenWide"));
00486     gp->process_event(e_moving());
00487     gp->process_event(i_isOpen_open());
00488     gp->process_event(e_moreThanDesiredJawPosition());
00489     gp->process_event(e_stalled());
00490     EXPECT_TRUE(scu::isActiveState(gp, "JawPositionCheck"));
00491     gp->process_event(e_atDesiredJawPosition());
00493     EXPECT_FALSE(scu::isActiveState(gp, "Done"));
00494     EXPECT_TRUE(scu::isActiveState(gp, "Listening"));
00495 
00497     gp->process_event(e_isNotOpen());
00498     gp->process_event(e_lessThanDesiredJawPosition());
00499     EXPECT_TRUE(scu::isActiveState(gp, "NotOpen"));
00500     EXPECT_TRUE(scu::isActiveState(gp, "LessThanDesiredJawPosition"));
00501     gp->process_event(c_set());
00502     EXPECT_TRUE(scu::isActiveState(gp, "OpenWide"));
00503     gp->process_event(e_moving());
00504     gp->process_event(e_moreThanDesiredJawPosition());
00505     gp->process_event(e_isOpen());
00506     EXPECT_TRUE(scu::isActiveState(gp, "JawPositionCheck"));
00507     gp->process_event(e_atDesiredJawPosition());
00509     EXPECT_FALSE(scu::isActiveState(gp, "Done"));
00510     EXPECT_TRUE(scu::isActiveState(gp, "Listening"));
00511 
00513     EXPECT_TRUE(scu::isActiveState(gp, "Open"));
00514     gp->process_event(e_moreThanDesiredJawPosition());
00515     EXPECT_TRUE(scu::isActiveState(gp, "MoreThanDesiredJawPosition"));
00516     gp->process_event(c_set());
00517     EXPECT_FALSE(scu::isActiveState(gp, "OpenWide"));
00518     EXPECT_TRUE(scu::isActiveState(gp, "JawPositionCheck"));
00519     gp->process_event(e_atDesiredJawPosition());
00520     EXPECT_FALSE(scu::isActiveState(gp, "Done"));
00521     EXPECT_TRUE(scu::isActiveState(gp, "Listening"));
00522 
00524     EXPECT_TRUE(scu::isActiveState(gp, "Open"));
00525     EXPECT_TRUE(scu::isActiveState(gp, "AtDesiredJawPosition"));
00526     gp->process_event(c_set());
00527     EXPECT_FALSE(scu::isActiveState(gp, "OpenWide"));
00528     EXPECT_FALSE(scu::isActiveState(gp, "JawPositionCheck"));
00529     EXPECT_FALSE(scu::isActiveState(gp, "Done"));
00530     EXPECT_TRUE(scu::isActiveState(gp, "Listening"));
00531 
00533     gp->setPoint.jawPosition               = 0.25;
00534     gp->setPoint.jawPositionDelta          = 0.100001;
00535     gp->jointControlData.controlMode.state = r2_msgs::JointControlMode::DRIVE;
00536     gp->process_event(e_isNotOpen());
00537     gp->process_event(e_lessThanDesiredJawPosition());
00538     EXPECT_TRUE(scu::isActiveState(gp, "NotOpen"));
00539     EXPECT_TRUE(scu::isActiveState(gp, "LessThanDesiredJawPosition"));
00540     gp->process_event(c_set());
00541     EXPECT_TRUE(scu::isActiveState(gp, "OpenWide"));
00542     gp->encoderState.velocity.assign(1, 10);
00543     gp->process_event(e_do());
00544     gp->jawLeftState.position.assign(1, gp->jawOpenPosition);
00545     gp->jawRightState.position.assign(1, gp->jawOpenPosition);
00546     gp->process_event(e_do());
00547     EXPECT_TRUE(scu::isActiveState(gp, "MoreThanDesiredJawPosition"));
00548     EXPECT_TRUE(scu::isActiveState(gp, "JawPositionCheck"));
00549     gp->encoderState.velocity.assign(1, 0);
00550     gp->jawLeftState.position.assign(1, 0.25);
00551     gp->jawRightState.position.assign(1, 0.25);
00552     gp->process_event(e_do());
00553     EXPECT_TRUE(scu::isActiveState(gp, "Listening"));
00554 }
00555 
00556 TEST_F(GripperPositionStateMachineTest, ActionLock)
00557 {
00559     gp->process_event(e_ready());
00560     gp->process_event(e_unlocked());
00561     gp->process_event(e_moreThanDesiredJawPosition());
00562     gp->process_event(c_lock());
00563     EXPECT_TRUE(scu::isActiveState(gp, "Close"));
00564     gp->process_event(e_atDesiredJawPosition());
00565     EXPECT_TRUE(scu::isActiveState(gp, "LockCheck"));
00566     gp->process_event(e_locked());
00567     EXPECT_TRUE(scu::isActiveState(gp, "LockCheck"));
00568     gp->process_event(e_stalled());
00570     EXPECT_FALSE(scu::isActiveState(gp, "Done"));
00571     EXPECT_TRUE(scu::isActiveState(gp, "Listening"));
00572 
00574     gp->process_event(e_unlocked());
00575     gp->process_event(c_lock());
00576     EXPECT_FALSE(scu::isActiveState(gp, "Close"));
00577     EXPECT_TRUE(scu::isActiveState(gp, "LockCheck"));
00578     gp->process_event(e_locked());
00579     gp->process_event(e_stalled());
00580     EXPECT_FALSE(scu::isActiveState(gp, "Done"));
00581     EXPECT_TRUE(scu::isActiveState(gp, "Listening"));
00582 
00584     gp->process_event(c_lock());
00585     gp->process_event(e_stalled());
00586     EXPECT_FALSE(scu::isActiveState(gp, "Close"));
00587     EXPECT_FALSE(scu::isActiveState(gp, "LockCheck"));
00588     EXPECT_FALSE(scu::isActiveState(gp, "Done"));
00589     EXPECT_TRUE(scu::isActiveState(gp, "Listening"));
00590 
00592     gp->jointControlData.controlMode.state = r2_msgs::JointControlMode::DRIVE;
00593     gp->process_event(e_unlocked());
00594     gp->process_event(e_moreThanDesiredJawPosition());
00595     gp->process_event(c_lock());
00596     EXPECT_TRUE(scu::isActiveState(gp, "Close"));
00598     gp->process_event(e_atDesiredJawPosition());
00599     EXPECT_TRUE(scu::isActiveState(gp, "LockCheck"));
00600     gp->process_event(e_locked());
00601     gp->process_event(e_stalled());
00602     EXPECT_TRUE(scu::isActiveState(gp, "Listening"));
00603 }
00604 
00605 TEST_F(GripperPositionStateMachineTest, ActionRelease)
00606 {
00609     gp->process_event(e_ready());
00610     gp->process_event(c_release());
00611     EXPECT_TRUE(scu::isActiveState(gp, "OpenUntilStall"));
00612     gp->process_event(e_stalled());
00614     EXPECT_FALSE(scu::isActiveState(gp, "Done"));
00615     EXPECT_TRUE(scu::isActiveState(gp, "Listening"));
00616 
00618     gp->process_event(c_release());
00619     EXPECT_TRUE(scu::isActiveState(gp, "OpenUntilStall"));
00620     gp->process_event(e_isOpen());
00621     EXPECT_FALSE(scu::isActiveState(gp, "Done"));
00622     EXPECT_TRUE(scu::isActiveState(gp, "Listening"));
00623 
00625     gp->process_event(e_isOpen());
00626     gp->process_event(c_release());
00627     EXPECT_FALSE(scu::isActiveState(gp, "OpenUntilStall"));
00628     EXPECT_FALSE(scu::isActiveState(gp, "Done"));
00629     EXPECT_TRUE(scu::isActiveState(gp, "Listening"));
00630 
00632     gp->jointControlData.controlMode.state = r2_msgs::JointControlMode::DRIVE;
00633     gp->process_event(e_isNotOpen());
00634     gp->process_event(e_notStalled());
00635     gp->process_event(c_release());
00636     EXPECT_TRUE(scu::isActiveState(gp, "OpenUntilStall"));
00638     setFloat(gp->measuredCurrentName, 10);
00639     gp->encoderState.velocity.assign(1, 0);
00640     gp->process_event(e_do());
00641     EXPECT_TRUE(scu::isActiveState(gp, "Listening"));
00642 }
00643 
00644 TEST_F(GripperPositionStateMachineTest, ActionInterrupt)
00645 {
00646     gp->process_event(e_ready());
00647     gp->process_event(c_set());
00648     EXPECT_TRUE(scu::isActiveState(gp, "OpenWide"));
00649     gp->process_event(e_moreThanDesiredJawPosition());
00650     gp->process_event(c_lock());
00651     EXPECT_TRUE(scu::isActiveState(gp, "Close"));
00652     gp->process_event(c_release());
00653     EXPECT_TRUE(scu::isActiveState(gp, "OpenUntilStall"));
00654 }
00655 
00656 TEST_F(GripperPositionStateMachineTest, ActionOpenWideFailure)
00657 {
00659     gp->process_event(e_ready());
00660     gp->process_event(c_set());
00661     EXPECT_TRUE(scu::isActiveState(gp, "OpenWide"));
00662     gp->process_event(e_stalled());
00663     EXPECT_TRUE(scu::isActiveState(gp, "Listening"));
00664 
00665     gp->process_event(c_set());
00666     EXPECT_TRUE(scu::isActiveState(gp, "OpenWide"));
00667     gp->process_event(e_moreThanDesiredJawPosition());
00668     gp->process_event(i_isOpen_open());
00669     EXPECT_FALSE(scu::isActiveState(gp, "NotOpen"));
00670     gp->process_event(e_stalled());
00671     EXPECT_FALSE(scu::isActiveState(gp, "Listening"));
00672 }
00673 
00674 TEST_F(GripperPositionStateMachineTest, ActionJawPositionCheckFailure)
00675 {
00677     gp->process_event(e_ready());
00678     gp->process_event(c_set());
00679     EXPECT_TRUE(scu::isActiveState(gp, "OpenWide"));
00680     gp->process_event(e_moreThanDesiredJawPosition());
00681     gp->process_event(e_isOpen());
00682     EXPECT_TRUE(scu::isActiveState(gp, "JawPositionCheck"));
00683     gp->process_event(e_stalled());
00684     EXPECT_TRUE(scu::isActiveState(gp, "Listening"));
00685 
00686     gp->process_event(e_isNotOpen());
00687     gp->process_event(c_set());
00688     EXPECT_TRUE(scu::isActiveState(gp, "OpenWide"));
00689     gp->process_event(e_isOpen());
00690     EXPECT_TRUE(scu::isActiveState(gp, "JawPositionCheck"));
00691     gp->process_event(i_atDesiredJawPosition_position());
00692     EXPECT_FALSE(scu::isActiveState(gp, "MoreThanDesiredJawPosition"));
00693     gp->process_event(e_stalled());
00694     EXPECT_FALSE(scu::isActiveState(gp, "Listening"));
00695 
00697 }
00698 
00699 TEST_F(GripperPositionStateMachineTest, ActionCloseFailure)
00700 {
00702     gp->process_event(e_ready());
00703     gp->process_event(e_moreThanDesiredJawPosition());
00704     gp->process_event(c_lock());
00705     EXPECT_TRUE(scu::isActiveState(gp, "Close"));
00706     gp->process_event(e_stalled());
00707     EXPECT_TRUE(scu::isActiveState(gp, "Listening"));
00708 
00709     gp->process_event(c_lock());
00710     EXPECT_TRUE(scu::isActiveState(gp, "Close"));
00711     gp->process_event(i_atDesiredJawPosition_position());
00712     EXPECT_FALSE(scu::isActiveState(gp, "MoreThanDesiredJawPosition"));
00713     gp->process_event(e_stalled());
00714     EXPECT_FALSE(scu::isActiveState(gp, "Listening"));
00715 
00717 }
00718 
00719 TEST_F(GripperPositionStateMachineTest, ActionLockCheckFailure)
00720 {
00722     gp->process_event(e_ready());
00723     gp->process_event(e_unlocked());
00724     gp->process_event(e_moreThanDesiredJawPosition());
00725     gp->process_event(c_lock());
00726     EXPECT_TRUE(scu::isActiveState(gp, "Close"));
00727     gp->process_event(e_atDesiredJawPosition());
00728     EXPECT_TRUE(scu::isActiveState(gp, "LockCheck"));
00729     gp->process_event(e_stalled());
00730     EXPECT_TRUE(scu::isActiveState(gp, "Listening"));
00731 
00732     gp->process_event(e_moreThanDesiredJawPosition());
00733     gp->process_event(c_lock());
00734     EXPECT_TRUE(scu::isActiveState(gp, "Close"));
00735     gp->process_event(e_atDesiredJawPosition());
00736     EXPECT_TRUE(scu::isActiveState(gp, "LockCheck"));
00737     gp->process_event(i_locked_lock());
00738     EXPECT_FALSE(scu::isActiveState(gp, "Unlocked"));
00739     gp->process_event(e_stalled());
00740     EXPECT_TRUE(scu::isActiveState(gp, "Listening"));
00741 
00743 }
00744 
00745 int main(int argc, char** argv)
00746 {
00747     testing::InitGoogleTest(&argc, argv);
00748     return RUN_ALL_TESTS();
00749 }

robodyn_mechanisms
Author(s):
autogenerated on Thu Jun 6 2019 21:22:48