00001 #include <gtest/gtest.h> 00002 #include "robodyn_mechanisms/GripperStatusChecker.h" 00003 #include <ros/package.h> 00004 00005 class GripperStatusCheckerTest : 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 gsc.initializeMap(grippers); 00016 gsc.setTimingParameters(0.5, 2.0); 00017 00018 pReq.data.push_back("r2/left_leg/gripper/joint0"); 00019 cReq.data.push_back("r2/right_leg/gripper/joint0"); 00020 00021 jntMode.controlMode.state = r2_msgs::JointControlMode::PARK; 00022 jntModeList.joint.push_back("r2/left_leg/gripper/joint0"); 00023 jntModeList.data.push_back(jntMode); 00024 jntMode.controlMode.state = r2_msgs::JointControlMode::FAULTED; 00025 jntModeList.joint.push_back("r2/right_leg/gripper/joint0"); 00026 jntModeList.data.push_back(jntMode); 00027 jntMode.controlMode.state = r2_msgs::JointControlMode::DRIVE; 00028 jntModeList.joint.push_back("joint1"); 00029 jntModeList.data.push_back(jntMode); 00030 00031 } 00032 00033 virtual void TearDown() 00034 { 00035 } 00036 00037 GripperStatusChecker gsc; 00038 std::vector<std::string> grippers; 00039 r2_msgs::StringArray pReq, cReq, eCheck; 00040 r2_msgs::JointControlDataArray jntModeList; 00041 r2_msgs::JointControlData jntMode; 00042 diagnostic_msgs::DiagnosticArray sweStopMsg; 00043 GripperStatusChecker::GripperStatusCheckInfo info; 00044 }; 00045 00046 TEST_F(GripperStatusCheckerTest, PopulateJointControlInfoTest) 00047 { 00048 gsc.populateJointControlPilotInfo(jntModeList); 00049 ASSERT_TRUE(gsc.getMapEntry(grippers[1], info)); 00050 EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, info.pilotMode); 00051 gsc.populateJointControlCaptainInfo(jntModeList); 00052 ASSERT_TRUE(gsc.getMapEntry(grippers[0], info)); 00053 EXPECT_EQ(r2_msgs::JointControlMode::PARK, info.captainMode); 00054 EXPECT_FALSE(gsc.getMapEntry("joint1", info)); 00055 } 00056 00057 TEST_F(GripperStatusCheckerTest, PopulatePilotRequestInfoTest) 00058 { 00059 00060 gsc.populatePilotRequestInfo(pReq); 00061 ASSERT_TRUE(gsc.getMapEntry(grippers[0], info)); 00062 EXPECT_TRUE(info.pilotRequest); 00063 EXPECT_FALSE(info.engineerCheck); 00064 00065 } 00066 00067 TEST_F(GripperStatusCheckerTest, PopulateCaptainRequestInfoTest) 00068 { 00069 00070 gsc.populateCaptainRequestInfo(cReq); 00071 ASSERT_TRUE(gsc.getMapEntry(grippers[1], info)); 00072 EXPECT_TRUE(info.captainRequest); 00073 EXPECT_FALSE(info.pilotRequest); 00074 } 00075 00076 TEST_F(GripperStatusCheckerTest, UpdateTest) 00077 { 00078 gsc.populateJointControlPilotInfo(jntModeList); 00079 gsc.populateJointControlCaptainInfo(jntModeList); 00080 gsc.update(); 00081 gsc.createEngineerCheckMsg(eCheck); 00082 gsc.createSweStopMessage(sweStopMsg); 00083 EXPECT_EQ(IDLE, gsc.getState(grippers[0])); 00084 EXPECT_TRUE(gsc.getMapEntry(grippers[1], info)); 00085 EXPECT_FALSE(info.pilotRequest); 00086 EXPECT_EQ(0, eCheck.data.size()); 00087 EXPECT_EQ(0, sweStopMsg.status.size()); 00088 00089 gsc.populatePilotRequestInfo(pReq); 00090 gsc.update(); 00091 gsc.createEngineerCheckMsg(eCheck); 00092 gsc.createSweStopMessage(sweStopMsg); 00093 EXPECT_EQ(PILOT_REQUEST, gsc.getState(grippers[0])); 00094 EXPECT_EQ(IDLE, gsc.getState(grippers[1])); 00095 EXPECT_TRUE(gsc.getMapEntry(grippers[0], info)); 00096 EXPECT_TRUE(info.pilotRequest); 00097 EXPECT_FALSE(info.engineerCheck); 00098 EXPECT_EQ(0, eCheck.data.size()); 00099 EXPECT_EQ(0, sweStopMsg.status.size()); 00100 00101 jntModeList.data[0].controlMode.state = r2_msgs::JointControlMode::DRIVE; 00102 gsc.populateJointControlPilotInfo(jntModeList); 00103 gsc.populateJointControlCaptainInfo(jntModeList); 00104 gsc.update(); 00105 gsc.createEngineerCheckMsg(eCheck); 00106 gsc.createSweStopMessage(sweStopMsg); 00107 EXPECT_EQ(0, eCheck.data.size()); 00108 ASSERT_EQ(1, sweStopMsg.status.size()); 00109 EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::ERROR, sweStopMsg.status[0].level); 00110 EXPECT_EQ(IDLE, gsc.getState(grippers[0])); 00111 EXPECT_TRUE(gsc.getMapEntry(grippers[0], info)); 00112 EXPECT_FALSE(info.pilotRequest); 00113 00114 jntModeList.data[0].controlMode.state = r2_msgs::JointControlMode::PARK; 00115 gsc.populateJointControlPilotInfo(jntModeList); 00116 gsc.populateJointControlCaptainInfo(jntModeList); 00117 gsc.populateCaptainRequestInfo(cReq); 00118 gsc.update(); 00119 gsc.createEngineerCheckMsg(eCheck); 00120 gsc.createSweStopMessage(sweStopMsg); 00121 EXPECT_EQ(0, eCheck.data.size()); 00122 EXPECT_EQ(0, sweStopMsg.status.size()); 00123 EXPECT_EQ(IDLE, gsc.getState(grippers[0])); 00124 EXPECT_EQ(CAPTAIN_REQUEST, gsc.getState(grippers[1])); 00125 EXPECT_TRUE(gsc.getMapEntry(grippers[1], info)); 00126 EXPECT_TRUE(info.captainRequest); 00127 EXPECT_FALSE(info.engineerCheck); 00128 ros::Duration(0.5).sleep(); 00129 gsc.update(); 00130 gsc.createEngineerCheckMsg(eCheck); 00131 gsc.createSweStopMessage(sweStopMsg); 00132 EXPECT_EQ(0, eCheck.data.size()); 00133 EXPECT_EQ(0, sweStopMsg.status.size()); 00134 EXPECT_EQ(IDLE, gsc.getState(grippers[1])); 00135 EXPECT_TRUE(gsc.getMapEntry(grippers[1], info)); 00136 EXPECT_FALSE(info.captainRequest); 00137 00138 gsc.populatePilotRequestInfo(pReq); 00139 gsc.update(); 00140 gsc.createEngineerCheckMsg(eCheck); 00141 gsc.createSweStopMessage(sweStopMsg); 00142 EXPECT_EQ(0, eCheck.data.size()); 00143 EXPECT_EQ(0, sweStopMsg.status.size()); 00144 EXPECT_EQ(PILOT_REQUEST, gsc.getState(grippers[0])); 00145 00146 cReq.data.clear(); 00147 cReq.data.push_back(grippers[0]); 00148 gsc.populateCaptainRequestInfo(cReq); 00149 gsc.update(); 00150 gsc.createEngineerCheckMsg(eCheck); 00151 gsc.createSweStopMessage(sweStopMsg); 00152 ASSERT_EQ(1, eCheck.data.size()); 00153 EXPECT_EQ(0, sweStopMsg.status.size()); 00154 EXPECT_EQ(grippers[0], eCheck.data[0]); 00155 EXPECT_TRUE(gsc.getMapEntry(grippers[0], info)); 00156 EXPECT_TRUE(info.captainRequest); 00157 EXPECT_TRUE(info.engineerCheck); 00158 00159 jntModeList.data[0].controlMode.state = r2_msgs::JointControlMode::DRIVE; 00160 gsc.populateJointControlPilotInfo(jntModeList); 00161 gsc.populateJointControlCaptainInfo(jntModeList); 00162 gsc.update(); 00163 gsc.createEngineerCheckMsg(eCheck); 00164 gsc.createSweStopMessage(sweStopMsg); 00165 EXPECT_EQ(1, eCheck.data.size()); 00166 EXPECT_EQ(0, sweStopMsg.status.size()); 00167 00168 ros::Duration(2.0).sleep(); 00169 gsc.update(); 00170 gsc.createEngineerCheckMsg(eCheck); 00171 gsc.createSweStopMessage(sweStopMsg); 00172 EXPECT_EQ(1, eCheck.data.size()); 00173 EXPECT_EQ(0, sweStopMsg.status.size()); 00174 EXPECT_EQ(DRIVING, gsc.getState(grippers[0])); 00175 EXPECT_EQ(IDLE, gsc.getState(grippers[1])); 00176 00177 jntModeList.data[0].controlMode.state = r2_msgs::JointControlMode::PARK; 00178 gsc.populateJointControlPilotInfo(jntModeList); 00179 gsc.populateJointControlCaptainInfo(jntModeList); 00180 gsc.update(); 00181 ros::Duration(0.75).sleep(); 00182 gsc.update(); 00183 gsc.createEngineerCheckMsg(eCheck); 00184 gsc.createSweStopMessage(sweStopMsg); 00185 EXPECT_EQ(0, eCheck.data.size()); 00186 EXPECT_EQ(0, sweStopMsg.status.size()); 00187 EXPECT_TRUE(gsc.getMapEntry(grippers[0], info)); 00188 EXPECT_FALSE(info.pilotRequest); 00189 EXPECT_FALSE(info.captainRequest); 00190 EXPECT_FALSE(info.engineerCheck); 00191 EXPECT_EQ(IDLE, gsc.getState(grippers[0])); 00192 EXPECT_EQ(IDLE, gsc.getState(grippers[1])); 00193 00194 } 00195 00196 int main(int argc, char** argv) 00197 { 00198 ::testing::InitGoogleTest(&argc, argv); 00199 return RUN_ALL_TESTS(); 00200 }