PTUControllerClient.cpp
Go to the documentation of this file.
1 #include "ptu_controller/PTUControllerClient.h"
2 #include <iostream>
3 #include <fstream>
4 
5 void spinThread()
6 {
7  ros::spin();
8 }
9 PTUControllerClient::PTUControllerClient(std::string name):
10  actionClient(name) {
11  ROS_INFO("Waiting for action server to start.");
12  actionClient.waitForServer();
13  goal.target_joint.header.seq = 0;
14  goal.target_joint.name.push_back("pan");
15  goal.target_joint.name.push_back("tilt");
16  goal.target_joint.velocity.push_back(0);
17  goal.target_joint.velocity.push_back(0);
18 }
19 void PTUControllerClient::sendJoint(double pan, double tilt, bool wait) {
20  ROS_DEBUG("Sending goal.");
21  goal.target_joint.position.clear();
22  goal.target_joint.position.push_back(pan);
23  goal.target_joint.position.push_back(tilt);
24  actionClient.sendGoal(goal);
25  if (wait) {
26  bool finished_before_timeout = actionClient.waitForResult(ros::Duration(30.0));
27  if (finished_before_timeout)
28  {
29  actionlib::SimpleClientGoalState state = actionClient.getState();
30  ROS_INFO("Action finished: %s",state.toString().c_str());
31  }
32  else
33  ROS_INFO("Action did not finish before the time out.");
34  }
35 
36 }
37 void PTUControllerClient::checkValidation(double maxPan, double minPan, double maxTilt, double minTilt) {
38  double epsilon = 0.1;
39  double greaterMaxPan = maxPan + epsilon;
40  double smallerMinPan = minPan - epsilon;
41  double greaterMaxTilt = maxTilt + epsilon;
42  double smallerMinTilt = minTilt - epsilon;
43 
44  double pans[3] = {0.0, greaterMaxPan, smallerMinPan};
45  double tilts[3] = {0.0, greaterMaxTilt, smallerMinTilt};
46 
47  for (int i = 0; i < 3; i++) {
48  for (int j = 0; j < 3; j++) {
49  if (i != 0 || j != 0)
50  this->sendJoint(pans[i],tilts[j], true);
51  }
52  }
53 }
54 
55 actionlib::SimpleClientGoalState PTUControllerClient::testFunction(double pan, double tilt) {
56  goal.target_joint.position.clear();
57  goal.target_joint.position.push_back(pan);
58  goal.target_joint.position.push_back(tilt);
59  actionClient.sendGoal(goal);
60  bool finished_before_timeout = actionClient.waitForResult(ros::Duration(60.0));
61  if (finished_before_timeout)
62  {
63  return actionClient.getState();
64  }
65  else
66  {
67  return actionlib::SimpleClientGoalState::ACTIVE;
68  }
69 }
70 
71 int main (int argc, char **argv)
72 {
73  ros::init(argc, argv, "ptu_controller_client");
74 
75  // create the action client
76  boost::thread spin_thread(&spinThread);
77  std::string actionServerName;
78  ros::NodeHandle n("~");
79  n.getParam("actionServerName", actionServerName);
80  PTUControllerClient* ptu_controller = new PTUControllerClient(actionServerName);
81  bool checkValidation;
82  n.getParam("checkValidation", checkValidation);
83  //############## Testing Only, Code untested ####################
84  bool testing;
85  n.getParam("testing", testing);
86  if(testing) {
87  std::ofstream testfile;
88  std::string filepath;
89  n.getParam("testlog_filepath", filepath);
90  testfile.open(filepath.c_str(), std::ios::trunc);
91  ROS_DEBUG("Test Nr. 00: Test initialization\n");
92  testfile << "Test Nr. 00:\nIt is tried to bring the PTU in starting position with pan = 0 and tilt = 0\n";
93  testfile << "It is expected that the Actionserver accepts the goal\n";
94  testfile << "Expected return state: SUCCEEDED\n";
95  testfile << "Recieved return state: ";
96  actionlib::SimpleClientGoalState state(ptu_controller->testFunction(0,0).state_, ptu_controller->testFunction(0,0).text_);
97  testfile << state.toString() << "\n";
98  if(state == actionlib::SimpleClientGoalState::SUCCEEDED) {
99  ROS_DEBUG("Correct state returned, for pan/tilt check look into the testlog\n");
100  testfile << "Expected pan: 0\n";
101  testfile << "Recieved pan: " << ptu_controller->actionClient.getResult()->end_joint.position[0] << "\n";
102  testfile << "Expected tilt: 0\n";
103  testfile << "Recieved tilt: " << ptu_controller->actionClient.getResult()->end_joint.position[1] << "\n";
104  }
105  else {
106  ROS_DEBUG("Failure\n");
107  }
108  testfile << "\n";
109 
110  ROS_DEBUG("Test Nr. 01: No Movement\n");
111  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";
112  testfile << "It is expected that the Actionserver takes the goal and executes it\n";
113  testfile << "Expected return state: SUCCEEDED\n";
114  testfile << "Recieved return state: ";
115  state = ptu_controller->testFunction(0,0);
116  testfile << state.toString() << "\n";
117  if(state == actionlib::SimpleClientGoalState::SUCCEEDED) {
118  ROS_DEBUG("Correct state returned, for pan/tilt check look into the testlog\n");
119  testfile << "Expected pan: 0\n";
120  testfile << "Recieved pan: " << ptu_controller->actionClient.getResult()->end_joint.position[0] << "\n";
121  testfile << "Expected tilt: 0\n";
122  testfile << "Recieved tilt: " << ptu_controller->actionClient.getResult()->end_joint.position[1] << "\n";
123  }
124  else {
125  ROS_DEBUG("Failure\n");
126  }
127  testfile << "\n";
128 
129  ROS_DEBUG("Test Nr. 02: Pan Movement\n");
130  testfile << "Test Nr. 02:\nIt is tried to move the pan-unit of the PTU from 0 to 5 degree\n";
131  testfile << "It is expected that the Actionserver accepts the goal and executes it\n";
132  testfile << "Expected return state: SUCCEEDED\n";
133  testfile << "Recieved return state: ";
134  state = ptu_controller->testFunction(5,0);
135  testfile << state.toString() << "\n";
136  if(state == actionlib::SimpleClientGoalState::SUCCEEDED) {
137  ROS_DEBUG("Correct state returned, for pan/tilt check look into the testlog\n");
138  testfile << "Expected pan: ~5\n";
139  testfile << "Recieved pan: " << ptu_controller->actionClient.getResult()->end_joint.position[0] << "\n";
140  testfile << "Expected tilt: 0\n";
141  testfile << "Recieved tilt: " << ptu_controller->actionClient.getResult()->end_joint.position[1] << "\n";
142  }
143  else {
144  ROS_DEBUG("Failure\n");
145  }
146  testfile << "\n";
147 
148  ROS_DEBUG("Test Nr. 03: Tilt Movement\n");
149  testfile << "Test Nr 03:\nIt is tried to move the tilt-unit of the PTU from 0 to 5 degree\n";
150  testfile << "It is expected that the Actionserver accepts the goal and executes it\n";
151  testfile << "Expected return state: SUCCEEDED\n";
152  testfile << "Recieved return state: ";
153  state = ptu_controller->testFunction(5,5);
154  testfile << state.toString() << "\n";
155  if(state == actionlib::SimpleClientGoalState::SUCCEEDED) {
156  ROS_DEBUG("Correct state returned, for pan/tilt check look into the testlog\n");
157  testfile << "Expected pan: ~5\n";
158  testfile << "Recieved pan: " << ptu_controller->actionClient.getResult()->end_joint.position[0] << "\n";
159  testfile << "Expected tilt: ~5\n";
160  testfile << "Recieved tilt: " << ptu_controller->actionClient.getResult()->end_joint.position[1] << "\n";
161  }
162  else {
163  ROS_DEBUG("Failure\n");
164  }
165  testfile << "\n";
166 
167  ROS_DEBUG("Test Nr. 04: Pan + Tilt Movement\n");
168  testfile << "Test Nr. 04:\nIt is tried to move the pan and the tilt unit within the same command from 5 to 10\n";
169  testfile << "It is expected that the Actionserver takes the command and executes it\n";
170  testfile << "Expected return state: SUCCEEDED\n";
171  testfile << "Recieved return state: ";
172  state = ptu_controller->testFunction(10,10);
173  testfile << state.toString() << "\n";
174  if(state == actionlib::SimpleClientGoalState::SUCCEEDED) {
175  ROS_DEBUG("Correct state returned, for pan/tilt check look into the testlog\n");
176  testfile << "Expected pan: ~10\n";
177  testfile << "Recieved pan: " << ptu_controller->actionClient.getResult()->end_joint.position[0] << "\n";
178  testfile << "Expected tilt: ~10\n";
179  testfile << "Recieved tilt: " << ptu_controller->actionClient.getResult()->end_joint.position[1] << "\n";
180  }
181  else {
182  ROS_DEBUG("Failure\n");
183  }
184  testfile << "\n";
185 
186  ROS_DEBUG("Test Nr. 05: Pan out of bounds \n");
187  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";
188  testfile << "It is expected that the server rejects (at the moment instant abort) the request and that no movement happens\n";
189  testfile << "Expected return state: ABORTED\n";
190  testfile << "Recieved return state: ";
191  state = ptu_controller->testFunction(178,10);
192  testfile << state.toString() << "\n";
193  if(state == actionlib::SimpleClientGoalState::ABORTED) {
194  ROS_DEBUG("Correct state returned, for pan/tilt check look into the testlog\n");
195  testfile << "Expected pan: ~10\n";
196  testfile << "Recieved pan: " << ptu_controller->actionClient.getResult()->end_joint.position[0] << "\n";
197  testfile << "Expected tilt: ~10\n";
198  testfile << "Recieved tilt: " << ptu_controller->actionClient.getResult()->end_joint.position[1] << "\n";
199  }
200  else {
201  ROS_DEBUG("Failure\n");
202  }
203  testfile << "\n";
204 
205  ROS_DEBUG("Test Nr. 06: Tilt out of bounds \n");
206  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";
207  testfile << "It is expected that the server rejects (at the moment instant abort) the request and that no movement happens\n";
208  testfile << "Expected return state: ABORTED\n";
209  testfile << "Recieved return state: ";
210  state = ptu_controller->testFunction(10,178);
211  testfile << state.toString() << "\n";
212  if(state == actionlib::SimpleClientGoalState::ABORTED) {
213  ROS_DEBUG("Correct state returned, for pan/tilt check look into the testlog\n");
214  testfile << "Expected pan: ~10\n";
215  testfile << "Recieved pan: " << ptu_controller->actionClient.getResult()->end_joint.position[0] << "\n";
216  testfile << "Expected tilt: ~10\n";
217  testfile << "Recieved tilt: " << ptu_controller->actionClient.getResult()->end_joint.position[1] << "\n";
218  }
219  else {
220  ROS_DEBUG("Failure\n");
221  }
222  testfile << "\n";
223 
224  ROS_DEBUG("Test Nr. 07: In forbidden area \n");
225  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)";
226  testfile << "It is expected that the server rejects (at the moment instant abort) the request and that no movement happens\n";
227  testfile << "Expected return state: ABORTED\n";
228  testfile << "Recieved return state: ";
229  state = ptu_controller->testFunction(-55,-30);
230  testfile << state.toString() << "\n";
231  if(state == actionlib::SimpleClientGoalState::ABORTED) {
232  ROS_DEBUG("Correct state returned, for pan/tilt check look into the testlog\n");
233  testfile << "Expected pan: ~10\n";
234  testfile << "Recieved pan: " << ptu_controller->actionClient.getResult()->end_joint.position[0] << "\n";
235  testfile << "Expected tilt: ~10\n";
236  testfile << "Recieved tilt: " << ptu_controller->actionClient.getResult()->end_joint.position[1] << "\n";
237  }
238  else {
239  ROS_DEBUG("Failure\n");
240  }
241  testfile << "\n";
242 
243  testfile.flush();
244  testfile.close();
245  return 0;
246 
247  }
248  //##################################################
249  if (checkValidation) {
250  ROS_DEBUG("checkValidation");
251  double maxPan;
252  double minPan;
253  double maxTilt;
254  double minTilt;
255 
256  n.getParam("minPan", minPan);
257  n.getParam("maxPan", maxPan);
258  n.getParam("minTilt", minTilt);
259  n.getParam("maxTilt", maxTilt);
260 
261  ptu_controller->checkValidation(maxPan, minPan, maxTilt, minTilt);
262  } else {
263  std::vector<double> pans;
264  std::vector<double> tilts;
265  std::vector<bool> waits;
266  n.getParam("pans",pans);
267  n.getParam("tilts", tilts);
268  n.getParam("waits",waits);
269 
270  if (pans.size() != tilts.size() || pans.size() != waits.size())
271  ROS_ERROR("ERROR in commands.yaml");
272  for(unsigned int i = 0; i < waits.size(); i++) {
273  ptu_controller->sendJoint(pans[i], tilts[i], waits[0]);
274  }
275  }
276 
277  ros::shutdown();
278  spin_thread.join();
279  return 0;
280 }
281 
282 
283 
PTUControllerClient::checkValidation
void checkValidation(double maxPan, double minPan, double maxTilt, double minTilt)
Definition: PTUControllerClient.cpp:37
main
int main(int argc, char **argv)
Definition: PTUControllerClient.cpp:71
PTUControllerClient::goal
asr_flir_ptu_controller::PTUMovementGoal goal
Definition: PTUControllerClient.h:17
epsilon
double epsilon
ros::NodeHandle
actionlib::SimpleActionClient::waitForServer
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
actionlib::SimpleActionClient::waitForResult
bool waitForResult(const ros::Duration &timeout=ros::Duration(0, 0))
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
PTUControllerClient::sendJoint
void sendJoint(double pan, double tilt, bool wait)
Definition: PTUControllerClient.cpp:19
actionlib::SimpleClientGoalState::toString
std::string toString() const
PTUControllerClient::testFunction
actionlib::SimpleClientGoalState testFunction(double pan, double tilt)
Definition: PTUControllerClient.cpp:55
ROS_INFO
#define ROS_INFO(...)
ros::spin
ROSCPP_DECL void spin()
spinThread
void spinThread()
Definition: PTUControllerClient.cpp:5
actionlib::SimpleActionClient::sendGoal
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
PTUControllerClient::actionClient
actionlib::SimpleActionClient< asr_flir_ptu_controller::PTUMovementAction > actionClient
Definition: PTUControllerClient.h:14
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
ros::shutdown
ROSCPP_DECL void shutdown()
actionlib::SimpleActionClient::getState
SimpleClientGoalState getState() const
actionlib::SimpleActionClient::getResult
ResultConstPtr getResult() const
PTUControllerClient::PTUControllerClient
PTUControllerClient(std::string name)
Definition: PTUControllerClient.cpp:9
ROS_ERROR
#define ROS_ERROR(...)
ROS_DEBUG
#define ROS_DEBUG(...)

asr_flir_ptu_controller
Author(s): Ralph Schleicher, Patrick Schlosser
autogenerated on Sun Nov 24 2019 03:28:47