00001 #include "ptu_controller/PTUControllerClient.h"
00002 #include <iostream>
00003 #include <fstream>
00004
00005 void spinThread()
00006 {
00007 ros::spin();
00008 }
00009 PTUControllerClient::PTUControllerClient(std::string name):
00010 actionClient(name) {
00011 ROS_INFO("Waiting for action server to start.");
00012 actionClient.waitForServer();
00013 goal.target_joint.header.seq = 0;
00014 goal.target_joint.name.push_back("pan");
00015 goal.target_joint.name.push_back("tilt");
00016 goal.target_joint.velocity.push_back(0);
00017 goal.target_joint.velocity.push_back(0);
00018 }
00019 void PTUControllerClient::sendJoint(double pan, double tilt, bool wait) {
00020 ROS_DEBUG("Sending goal.");
00021 goal.target_joint.position.clear();
00022 goal.target_joint.position.push_back(pan);
00023 goal.target_joint.position.push_back(tilt);
00024 actionClient.sendGoal(goal);
00025 if (wait) {
00026 bool finished_before_timeout = actionClient.waitForResult(ros::Duration(30.0));
00027 if (finished_before_timeout)
00028 {
00029 actionlib::SimpleClientGoalState state = actionClient.getState();
00030 ROS_INFO("Action finished: %s",state.toString().c_str());
00031 }
00032 else
00033 ROS_INFO("Action did not finish before the time out.");
00034 }
00035
00036 }
00037 void PTUControllerClient::checkValidation(double maxPan, double minPan, double maxTilt, double minTilt) {
00038 double epsilon = 0.1;
00039 double greaterMaxPan = maxPan + epsilon;
00040 double smallerMinPan = minPan - epsilon;
00041 double greaterMaxTilt = maxTilt + epsilon;
00042 double smallerMinTilt = minTilt - epsilon;
00043
00044 double pans[3] = {0.0, greaterMaxPan, smallerMinPan};
00045 double tilts[3] = {0.0, greaterMaxTilt, smallerMinTilt};
00046
00047 for (int i = 0; i < 3; i++) {
00048 for (int j = 0; j < 3; j++) {
00049 if (i != 0 || j != 0)
00050 this->sendJoint(pans[i],tilts[j], true);
00051 }
00052 }
00053 }
00054
00055 actionlib::SimpleClientGoalState PTUControllerClient::testFunction(double pan, double tilt) {
00056 goal.target_joint.position.clear();
00057 goal.target_joint.position.push_back(pan);
00058 goal.target_joint.position.push_back(tilt);
00059 actionClient.sendGoal(goal);
00060 bool finished_before_timeout = actionClient.waitForResult(ros::Duration(60.0));
00061 if (finished_before_timeout)
00062 {
00063 return actionClient.getState();
00064 }
00065 else
00066 {
00067 return actionlib::SimpleClientGoalState::ACTIVE;
00068 }
00069 }
00070
00071 int main (int argc, char **argv)
00072 {
00073 ros::init(argc, argv, "ptu_controller_client");
00074
00075
00076 boost::thread spin_thread(&spinThread);
00077 std::string actionServerName;
00078 ros::NodeHandle n("~");
00079 n.getParam("actionServerName", actionServerName);
00080 PTUControllerClient* ptu_controller = new PTUControllerClient(actionServerName);
00081 bool checkValidation;
00082 n.getParam("checkValidation", checkValidation);
00083
00084 bool testing;
00085 n.getParam("testing", testing);
00086 if(testing) {
00087 std::ofstream testfile;
00088 std::string filepath;
00089 n.getParam("testlog_filepath", filepath);
00090 testfile.open(filepath.c_str(), std::ios::trunc);
00091 ROS_DEBUG("Test Nr. 00: Test initialization\n");
00092 testfile << "Test Nr. 00:\nIt is tried to bring the PTU in starting position with pan = 0 and tilt = 0\n";
00093 testfile << "It is expected that the Actionserver accepts the goal\n";
00094 testfile << "Expected return state: SUCCEEDED\n";
00095 testfile << "Recieved return state: ";
00096 actionlib::SimpleClientGoalState state(ptu_controller->testFunction(0,0).state_, ptu_controller->testFunction(0,0).text_);
00097 testfile << state.toString() << "\n";
00098 if(state == actionlib::SimpleClientGoalState::SUCCEEDED) {
00099 ROS_DEBUG("Correct state returned, for pan/tilt check look into the testlog\n");
00100 testfile << "Expected pan: 0\n";
00101 testfile << "Recieved pan: " << ptu_controller->actionClient.getResult()->end_joint.position[0] << "\n";
00102 testfile << "Expected tilt: 0\n";
00103 testfile << "Recieved tilt: " << ptu_controller->actionClient.getResult()->end_joint.position[1] << "\n";
00104 }
00105 else {
00106 ROS_DEBUG("Failure\n");
00107 }
00108 testfile << "\n";
00109
00110 ROS_DEBUG("Test Nr. 01: No Movement\n");
00111 testfile << "Test Nr. 01:\nIt is tried if the Actionserver works with a command that moves the PTU to the same position where it is located at the moment\n";
00112 testfile << "It is expected that the Actionserver takes the goal and executes it\n";
00113 testfile << "Expected return state: SUCCEEDED\n";
00114 testfile << "Recieved return state: ";
00115 state = ptu_controller->testFunction(0,0);
00116 testfile << state.toString() << "\n";
00117 if(state == actionlib::SimpleClientGoalState::SUCCEEDED) {
00118 ROS_DEBUG("Correct state returned, for pan/tilt check look into the testlog\n");
00119 testfile << "Expected pan: 0\n";
00120 testfile << "Recieved pan: " << ptu_controller->actionClient.getResult()->end_joint.position[0] << "\n";
00121 testfile << "Expected tilt: 0\n";
00122 testfile << "Recieved tilt: " << ptu_controller->actionClient.getResult()->end_joint.position[1] << "\n";
00123 }
00124 else {
00125 ROS_DEBUG("Failure\n");
00126 }
00127 testfile << "\n";
00128
00129 ROS_DEBUG("Test Nr. 02: Pan Movement\n");
00130 testfile << "Test Nr. 02:\nIt is tried to move the pan-unit of the PTU from 0 to 5 degree\n";
00131 testfile << "It is expected that the Actionserver accepts the goal and executes it\n";
00132 testfile << "Expected return state: SUCCEEDED\n";
00133 testfile << "Recieved return state: ";
00134 state = ptu_controller->testFunction(5,0);
00135 testfile << state.toString() << "\n";
00136 if(state == actionlib::SimpleClientGoalState::SUCCEEDED) {
00137 ROS_DEBUG("Correct state returned, for pan/tilt check look into the testlog\n");
00138 testfile << "Expected pan: ~5\n";
00139 testfile << "Recieved pan: " << ptu_controller->actionClient.getResult()->end_joint.position[0] << "\n";
00140 testfile << "Expected tilt: 0\n";
00141 testfile << "Recieved tilt: " << ptu_controller->actionClient.getResult()->end_joint.position[1] << "\n";
00142 }
00143 else {
00144 ROS_DEBUG("Failure\n");
00145 }
00146 testfile << "\n";
00147
00148 ROS_DEBUG("Test Nr. 03: Tilt Movement\n");
00149 testfile << "Test Nr 03:\nIt is tried to move the tilt-unit of the PTU from 0 to 5 degree\n";
00150 testfile << "It is expected that the Actionserver accepts the goal and executes it\n";
00151 testfile << "Expected return state: SUCCEEDED\n";
00152 testfile << "Recieved return state: ";
00153 state = ptu_controller->testFunction(5,5);
00154 testfile << state.toString() << "\n";
00155 if(state == actionlib::SimpleClientGoalState::SUCCEEDED) {
00156 ROS_DEBUG("Correct state returned, for pan/tilt check look into the testlog\n");
00157 testfile << "Expected pan: ~5\n";
00158 testfile << "Recieved pan: " << ptu_controller->actionClient.getResult()->end_joint.position[0] << "\n";
00159 testfile << "Expected tilt: ~5\n";
00160 testfile << "Recieved tilt: " << ptu_controller->actionClient.getResult()->end_joint.position[1] << "\n";
00161 }
00162 else {
00163 ROS_DEBUG("Failure\n");
00164 }
00165 testfile << "\n";
00166
00167 ROS_DEBUG("Test Nr. 04: Pan + Tilt Movement\n");
00168 testfile << "Test Nr. 04:\nIt is tried to move the pan and the tilt unit within the same command from 5 to 10\n";
00169 testfile << "It is expected that the Actionserver takes the command and executes it\n";
00170 testfile << "Expected return state: SUCCEEDED\n";
00171 testfile << "Recieved return state: ";
00172 state = ptu_controller->testFunction(10,10);
00173 testfile << state.toString() << "\n";
00174 if(state == actionlib::SimpleClientGoalState::SUCCEEDED) {
00175 ROS_DEBUG("Correct state returned, for pan/tilt check look into the testlog\n");
00176 testfile << "Expected pan: ~10\n";
00177 testfile << "Recieved pan: " << ptu_controller->actionClient.getResult()->end_joint.position[0] << "\n";
00178 testfile << "Expected tilt: ~10\n";
00179 testfile << "Recieved tilt: " << ptu_controller->actionClient.getResult()->end_joint.position[1] << "\n";
00180 }
00181 else {
00182 ROS_DEBUG("Failure\n");
00183 }
00184 testfile << "\n";
00185
00186 ROS_DEBUG("Test Nr. 05: Pan out of bounds \n");
00187 testfile << "Test Nr. 05:\nIt is tested if it is possible to pass a pan position to the Actionserver which is out of the working area of the PTU (pan 178)\n";
00188 testfile << "It is expected that the server rejects (at the moment instant abort) the request and that no movement happens\n";
00189 testfile << "Expected return state: ABORTED\n";
00190 testfile << "Recieved return state: ";
00191 state = ptu_controller->testFunction(178,10);
00192 testfile << state.toString() << "\n";
00193 if(state == actionlib::SimpleClientGoalState::ABORTED) {
00194 ROS_DEBUG("Correct state returned, for pan/tilt check look into the testlog\n");
00195 testfile << "Expected pan: ~10\n";
00196 testfile << "Recieved pan: " << ptu_controller->actionClient.getResult()->end_joint.position[0] << "\n";
00197 testfile << "Expected tilt: ~10\n";
00198 testfile << "Recieved tilt: " << ptu_controller->actionClient.getResult()->end_joint.position[1] << "\n";
00199 }
00200 else {
00201 ROS_DEBUG("Failure\n");
00202 }
00203 testfile << "\n";
00204
00205 ROS_DEBUG("Test Nr. 06: Tilt out of bounds \n");
00206 testfile << "Test Nr. 06:\nIt is tested if it is possible to pass a pan position to the Actionserver which is out of the working area of the PTU (tilt 178)\n";
00207 testfile << "It is expected that the server rejects (at the moment instant abort) the request and that no movement happens\n";
00208 testfile << "Expected return state: ABORTED\n";
00209 testfile << "Recieved return state: ";
00210 state = ptu_controller->testFunction(10,178);
00211 testfile << state.toString() << "\n";
00212 if(state == actionlib::SimpleClientGoalState::ABORTED) {
00213 ROS_DEBUG("Correct state returned, for pan/tilt check look into the testlog\n");
00214 testfile << "Expected pan: ~10\n";
00215 testfile << "Recieved pan: " << ptu_controller->actionClient.getResult()->end_joint.position[0] << "\n";
00216 testfile << "Expected tilt: ~10\n";
00217 testfile << "Recieved tilt: " << ptu_controller->actionClient.getResult()->end_joint.position[1] << "\n";
00218 }
00219 else {
00220 ROS_DEBUG("Failure\n");
00221 }
00222 testfile << "\n";
00223
00224 ROS_DEBUG("Test Nr. 07: In forbidden area \n");
00225 testfile << "Test Nr. 07\nIt is tested if it is possible to pass a position within a forbidden area but within the working area of the PTU to the Actionserver (pan -55, tilt - 30)";
00226 testfile << "It is expected that the server rejects (at the moment instant abort) the request and that no movement happens\n";
00227 testfile << "Expected return state: ABORTED\n";
00228 testfile << "Recieved return state: ";
00229 state = ptu_controller->testFunction(-55,-30);
00230 testfile << state.toString() << "\n";
00231 if(state == actionlib::SimpleClientGoalState::ABORTED) {
00232 ROS_DEBUG("Correct state returned, for pan/tilt check look into the testlog\n");
00233 testfile << "Expected pan: ~10\n";
00234 testfile << "Recieved pan: " << ptu_controller->actionClient.getResult()->end_joint.position[0] << "\n";
00235 testfile << "Expected tilt: ~10\n";
00236 testfile << "Recieved tilt: " << ptu_controller->actionClient.getResult()->end_joint.position[1] << "\n";
00237 }
00238 else {
00239 ROS_DEBUG("Failure\n");
00240 }
00241 testfile << "\n";
00242
00243 testfile.flush();
00244 testfile.close();
00245 return 0;
00246
00247 }
00248
00249 if (checkValidation) {
00250 ROS_DEBUG("checkValidation");
00251 double maxPan;
00252 double minPan;
00253 double maxTilt;
00254 double minTilt;
00255
00256 n.getParam("minPan", minPan);
00257 n.getParam("maxPan", maxPan);
00258 n.getParam("minTilt", minTilt);
00259 n.getParam("maxTilt", maxTilt);
00260
00261 ptu_controller->checkValidation(maxPan, minPan, maxTilt, minTilt);
00262 } else {
00263 std::vector<double> pans;
00264 std::vector<double> tilts;
00265 std::vector<bool> waits;
00266 n.getParam("pans",pans);
00267 n.getParam("tilts", tilts);
00268 n.getParam("waits",waits);
00269
00270 if (pans.size() != tilts.size() || pans.size() != waits.size())
00271 ROS_ERROR("ERROR in commands.yaml");
00272 for(unsigned int i = 0; i < waits.size(); i++) {
00273 ptu_controller->sendJoint(pans[i], tilts[i], waits[0]);
00274 }
00275 }
00276
00277 ros::shutdown();
00278 spin_thread.join();
00279 return 0;
00280 }
00281
00282
00283