Go to the documentation of this file.00001 #include <gtest/gtest.h>
00002 #include "robodyn_controllers/BaseFrameDetector.h"
00003 #include <ros/package.h>
00004
00005 class BaseFrameDetectorTest : public ::testing::Test
00006 {
00007 protected:
00008 virtual void SetUp()
00009 {
00010 ros::Time::init();
00011
00012 eefState.name.push_back("/r2/left_leg/gripper/joint0");
00013 eefState.name.push_back("/r2/right_leg/gripper/joint0");
00014 eefState.locked.push_back(1);
00015 eefState.loaded.push_back(1);
00016 eefState.locked.push_back(1);
00017 eefState.loaded.push_back(1);
00018
00019 bfVerif.name.push_back("/r2/left_leg/gripper/joint0");
00020 bfVerif.name.push_back("/r2/right_leg/gripper/joint0");
00021 bfVerif.verified.push_back(1);
00022 bfVerif.verified.push_back(0);
00023 }
00024
00025 virtual void TearDown()
00026 {
00027 }
00028
00029 BaseFrameDetector bfd;
00030 r2_msgs::GripperPositionState eefState, bfVerif;
00031 };
00032
00033
00034 TEST_F(BaseFrameDetectorTest, PopulateEEFStateInfoTest)
00035 {
00036
00037 bfd.PopulateEEFStateInfo(eefState);
00038
00039 EXPECT_EQ(1, bfd.endEffectorMap["/r2/left_leg/gripper/joint0"].locked);
00040 EXPECT_EQ(1, bfd.endEffectorMap["/r2/right_leg/gripper/joint0"].loaded);
00041
00042 eefState.locked[1] = 0;
00043 bfd.PopulateEEFStateInfo(eefState);
00044 EXPECT_EQ(0, bfd.endEffectorMap["/r2/right_leg/gripper/joint0"].locked);
00045
00046 }
00047
00048 TEST_F(BaseFrameDetectorTest, PopulateBaseVerifInfoTest)
00049 {
00050 bfd.PopulateBaseVerifInfo(bfVerif);
00051
00052 EXPECT_EQ(1, bfd.endEffectorMap["/r2/left_leg/gripper/joint0"].verified);
00053 EXPECT_EQ(0, bfd.endEffectorMap["/r2/right_leg/gripper/joint0"].verified);
00054
00055 }
00056
00057 TEST_F(BaseFrameDetectorTest, PopulateBaseFrameMsgTest)
00058 {
00059 bfd.PopulateEEFStateInfo(eefState);
00060 bfd.PopulateBaseVerifInfo(bfVerif);
00061
00062 r2_msgs::StringArray baseList;
00063 std::string baseFrameCmd = "/r2/right_leg/gripper/tip";
00064 bfd.PopulateBaseFrameMsg(baseFrameCmd, baseList);
00065
00066 ASSERT_EQ(1, baseList.data.size());
00067 EXPECT_EQ("/r2/left_leg/gripper/tip", baseList.data[0]);
00068
00069 baseList.data.clear();
00070 bfd.endEffectorMap["/r2/right_leg/gripper/joint0"].verified = 1;
00071 bfd.PopulateBaseFrameMsg(baseFrameCmd, baseList);
00072
00073 ASSERT_EQ(2, baseList.data.size());
00074 EXPECT_EQ("/r2/right_leg/gripper/tip", baseList.data[0]);
00075 EXPECT_EQ("/r2/left_leg/gripper/tip", baseList.data[1]);
00076
00077 baseList.data.clear();
00078 bfd.endEffectorMap["/r2/right_leg/gripper/joint0"].verified = 0;
00079 bfd.endEffectorMap["/r2/left_leg/gripper/joint0"].verified = 0;
00080 baseFrameCmd = "link1";
00081 bfd.PopulateBaseFrameMsg(baseFrameCmd, baseList);
00082
00083 ASSERT_EQ(1, baseList.data.size());
00084 EXPECT_EQ(baseFrameCmd, baseList.data[0]);
00085 }
00086
00087 int main(int argc, char** argv)
00088 {
00089 ::testing::InitGoogleTest(&argc, argv);
00090 return RUN_ALL_TESTS();
00091 }