GripperSupervisorTest.cpp
Go to the documentation of this file.
00001 #include <gtest/gtest.h>
00002 #include "robodyn_mechanisms/GripperSupervisor.h"
00003 #include <ros/package.h>
00004 
00005 class GripperSupervisorTest : public ::testing::Test
00006 {
00007 protected:
00008     virtual void SetUp()
00009     {
00010         ros::Time::init();
00011 
00012         grippers.push_back("r2/left_leg/gripper/joint0");
00013         grippers.push_back("r2/right_leg/gripper/joint0");
00014 
00015         gripperState.name.push_back("r2/left_leg/gripper/joint0");
00016         gripperState.name.push_back("r2/right_leg/gripper/joint0");
00017         gripperState.locked.push_back(1);
00018         gripperState.locked.push_back(0);
00019         gripperState.loaded.push_back(1);
00020         gripperState.loaded.push_back(1);
00021 
00022         baseFrames.data.push_back("r2/left_leg/gripper/tip");
00023 
00024         goalStat.goal_id.id = "r2/left_leg/gripper/joint0";
00025         goalStat.goal_id.stamp = ros::Time::now();
00026         goalStat.status = actionlib_msgs::GoalStatus::SUCCEEDED;
00027         goalStati.status_list.push_back(goalStat);
00028         goalStat.goal_id.id = "r2/right_leg/gripper/joint0";
00029         goalStat.status = actionlib_msgs::GoalStatus::PENDING;
00030         goalStati.status_list.push_back(goalStat);
00031         goalStat.status = actionlib_msgs::GoalStatus::ABORTED;
00032         goalStati.status_list.push_back(goalStat);
00033 
00034         jntMode.controlMode.state = r2_msgs::JointControlMode::PARK;
00035         jntMode.coeffState.state = r2_msgs::JointControlCoeffState::LOADED;
00036         jntModeList.joint.push_back("r2/left_leg/gripper/joint0");
00037         jntModeList.data.push_back(jntMode);
00038         jntMode.controlMode.state = r2_msgs::JointControlMode::FAULTED;
00039         jntMode.coeffState.state = r2_msgs::JointControlCoeffState::NOTLOADED;
00040         jntModeList.joint.push_back("r2/right_leg/gripper/joint0");
00041         jntModeList.data.push_back(jntMode);
00042         jntMode.controlMode.state = r2_msgs::JointControlMode::DRIVE;
00043         jntModeList.joint.push_back("joint1");
00044         jntModeList.data.push_back(jntMode);
00045 
00046         gripCmd.name.push_back("r2/left_leg/gripper/joint0");
00047         gripCmd.setpoint.push_back("null");
00048         gripCmd.command.push_back("release");
00049         gripCmd.name.push_back("r2/right_leg/gripper/joint0");
00050         gripCmd.setpoint.push_back("null");
00051         gripCmd.command.push_back("lock");
00052 
00053         driveRequest.data.push_back("r2/left_leg/gripper/joint0");
00054 
00055     }
00056 
00057     virtual void TearDown()
00058     {
00059     }
00060 
00061     GripperSupervisor                           gs;
00062     std::vector<std::string>                    grippers;
00063     r2_msgs::GripperPositionState   gripperState;
00064     r2_msgs::StringArray            baseFrames;
00065     actionlib_msgs::GoalStatusArray             goalStati;
00066     actionlib_msgs::GoalStatus                  goalStat;
00067     r2_msgs::JointControlDataArray  jntModeList;
00068     r2_msgs::JointControlData       jntMode;
00069     r2_msgs::GripperPositionCommand gripCmd;
00070     r2_msgs::StringArray            driveRequest, requestOut;
00071 };
00072 
00073 TEST_F(GripperSupervisorTest, InitializeMapsTest)
00074 {
00075 
00076     // Initialize maps with test tree
00077     gs.initializeMaps(grippers);
00078 
00079     ASSERT_FALSE(gs.endEffectorMap.empty());
00080     EXPECT_EQ (0, gs.endEffectorMap.count("joint1"));
00081     EXPECT_EQ (IDLE, gs.getState("r2/left_leg/gripper/joint0"));
00082     EXPECT_EQ (0, gs.gripperJointModeMap.count("link1"));
00083 }
00084 
00085 TEST_F(GripperSupervisorTest, PopulateGripperStateInfoTest)
00086 {
00087     gs.initializeMaps(grippers);
00088 
00089     gs.populateGripperStateInfo(gripperState);
00090 
00091     EXPECT_EQ(1, gs.endEffectorMap["r2/left_leg/gripper/joint0"].locked);
00092     EXPECT_EQ(1, gs.endEffectorMap["r2/right_leg/gripper/joint0"].loaded);
00093 
00094 }
00095 
00096 TEST_F(GripperSupervisorTest, PopulateBaseFrameInfoTest)
00097 {
00098     gs.initializeMaps(grippers);
00099 
00100     gs.populateBaseFrameInfo(baseFrames);
00101 
00102     EXPECT_EQ(1, gs.baseFrameList.size());
00103     EXPECT_EQ("r2/left_leg/gripper/tip", gs.baseFrameList[0]);
00104 
00105     gs.populateBaseFrameInfo(baseFrames);
00106     EXPECT_EQ(1, gs.baseFrameList.size());
00107 
00108 }
00109 
00110 TEST_F(GripperSupervisorTest, PopulateCheckedGrippersTest)
00111 {
00112     gs.initializeMaps(grippers);
00113 
00114     gs.populateCheckedGrippers(true);
00115 
00116     EXPECT_TRUE(gs.gripperDriveApprovedMap["r2/left_leg/gripper/joint0"]);
00117     EXPECT_TRUE(gs.gripperDriveApprovedMap["r2/right_leg/gripper/joint0"]);
00118 
00119     gs.populateCheckedGrippers(driveRequest);
00120 
00121     EXPECT_TRUE(gs.gripperDriveApprovedMap["r2/left_leg/gripper/joint0"]);
00122     EXPECT_FALSE(gs.gripperDriveApprovedMap["r2/right_leg/gripper/joint0"]);
00123 
00124 }
00125 
00126 TEST_F(GripperSupervisorTest, PopulateGoalStatusTest)
00127 {
00128     gs.initializeMaps(grippers);
00129 
00130     gs.populateGoalStatus(goalStati);
00131 
00132     EXPECT_EQ(2, gs.goalStatuses.status_list.size());
00133     EXPECT_EQ(actionlib_msgs::GoalStatus::SUCCEEDED, gs.goalStatuses.status_list[0].status);
00134     EXPECT_EQ("r2/right_leg/gripper/joint0", gs.goalStatuses.status_list[1].goal_id.id);
00135 }
00136 
00137 TEST_F(GripperSupervisorTest, PopulateJointControlModeTest)
00138 {
00139     gs.initializeMaps(grippers);
00140 
00141     gs.populateJointControlMode(jntModeList);
00142 
00143     EXPECT_EQ(0, gs.gripperJointModeMap.count("joint1"));
00144     EXPECT_EQ(r2_msgs::JointControlMode::PARK, gs.gripperJointModeMap["r2/left_leg/gripper/joint0"].state);
00145     EXPECT_EQ(1, gs.gripperJointModeMap.count("r2/right_leg/gripper/joint0"));
00146 }
00147 
00148 TEST_F(GripperSupervisorTest, PopulateGripperCommandsInTest)
00149 {
00150     gs.initializeMaps(grippers);
00151 
00152     gs.populateGripperCommandsIn(gripCmd);
00153 
00154     EXPECT_EQ(2, gs.gripperCmds.name.size());
00155     EXPECT_EQ("release", gs.gripperCmds.command[0]);
00156     EXPECT_EQ("r2/right_leg/gripper/joint0", gs.gripperCmds.name[1]);
00157 }
00158 
00159 TEST_F(GripperSupervisorTest, CheckTransitionsTest)
00160 {
00161 
00162     r2_msgs::JointControlDataArray modeCmdOut;
00163     r2_msgs::GripperPositionCommand gripCmdOut;
00164     actionlib_msgs::GoalStatusArray statusOut;
00165 
00166     gs.initializeMaps(grippers);
00167     gs.populateBaseFrameInfo(baseFrames);
00168     gs.populateGripperStateInfo(gripperState);
00169     gs.populateCheckedGrippers(true);
00170     gs.setLimits(255, 400);
00171 
00172     // check coeffs not loaded response
00173     gs.populateJointControlMode(jntModeList);
00174     gs.checkTransitions(0);
00175     gs.populateJointControlCommandMsg(modeCmdOut);
00176     gs.populateGoalStatusMsg(statusOut);
00177     gs.populateGripperCommandMsg(gripCmdOut);
00178     EXPECT_EQ(IDLE, gs.getState("r2/right_leg/gripper/joint0"));
00179     EXPECT_EQ(IDLE, gs.getState("r2/left_leg/gripper/joint0"));
00180     ASSERT_EQ(0, modeCmdOut.joint.size());  // since not in drive
00181     ASSERT_EQ(0, gripCmdOut.name.size());
00182 
00183     // check joint fault bit response
00184     jntModeList.data[1].coeffState.state = r2_msgs::JointControlCoeffState::LOADED;
00185     gs.populateJointControlMode(jntModeList);
00186     gs.checkTransitions(0);
00187     gs.populateJointControlCommandMsg(modeCmdOut);
00188     gs.populateGoalStatusMsg(statusOut);
00189     gs.populateGripperCommandMsg(gripCmdOut);
00190     EXPECT_EQ(FAULT, gs.getState("r2/right_leg/gripper/joint0"));
00191     EXPECT_EQ(IDLE, gs.getState("r2/left_leg/gripper/joint0"));
00192     ASSERT_EQ(0, modeCmdOut.joint.size());  // since not in drive
00193     ASSERT_EQ(1, gripCmdOut.name.size());
00194     EXPECT_EQ("c_cancel", gripCmdOut.command[0]);
00195     EXPECT_EQ("null", gripCmdOut.setpoint[0]);
00196 
00197     // check running this with no commands (should be no change)
00198     gs.checkTransitions(0);
00199     gs.populateJointControlCommandMsg(modeCmdOut);
00200     gs.populateGoalStatusMsg(statusOut);
00201     gs.populateGripperCommandMsg(gripCmdOut);
00202     EXPECT_EQ(FAULT, gs.getState("r2/right_leg/gripper/joint0"));
00203     EXPECT_EQ(IDLE, gs.getState("r2/left_leg/gripper/joint0"));
00204     ASSERT_EQ(0, modeCmdOut.joint.size());
00205     ASSERT_EQ(0, gripCmdOut.name.size());
00206 
00207     // Remove the fault from the current joint
00208     jntModeList.data[1].controlMode.state = r2_msgs::JointControlMode::PARK;
00209     gs.populateJointControlMode(jntModeList);
00210     gs.checkTransitions(0);
00211     gs.populateJointControlCommandMsg(modeCmdOut);
00212     gs.populateGoalStatusMsg(statusOut);
00213     gs.populateGripperCommandMsg(gripCmdOut);
00214     EXPECT_EQ(IDLE, gs.getState("r2/right_leg/gripper/joint0"));
00215     ASSERT_EQ(0, modeCmdOut.joint.size());
00216     ASSERT_EQ(0, gripCmdOut.name.size());
00217 
00218     // Send in some goal status
00219     jntModeList.data[0].controlMode.state = r2_msgs::JointControlMode::DRIVE;
00220     jntModeList.data[1].controlMode.state = r2_msgs::JointControlMode::DRIVE;
00221     gs.populateJointControlMode(jntModeList);
00222     gs.populateGoalStatus(goalStati);
00223     gs.checkTransitions(0);
00224     gs.populateJointControlCommandMsg(modeCmdOut);
00225     gs.populateGoalStatusMsg(statusOut);
00226     gs.populateGripperCommandMsg(gripCmdOut);
00227     EXPECT_EQ(IDLE, gs.getState("r2/left_leg/gripper/joint0"));
00228     EXPECT_EQ(FAILURE, gs.getState("r2/right_leg/gripper/joint0"));
00229     ASSERT_EQ(2, modeCmdOut.joint.size());
00230     EXPECT_EQ(r2_msgs::JointControlMode::PARK, modeCmdOut.data[1].controlMode.state);
00231     ASSERT_EQ(0, gripCmdOut.name.size());
00232 
00233     // Send two gripper commands, one invalid
00234     jntModeList.data[0].controlMode.state = r2_msgs::JointControlMode::PARK;
00235     jntModeList.data[1].controlMode.state = r2_msgs::JointControlMode::PARK;
00236     gs.populateJointControlMode(jntModeList);
00237     gs.populateGripperCommandsIn(gripCmd);
00238     gs.checkTransitions(0);
00239     gs.populateJointControlCommandMsg(modeCmdOut);
00240     gs.populateGoalStatusMsg(statusOut);
00241     gs.populateGripperCommandMsg(gripCmdOut);
00242     EXPECT_EQ(IDLE, gs.getState("r2/left_leg/gripper/joint0"));
00243     EXPECT_EQ(FAILURE, gs.getState("r2/right_leg/gripper/joint0"));
00244     ASSERT_EQ(0, modeCmdOut.joint.size());
00245     ASSERT_EQ(0, gripCmdOut.name.size());
00246     //EXPECT_EQ(actionlib_msgs::GoalStatus::REJECTED, statusOut.status_list[0].status);
00247 
00248     // Reset the failed joint
00249     gripCmd.name.clear();
00250     gripCmd.setpoint.clear();
00251     gripCmd.command.clear();
00252     gripCmd.name.push_back("r2/right_leg/gripper/joint0");
00253     gripCmd.setpoint.push_back("null");
00254     gripCmd.command.push_back("reset");
00255     gs.populateGripperCommandsIn(gripCmd);
00256     gs.checkTransitions(0);
00257     gs.populateJointControlCommandMsg(modeCmdOut);
00258     gs.populateGoalStatusMsg(statusOut);
00259     gs.populateGripperCommandMsg(gripCmdOut);
00260     EXPECT_EQ(IDLE, gs.getState("r2/right_leg/gripper/joint0"));
00261     ASSERT_EQ(0, modeCmdOut.joint.size());
00262     ASSERT_EQ(0, gripCmdOut.name.size());
00263 
00264     // close right_leg/gripper/joint0
00265     gripCmd.name.clear();
00266     gripCmd.setpoint.clear();
00267     gripCmd.command.clear();
00268     gripCmd.name.push_back("r2/right_leg/gripper/joint0");
00269     gripCmd.setpoint.push_back("closed");
00270     gripCmd.command.push_back("set");
00271     gs.populateGripperCommandsIn(gripCmd);
00272     gs.checkTransitions(0);
00273     gs.populateJointControlCommandMsg(modeCmdOut);
00274     gs.populateGoalStatusMsg(statusOut);
00275     gs.populateGripperCommandMsg(gripCmdOut);
00276     EXPECT_EQ(SET, gs.getState("r2/right_leg/gripper/joint0"));
00277     ASSERT_EQ(1, modeCmdOut.joint.size());
00278     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, modeCmdOut.data[0].controlMode.state);
00279     EXPECT_EQ(0, gripCmdOut.name.size());
00280 
00281     // send drive command
00282     jntModeList.data[1].controlMode.state = r2_msgs::JointControlMode::DRIVE;
00283     gs.populateJointControlMode(jntModeList);
00284     gs.checkTransitions(0);
00285     gs.populateJointControlCommandMsg(modeCmdOut);
00286     gs.populateGoalStatusMsg(statusOut);
00287     gs.populateGripperCommandMsg(gripCmdOut);
00288     EXPECT_EQ(SET_PENDING, gs.getState("r2/right_leg/gripper/joint0"));
00289     EXPECT_EQ(0, modeCmdOut.joint.size());
00290     ASSERT_EQ(1, gripCmdOut.name.size());
00291     EXPECT_EQ("c_set", gripCmdOut.command[0]);
00292     EXPECT_EQ(255, gripCmdOut.dutyCycle[0]);
00293 
00294     // send success
00295     goalStati.status_list.clear();
00296     goalStat.goal_id.id = "r2/right_leg/gripper/joint0";
00297     goalStat.goal_id.stamp = ros::Time::now();
00298     goalStat.status = actionlib_msgs::GoalStatus::SUCCEEDED;
00299     goalStati.status_list.push_back(goalStat);
00300     gs.populateGoalStatus(goalStati);
00301     gs.checkTransitions(0);
00302     gs.populateJointControlCommandMsg(modeCmdOut);
00303     gs.populateGoalStatusMsg(statusOut);
00304     gs.populateGripperCommandMsg(gripCmdOut);
00305     EXPECT_EQ(IDLE, gs.getState("r2/right_leg/gripper/joint0"));
00306     EXPECT_EQ(0, modeCmdOut.joint.size());
00307     EXPECT_EQ(0, gripCmdOut.name.size());
00308 
00309     // lock right_leg/gripper/joint0
00310     gripCmd.name.clear();
00311     gripCmd.setpoint.clear();
00312     gripCmd.command.clear();
00313     gripCmd.name.push_back("r2/right_leg/gripper/joint0");
00314     gripCmd.setpoint.push_back("Dustin smells funny");
00315     gripCmd.command.push_back("lock");
00316     gs.populateGripperCommandsIn(gripCmd);
00317     //gs.populateGripperStateInfo(gripperState);
00318     gs.checkTransitions(0);
00319     gs.populateJointControlCommandMsg(modeCmdOut);
00320     gs.populateGoalStatusMsg(statusOut);
00321     gs.populateGripperCommandMsg(gripCmdOut);
00322     EXPECT_EQ(LOCK_PENDING, gs.getState("r2/right_leg/gripper/joint0"));
00323     ASSERT_EQ(1, gripCmdOut.name.size());
00324     EXPECT_EQ(0, modeCmdOut.joint.size());
00325     EXPECT_EQ("c_lock", gripCmdOut.command[0]);
00326     EXPECT_EQ("Dustin smells funny", gripCmdOut.setpoint[0]);
00327     EXPECT_EQ(255, gripCmdOut.dutyCycle[0]);
00328 
00329     // attempt to lock left_leg/gripper/joint0
00330     gripCmd.name.clear();
00331     gripCmd.setpoint.clear();
00332     gripCmd.command.clear();
00333     gripCmd.name.push_back("r2/left_leg/gripper/joint0");
00334     gripCmd.setpoint.push_back("Dustin smells funny");
00335     gripCmd.command.push_back("lock");
00336     gs.populateGripperCommandsIn(gripCmd);
00337     gs.checkTransitions(0);
00338     gs.populateJointControlCommandMsg(modeCmdOut);
00339     gs.populateGoalStatusMsg(statusOut);
00340     gs.populateGripperCommandMsg(gripCmdOut);
00341     EXPECT_EQ(0, modeCmdOut.joint.size());
00342     EXPECT_EQ(0, gripCmdOut.name.size());
00343 
00344     // send success
00345     goalStati.status_list.clear();
00346     goalStat.goal_id.id = "r2/right_leg/gripper/joint0";
00347     goalStat.goal_id.stamp = ros::Time::now();
00348     goalStat.status = actionlib_msgs::GoalStatus::SUCCEEDED;
00349     goalStati.status_list.push_back(goalStat);
00350     gs.populateGoalStatus(goalStati);
00351     gripperState.locked[1] = 1;
00352     gs.populateGripperStateInfo(gripperState);
00353     gs.checkTransitions(0);
00354     gs.populateJointControlCommandMsg(modeCmdOut);
00355     gs.populateGoalStatusMsg(statusOut);
00356     gs.populateGripperCommandMsg(gripCmdOut);
00357     EXPECT_EQ(LOCK_PENDING, gs.getState("r2/right_leg/gripper/joint0"));
00358     ASSERT_EQ(1, modeCmdOut.joint.size());
00359     EXPECT_EQ(0, gripCmdOut.name.size());
00360     EXPECT_EQ(r2_msgs::JointControlMode::PARK, modeCmdOut.data[0].controlMode.state);
00361 
00362     jntModeList.data[1].controlMode.state = r2_msgs::JointControlMode::PARK;
00363     gs.populateJointControlMode(jntModeList);
00364     gs.checkTransitions(0);
00365     EXPECT_EQ(IDLE, gs.getState("r2/right_leg/gripper/joint0"));
00366 
00367     // release left_leg/gripper/joint0
00368     gs.populateCheckedGrippers(false);
00369     baseFrames.data.push_back("r2/right_leg/gripper/tip");
00370     gs.populateBaseFrameInfo(baseFrames);
00371     gripCmd.name.clear();
00372     gripCmd.setpoint.clear();
00373     gripCmd.command.clear();
00374     gripCmd.name.push_back("r2/left_leg/gripper/joint0");
00375     gripCmd.setpoint.push_back("null");
00376     gripCmd.command.push_back("release");
00377     gs.populateGripperCommandsIn(gripCmd);
00378     gs.checkTransitions(0);
00379     gs.populateJointControlCommandMsg(modeCmdOut);
00380     gs.populateGoalStatusMsg(statusOut);
00381     gs.populateGripperCommandMsg(gripCmdOut);
00382     gs.populateDriveRequestMsg(requestOut);
00383     EXPECT_EQ(REQUEST_PENDING, gs.getState("r2/left_leg/gripper/joint0"));
00384     EXPECT_EQ(0, modeCmdOut.joint.size());
00385     EXPECT_EQ(0, gripCmdOut.name.size());
00386     ASSERT_EQ(1, requestOut.data.size());
00387     EXPECT_EQ("r2/left_leg/gripper/joint0", requestOut.data[0]);
00388 
00389     // send in permission
00390     gs.populateCheckedGrippers(driveRequest);
00391     gs.checkTransitions(0);
00392     gs.populateJointControlCommandMsg(modeCmdOut);
00393     gs.populateGoalStatusMsg(statusOut);
00394     gs.populateGripperCommandMsg(gripCmdOut);
00395     gs.populateDriveRequestMsg(requestOut);
00396     EXPECT_EQ(RELEASE, gs.getState("r2/left_leg/gripper/joint0"));
00397     ASSERT_EQ(1, modeCmdOut.joint.size());
00398     EXPECT_EQ(0, gripCmdOut.name.size());
00399     EXPECT_EQ(0, requestOut.data.size());
00400     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, modeCmdOut.data[0].controlMode.state);
00401 
00402     // send failure
00403     gs.populateCheckedGrippers(true);
00404     jntModeList.data[0].controlMode.state = r2_msgs::JointControlMode::DRIVE;
00405     gs.populateJointControlMode(jntModeList);
00406     goalStati.status_list.clear();
00407     goalStat.goal_id.id = "r2/left_leg/gripper/joint0";
00408     goalStat.goal_id.stamp = ros::Time::now();
00409     goalStat.status = actionlib_msgs::GoalStatus::ABORTED;
00410     goalStati.status_list.push_back(goalStat);
00411     gs.populateGoalStatus(goalStati);
00412     gs.checkTransitions(0);
00413     gs.populateJointControlCommandMsg(modeCmdOut);
00414     gs.populateGoalStatusMsg(statusOut);
00415     gs.populateGripperCommandMsg(gripCmdOut);
00416     EXPECT_EQ(FAILURE, gs.getState("r2/left_leg/gripper/joint0"));
00417     ASSERT_EQ(1, modeCmdOut.joint.size());
00418     EXPECT_EQ(0, gripCmdOut.name.size());
00419     EXPECT_EQ(r2_msgs::JointControlMode::PARK, modeCmdOut.data[0].controlMode.state);
00420 
00421     // send reset
00422     jntModeList.data[0].controlMode.state = r2_msgs::JointControlMode::PARK;
00423     jntModeList.data[1].controlMode.state = r2_msgs::JointControlMode::PARK;
00424     gs.populateJointControlMode(jntModeList);
00425     gripCmd.name.clear();
00426     gripCmd.setpoint.clear();
00427     gripCmd.command.clear();
00428     gripCmd.name.push_back("r2/left_leg/gripper/joint0");
00429     gripCmd.setpoint.push_back("null");
00430     gripCmd.command.push_back("reset");
00431     gs.populateGripperCommandsIn(gripCmd);
00432     gs.checkTransitions(0);
00433     gs.populateJointControlCommandMsg(modeCmdOut);
00434     gs.populateGoalStatusMsg(statusOut);
00435     gs.populateGripperCommandMsg(gripCmdOut);
00436     EXPECT_EQ(IDLE, gs.getState("r2/left_leg/gripper/joint0"));
00437     ASSERT_EQ(0, modeCmdOut.joint.size());
00438     EXPECT_EQ(0, gripCmdOut.name.size());
00439 
00440     // send two open commands at a time
00441     gripCmd.name.clear();
00442     gripCmd.setpoint.clear();
00443     gripCmd.command.clear();
00444     gripCmd.name.push_back("r2/left_leg/gripper/joint0");
00445     gripCmd.setpoint.push_back("null");
00446     gripCmd.command.push_back("release");
00447     gripCmd.name.push_back("r2/right_leg/gripper/joint0");
00448     gripCmd.setpoint.push_back("null");
00449     gripCmd.command.push_back("release");
00450     gs.populateGripperCommandsIn(gripCmd);
00451     gs.checkTransitions(0);
00452     gs.populateJointControlCommandMsg(modeCmdOut);
00453     gs.populateGoalStatusMsg(statusOut);
00454     gs.populateGripperCommandMsg(gripCmdOut);
00455     EXPECT_EQ(RELEASE, gs.getState("r2/left_leg/gripper/joint0"));
00456     EXPECT_EQ(IDLE, gs.getState("r2/right_leg/gripper/joint0"));
00457     ASSERT_EQ(1, modeCmdOut.joint.size());
00458     EXPECT_EQ(0, gripCmdOut.name.size());
00459     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, modeCmdOut.data[0].controlMode.state);
00460 
00461 }
00462 
00463 int main(int argc, char** argv)
00464 {
00465     ::testing::InitGoogleTest(&argc, argv);
00466     return RUN_ALL_TESTS();
00467 }

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