00001 #include <gtest/gtest.h>
00002 #include <ros/package.h>
00003 #include <vector>
00004 #include "robodyn_controllers/ModeArbiter.h"
00005
00006 class ModeArbiterTest : public ::testing::Test
00007 {
00008 protected:
00009 virtual void SetUp()
00010 {
00011 modeMsg.originator = "user";
00012 modeMsg.supervisor.push_back("George");
00013 modeMsg.incumbent.push_back("Sally");
00014 modeMsg.mode.push_back("EEF");
00015
00016 std::string packagePath = ros::package::getPath("robodyn_controllers");
00017 ma.loadFromFile(packagePath + "/test/urdf/SimpleTestRobot.xml");
00018
00019 exJoints.push_back("joint5");
00020
00021 ma.InitializeMaps(exJoints, exJoints);
00022 ros::Time::init();
00023
00024 }
00025
00026 virtual void TearDown()
00027 {
00028 }
00029
00030 ModeArbiter ma;
00031 r2_msgs::Modes modeMsg;
00032 std::string value;
00033 std::vector<std::string> exJoints;
00034
00035 };
00036
00037 TEST_F(ModeArbiterTest, InitializeMapsTest)
00038 {
00039
00040 ASSERT_TRUE(ma.GetMapElement("supervisor", "joint1", value));
00041 EXPECT_EQ("user", value);
00042 ASSERT_TRUE(ma.GetMapElement("incumbent", "joint2", value));
00043 EXPECT_EQ("user", value);
00044 ASSERT_TRUE(ma.GetMapElement("mode", "joint1", value));
00045 EXPECT_EQ("IK", value);
00046 ASSERT_TRUE(ma.GetMapElement("supervisor", "joint5", value));
00047 EXPECT_EQ("user", value);
00048
00049 }
00050
00051 TEST_F(ModeArbiterTest, AssignMasterTest)
00052 {
00053 std::string master;
00054 ma.AssignMaster("Pedro");
00055 ma.GetMaster(master);
00056 EXPECT_EQ("Pedro", master);
00057 }
00058
00059 TEST_F(ModeArbiterTest, AssignArbitrationPropertiesTest)
00060 {
00061 EXPECT_EQ(3, ma.AssignArbitrationProperties(modeMsg));
00062 EXPECT_TRUE(ma.GetMapElement("supervisor", "joint1", value));
00063 EXPECT_EQ("George", value);
00064 EXPECT_TRUE(ma.GetMapElement("incumbent", "joint2", value));
00065 EXPECT_EQ("Sally", value);
00066 EXPECT_TRUE(ma.GetMapElement("mode", "joint1", value));
00067 EXPECT_EQ("EEF", value);
00068
00069 modeMsg.supervisor.clear();
00070 modeMsg.incumbent.clear();
00071 modeMsg.mode.clear();
00072
00073 modeMsg.joint_names.push_back("joint1");
00074 modeMsg.joint_names.push_back("joint2");
00075 modeMsg.supervisor.push_back("Jose");
00076 modeMsg.supervisor.push_back("user");
00077 modeMsg.incumbent.push_back("George");
00078 modeMsg.incumbent.push_back("user");
00079 modeMsg.mode.push_back("IK");
00080 modeMsg.mode.push_back("IK");
00081
00082 EXPECT_EQ(0, ma.AssignArbitrationProperties(modeMsg));
00083 modeMsg.originator = "George";
00084 EXPECT_EQ(3, ma.AssignArbitrationProperties(modeMsg));
00085 EXPECT_TRUE(ma.GetMapElement("supervisor", "joint1", value));
00086 EXPECT_EQ("Jose", value);
00087 EXPECT_TRUE(ma.GetMapElement("incumbent", "joint2", value));
00088 EXPECT_EQ("user", value);
00089 EXPECT_TRUE(ma.GetMapElement("mode", "joint1", value));
00090 EXPECT_EQ("IK", value);
00091
00092 }
00093
00094 TEST_F(ModeArbiterTest, ValidHijackTest)
00095 {
00096 ma.AssignMaster("Pedro");
00097 ma.AssignArbitrationProperties(modeMsg);
00098 EXPECT_FALSE(ma.isValidHijack("Paul"));
00099 EXPECT_TRUE(ma.isValidHijack("George"));
00100 }
00101
00102
00103 TEST_F(ModeArbiterTest, CreateTriggerMessagesTest)
00104 {
00105 std::vector<r2_msgs::Modes> triggerMsgs;
00106
00107 ma.AssignArbitrationProperties(modeMsg);
00108 ma.CreateTriggerMessages(triggerMsgs);
00109 ASSERT_EQ(1, triggerMsgs.size());
00110 EXPECT_EQ(4, triggerMsgs[0].joint_names.size());
00111 EXPECT_EQ("George", triggerMsgs[0].supervisor[0]);
00112 EXPECT_EQ("Sally", triggerMsgs[0].incumbent[0]);
00113
00114 modeMsg.supervisor.clear();
00115 modeMsg.incumbent.clear();
00116 modeMsg.mode.clear();
00117 modeMsg.originator = "George";
00118 modeMsg.joint_names.push_back("joint1");
00119 modeMsg.joint_names.push_back("joint2");
00120 modeMsg.supervisor.push_back("Jose");
00121 modeMsg.supervisor.push_back("user");
00122 modeMsg.incumbent.push_back("George");
00123 modeMsg.incumbent.push_back("user");
00124 modeMsg.mode.push_back("IK");
00125 modeMsg.mode.push_back("IK");
00126 ma.AssignArbitrationProperties(modeMsg);
00127
00128 triggerMsgs.clear();
00129 ma.CreateTriggerMessages(triggerMsgs);
00130 ASSERT_EQ(3, triggerMsgs.size());
00131 EXPECT_EQ(1, triggerMsgs[0].joint_names.size());
00132 EXPECT_EQ(1, triggerMsgs[1].joint_names.size());
00133
00134 }
00135
00136 TEST_F(ModeArbiterTest, CreateCommandMessageTest)
00137 {
00138 ma.AssignArbitrationProperties(modeMsg);
00139 ma.AssignMaster("user");
00140
00141 r2_msgs::LabeledGains gainsIn;
00142 r2_msgs::Gains gainsOut;
00143 gainsIn.originator = "Paul";
00144 gainsIn.joint_names.push_back("joint1");
00145 gainsIn.naturalFreq.push_back(3);
00146 gainsIn.dampingRatio.push_back(1.5);
00147 EXPECT_FALSE(ma.CreateCommandMessage(gainsIn, gainsOut));
00148 gainsIn.originator = "George";
00149 ASSERT_TRUE(ma.CreateCommandMessage(gainsIn, gainsOut));
00150 EXPECT_EQ(gainsIn.joint_names[0], gainsOut.joint_names[0]);
00151 EXPECT_EQ(3, gainsOut.naturalFreq[0]);
00152
00153 r2_msgs::LabeledJointState jsIn;
00154 sensor_msgs::JointState jsOut;
00155 jsIn.originator = "Petunia";
00156 jsIn.name.push_back("joint1");
00157 jsIn.name.push_back("joint2");
00158 jsIn.position.push_back(17);
00159 jsIn.position.push_back(3);
00160 EXPECT_FALSE(ma.CreateCommandMessage(jsIn, jsOut));
00161 jsIn.originator = "George";
00162 ASSERT_TRUE(ma.CreateCommandMessage(jsIn, jsOut));
00163 EXPECT_EQ(17, jsOut.position[0]);
00164 EXPECT_EQ("joint2", jsOut.name[1]);
00165
00166 r2_msgs::LabeledPoseTrajectory ptIn;
00167 r2_msgs::PoseTrajectory ptOut;
00168 ptIn.originator = "Charlie";
00169 ptIn.nodes.push_back("joint2");
00170 r2_msgs::PoseTrajectoryPoint trajPoint;
00171 geometry_msgs::Pose trajPose;
00172 trajPose.position.x = 1.0;
00173 trajPose.position.y = 2.0;
00174 trajPose.position.z = 3.0;
00175 trajPose.orientation.x = 0.1;
00176 trajPose.orientation.y = 0.2;
00177 trajPose.orientation.z = 0.3;
00178 trajPose.orientation.w = 0.4;
00179 trajPoint.positions.push_back(trajPose);
00180 ptIn.refFrames.push_back("joint1");
00181 ptIn.points.push_back(trajPoint);
00182 r2_msgs::PriorityArray pArray;
00183 pArray.axis_priorities.push_back(128);
00184 ptIn.node_priorities.push_back(pArray);
00185 EXPECT_FALSE(ma.CreateCommandMessage(ptIn, ptOut));
00186 ptIn.originator = "George";
00187 ASSERT_TRUE(ma.CreateCommandMessage(ptIn, ptOut));
00188 EXPECT_EQ("joint2", ptOut.nodes[0]);
00189 EXPECT_EQ("joint1", ptOut.refFrames[0]);
00190 EXPECT_EQ(128, ptOut.node_priorities[0].axis_priorities[0]);
00191 EXPECT_EQ(2.0, ptOut.points[0].positions[0].position.y);
00192 EXPECT_EQ(0.3, ptOut.points[0].positions[0].orientation.z);
00193
00194 r2_msgs::LabeledControllerPoseSettings psIn;
00195 r2_msgs::ControllerPoseSettings psOut;
00196 psIn.originator = "Ben";
00197 psIn.maxLinearVelocity = 1.0;
00198 psIn.maxRotationalVelocity = 2.0;
00199 psIn.maxLinearAcceleration = 3.0;
00200 psIn.maxRotationalAcceleration = 4.0;
00201 EXPECT_FALSE(ma.CreateCommandMessage(psIn, psOut));
00202 psIn.originator = "George";
00203 ASSERT_TRUE(ma.CreateCommandMessage(psIn, psOut));
00204 EXPECT_EQ(1.0, psOut.maxLinearVelocity);
00205 EXPECT_EQ(4.0, psOut.maxRotationalAcceleration);
00206
00207 r2_msgs::LabeledJointTrajectory jtIn;
00208 trajectory_msgs::JointTrajectory jtOut;
00209 jtIn.originator = "Sue";
00210 jtIn.joint_names.push_back("joint1");
00211 jtIn.joint_names.push_back("joint2");
00212 trajectory_msgs::JointTrajectoryPoint jtPoint;
00213 jtPoint.positions.push_back(1.0);
00214 jtPoint.positions.push_back(2.0);
00215 jtIn.points.push_back(jtPoint);
00216 jtPoint.positions.clear();
00217 jtPoint.positions.push_back(3.0);
00218 jtPoint.positions.push_back(5.0);
00219 jtIn.points.push_back(jtPoint);
00220 jtIn.header.stamp = ros::Time::now();
00221 EXPECT_FALSE(ma.CreateCommandMessage(jtIn, jtOut));
00222 jtIn.originator = "George";
00223 ASSERT_TRUE(ma.CreateCommandMessage(jtIn, jtOut));
00224 EXPECT_EQ("joint1", jtOut.joint_names[0]);
00225 EXPECT_EQ(5.0, jtOut.points[1].positions[1]);
00226
00227 r2_msgs::LabeledControllerJointSettings ksIn;
00228 r2_msgs::ControllerJointSettings ksOut;
00229 ksIn.originator = "Ursula";
00230 ksIn.joint_names.push_back("joint1");
00231 ksIn.joint_names.push_back("joint2");
00232 ksIn.jointVelocityLimits.push_back(3.0);
00233 ksIn.jointVelocityLimits.push_back(4.0);
00234 ksIn.jointAccelerationLimits.push_back(5.0);
00235 ksIn.jointAccelerationLimits.push_back(7.0);
00236 EXPECT_FALSE(ma.CreateCommandMessage(ksIn, ksOut));
00237 ksIn.originator = "George";
00238 ASSERT_TRUE(ma.CreateCommandMessage(ksIn, ksOut));
00239 EXPECT_EQ("joint2", ksOut.joint_names[1]);
00240 EXPECT_EQ(4.0, ksOut.jointVelocityLimits[1]);
00241 EXPECT_EQ(5.0, ksOut.jointAccelerationLimits[0]);
00242
00243 r2_msgs::Modes masterMsg;
00244 masterMsg.master = "Peter";
00245 masterMsg.originator = "Pedro";
00246 ma.AssignArbitrationProperties(masterMsg);
00247 std::string mr;
00248 ma.GetMaster(mr);
00249 EXPECT_EQ("user", mr);
00250 masterMsg.originator = "user";
00251 ma.AssignArbitrationProperties(masterMsg);
00252 ma.GetMaster(mr);
00253 EXPECT_EQ("Peter", mr);
00254 gainsIn.joint_names.push_back("joint2");
00255 gainsIn.naturalFreq.push_back(2);
00256 gainsIn.dampingRatio.push_back(4.5);
00257 gainsIn.originator = "Peter";
00258 ASSERT_TRUE(ma.CreateCommandMessage(gainsIn, gainsOut));
00259
00260 r2_msgs::LabeledGripperPositionCommand gpIn;
00261 r2_msgs::GripperPositionCommand gpOut;
00262 gpIn.originator = "Jason";
00263 gpIn.name.push_back("joint1");
00264 gpIn.setpoint.push_back("closed");
00265 gpIn.command.push_back("set");
00266 gpIn.dutyCycle.push_back(255);
00267 EXPECT_FALSE(ma.CreateCommandMessage(gpIn, gpOut));
00268 gpIn.originator = "Peter";
00269 ASSERT_TRUE(ma.CreateCommandMessage(gpIn, gpOut));
00270 EXPECT_EQ("closed", gpOut.setpoint[0]);
00271
00272 r2_msgs::LabeledJointControlDataArray jcIn;
00273 r2_msgs::JointControlDataArray jcOut;
00274 jcIn.originator = "Roger";
00275 jcIn.joint.push_back("joint1");
00276 r2_msgs::JointControlData jcData;
00277 jcData.controlMode.state = 6;
00278 jcIn.data.push_back(jcData);
00279 EXPECT_FALSE(ma.CreateCommandMessage(jcIn, jcOut));
00280 jcIn.originator = "Peter";
00281 ASSERT_TRUE(ma.CreateCommandMessage(jcIn, jcOut));
00282 EXPECT_EQ("joint1", jcOut.joint[0]);
00283 EXPECT_EQ(1, jcOut.data.size());
00284 EXPECT_EQ(r2_msgs::JointControlMode::PARK, jcOut.data[0].controlMode.state);
00285
00286 jcIn.joint.push_back("joint5");
00287 jcData.controlMode.state = r2_msgs::JointControlMode::DRIVE;
00288 jcIn.data.push_back(jcData);
00289 ASSERT_TRUE(ma.CreateCommandMessage(jcIn, jcOut));
00290 EXPECT_EQ(2, jcOut.data.size());
00291 }
00292
00293 int main(int argc, char** argv)
00294 {
00295 ::testing::InitGoogleTest(&argc, argv);
00296 return RUN_ALL_TESTS();
00297 }
00298