Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
rx28_thread.cpp
1 
2 /***************************************************************************
3  * rx28_thread.cpp - RX28 pan/tilt unit act thread
4  *
5  * Created: Thu Jun 18 09:53:49 2009
6  * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "rx28_thread.h"
23 #include "rx28.h"
24 
25 #include <utils/math/angle.h>
26 #include <core/threading/mutex_locker.h>
27 #include <core/threading/read_write_lock.h>
28 #include <core/threading/scoped_rwlock.h>
29 #include <core/threading/wait_condition.h>
30 #include <interfaces/PanTiltInterface.h>
31 #include <interfaces/LedInterface.h>
32 
33 #include <cstdarg>
34 #include <cmath>
35 #include <unistd.h>
36 
37 using namespace fawkes;
38 
39 /** @class PanTiltRX28Thread "rx28_thread.h"
40  * PanTilt act thread for RX28 PTU.
41  * This thread integrates into the Fawkes main loop at the ACT_EXEC hook and
42  * interacts with the controller of the RX28 PTU.
43  * @author Tim Niemueller
44  */
45 
46 /** Constructor.
47  * @param pantilt_cfg_prefix pantilt plugin configuration prefix
48  * @param ptu_cfg_prefix configuration prefix specific for the PTU
49  * @param ptu_name name of the PTU configuration
50  */
51 PanTiltRX28Thread::PanTiltRX28Thread(std::string &pantilt_cfg_prefix,
52  std::string &ptu_cfg_prefix,
53  std::string &ptu_name)
54  : PanTiltActThread("PanTiltRX28Thread"),
55 #ifdef HAVE_TF
56  TransformAspect(TransformAspect::ONLY_PUBLISHER,
57  (std::string("PTU ") + ptu_name).c_str()),
58 #endif
59  BlackBoardInterfaceListener("PanTiltRX28Thread(%s)", ptu_name.c_str())
60 {
61  set_name("PanTiltRX28Thread(%s)", ptu_name.c_str());
62 
63  __pantilt_cfg_prefix = pantilt_cfg_prefix;
64  __ptu_cfg_prefix = ptu_cfg_prefix;
65  __ptu_name = ptu_name;
66 
67  __rx28 = NULL;
68 }
69 
70 
71 void
73 {
74  __last_pan = __last_tilt = 0.f;
75 
76  // Note: due to the use of auto_ptr and RefPtr resources are automatically
77  // freed on destruction, therefore no special handling is necessary in init()
78  // itself!
79 
80  __cfg_device = config->get_string((__ptu_cfg_prefix + "device").c_str());
81  __cfg_read_timeout_ms = config->get_uint((__ptu_cfg_prefix + "read_timeout_ms").c_str());
82  __cfg_disc_timeout_ms = config->get_uint((__ptu_cfg_prefix + "discover_timeout_ms").c_str());
83  __cfg_pan_servo_id = config->get_uint((__ptu_cfg_prefix + "pan_servo_id").c_str());
84  __cfg_tilt_servo_id = config->get_uint((__ptu_cfg_prefix + "tilt_servo_id").c_str());
85  __cfg_pan_offset = deg2rad(config->get_float((__ptu_cfg_prefix + "pan_offset").c_str()));
86  __cfg_tilt_offset = deg2rad(config->get_float((__ptu_cfg_prefix + "tilt_offset").c_str()));
87  __cfg_goto_zero_start = config->get_bool((__ptu_cfg_prefix + "goto_zero_start").c_str());
88  __cfg_turn_off = config->get_bool((__ptu_cfg_prefix + "turn_off").c_str());
89  __cfg_cw_compl_margin = config->get_uint((__ptu_cfg_prefix + "cw_compl_margin").c_str());
90  __cfg_ccw_compl_margin = config->get_uint((__ptu_cfg_prefix + "ccw_compl_margin").c_str());
91  __cfg_cw_compl_slope = config->get_uint((__ptu_cfg_prefix + "cw_compl_slope").c_str());
92  __cfg_ccw_compl_slope = config->get_uint((__ptu_cfg_prefix + "ccw_compl_slope").c_str());
93  __cfg_pan_min = config->get_float((__ptu_cfg_prefix + "pan_min").c_str());
94  __cfg_pan_max = config->get_float((__ptu_cfg_prefix + "pan_max").c_str());
95  __cfg_tilt_min = config->get_float((__ptu_cfg_prefix + "tilt_min").c_str());
96  __cfg_tilt_max = config->get_float((__ptu_cfg_prefix + "tilt_max").c_str());
97  __cfg_pan_margin = config->get_float((__ptu_cfg_prefix + "pan_margin").c_str());
98  __cfg_tilt_margin = config->get_float((__ptu_cfg_prefix + "tilt_margin").c_str());
99  __cfg_pan_start = config->get_float((__ptu_cfg_prefix + "pan_start").c_str());
100  __cfg_tilt_start = config->get_float((__ptu_cfg_prefix + "tilt_start").c_str());
101 
102 #ifdef HAVE_TF
103  float pan_trans_x =
104  config->get_float((__ptu_cfg_prefix + "pan_trans_x").c_str());
105  float pan_trans_y =
106  config->get_float((__ptu_cfg_prefix + "pan_trans_y").c_str());
107  float pan_trans_z =
108  config->get_float((__ptu_cfg_prefix + "pan_trans_z").c_str());
109  float tilt_trans_x =
110  config->get_float((__ptu_cfg_prefix + "tilt_trans_x").c_str());
111  float tilt_trans_y =
112  config->get_float((__ptu_cfg_prefix + "tilt_trans_y").c_str());
113  float tilt_trans_z =
114  config->get_float((__ptu_cfg_prefix + "tilt_trans_z").c_str());
115 
116 
117  std::string frame_id_prefix = std::string("/") + __ptu_name;
118  try {
119  frame_id_prefix =
120  config->get_string((__ptu_cfg_prefix + "frame_id_prefix").c_str());
121  } catch (Exception &e) {} // ignore, use default
122 
123  __cfg_base_frame = frame_id_prefix + "/base";
124  __cfg_pan_link = frame_id_prefix + "/pan";
125  __cfg_tilt_link = frame_id_prefix + "/tilt";
126 
127  __translation_pan.setValue(pan_trans_x, pan_trans_y, pan_trans_z);
128  __translation_tilt.setValue(tilt_trans_x, tilt_trans_y, tilt_trans_z);
129 #endif
130 
131  bool pan_servo_found = false, tilt_servo_found = false;
132 
133  __rx28 = new RobotisRX28(__cfg_device.c_str(), __cfg_read_timeout_ms);
134  RobotisRX28::DeviceList devl = __rx28->discover();
135  for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
136  if (__cfg_pan_servo_id == *i) {
137  pan_servo_found = true;
138  } else if (__cfg_tilt_servo_id == *i) {
139  tilt_servo_found = true;
140  } else {
141  logger->log_warn(name(), "Servo %u in PTU servo chain, but neither "
142  "configured as pan nor as tilt servo", *i);
143  }
144  }
145 
146  // We only want responses to be sent on explicit READ to speed up communication
148  // set compliance values
150  __cfg_cw_compl_margin, __cfg_cw_compl_slope,
151  __cfg_ccw_compl_margin, __cfg_ccw_compl_slope);
152  __rx28->set_led_enabled(__cfg_pan_servo_id, false);
153 
154 
155  if (! (pan_servo_found && tilt_servo_found)) {
156  throw Exception("Pan and/or tilt servo not found: pan: %i tilt: %i",
157  pan_servo_found, tilt_servo_found);
158  }
159 
160  // If you have more than one interface: catch exception and close them!
161  std::string bbid = "PanTilt " + __ptu_name;
162  __pantilt_if = blackboard->open_for_writing<PanTiltInterface>(bbid.c_str());
163  __pantilt_if->set_calibrated(true);
164  __pantilt_if->set_min_pan(__cfg_pan_min);
165  __pantilt_if->set_max_pan(__cfg_pan_max);
166  __pantilt_if->set_min_tilt(__cfg_tilt_min);
167  __pantilt_if->set_max_tilt(__cfg_tilt_max);
168  __pantilt_if->set_pan_margin(__cfg_pan_margin);
169  __pantilt_if->set_tilt_margin(__cfg_tilt_margin);
170  __pantilt_if->set_max_pan_velocity(__rx28->get_max_supported_speed(__cfg_pan_servo_id));
171  __pantilt_if->set_max_tilt_velocity(__rx28->get_max_supported_speed(__cfg_tilt_servo_id));
172  __pantilt_if->set_pan_velocity(__rx28->get_max_supported_speed(__cfg_pan_servo_id));
173  __pantilt_if->set_tilt_velocity(__rx28->get_max_supported_speed(__cfg_tilt_servo_id));
174  __pantilt_if->write();
175 
176  __led_if = blackboard->open_for_writing<LedInterface>(bbid.c_str());
177 
178  __wt = new WorkerThread(__ptu_name, logger, __rx28,
179  __cfg_pan_servo_id, __cfg_tilt_servo_id,
180  __cfg_pan_min, __cfg_pan_max, __cfg_tilt_min, __cfg_tilt_max,
181  __cfg_pan_offset, __cfg_tilt_offset);
182  __wt->set_margins(__cfg_pan_margin, __cfg_tilt_margin);
183  __wt->start();
184  __wt->set_enabled(true);
185  if ( __cfg_goto_zero_start ) {
186  __wt->goto_pantilt_timed(__cfg_pan_start, __cfg_tilt_start, 3.0);
187  }
188 
189  bbil_add_message_interface(__pantilt_if);
191 
192 #ifdef USE_TIMETRACKER
193  __tt.reset(new TimeTracker());
194  __tt_count = 0;
195  __ttc_read_sensor = __tt->add_class("Read Sensor");
196 #endif
197 
198 }
199 
200 
201 bool
203 {
204  if (__cfg_turn_off) {
205  logger->log_info(name(), "Moving to park position");
206  __wt->goto_pantilt_timed(0, __cfg_tilt_max, 2.0);
207  // we need to wait twice, because the first wakeup is likely to happen
208  // before the command is actually send
209  __wt->wait_for_fresh_data();
210  __wt->wait_for_fresh_data();
211 
212  while (! __wt->is_final()) {
213  //__wt->wakeup();
214  __wt->wait_for_fresh_data();
215  }
216  }
217  return true;
218 }
219 
220 void
222 {
224  blackboard->close(__pantilt_if);
225  blackboard->close(__led_if);
226 
227  __wt->cancel();
228  __wt->join();
229  delete __wt;
230 
231  if (__cfg_turn_off) {
232  logger->log_info(name(), "Turning off PTU");
233  try {
234  __rx28->set_led_enabled(__cfg_pan_servo_id, false);
235  __rx28->set_led_enabled(__cfg_tilt_servo_id, false);
236  __rx28->set_torques_enabled(false, 2, __cfg_pan_servo_id, __cfg_tilt_servo_id);
237  } catch (Exception &e) {
238  logger->log_warn(name(), "Failed to turn of PTU: %s", e.what());
239  }
240  }
241 
242  // Setting to NULL deletes instance (RefPtr)
243  __rx28 = NULL;
244 }
245 
246 
247 /** Update sensor values as necessary.
248  * To be called only from PanTiltSensorThread. Writes the current pan/tilt
249  * data into the interface.
250  */
251 void
253 {
254  if (__wt->has_fresh_data()) {
255  float pan = 0, tilt = 0, panvel=0, tiltvel=0;
256  fawkes::Time time;
257  __wt->get_pantilt(pan, tilt, time);
258  __wt->get_velocities(panvel, tiltvel);
259 
260  // poor man's filter: only update if we get a change of least half a degree
261  if (fabs(__last_pan - pan) >= 0.009 || fabs(__last_tilt - tilt) >= 0.009) {
262  __last_pan = pan;
263  __last_tilt = tilt;
264  } else {
265  pan = __last_pan;
266  tilt = __last_tilt;
267  }
268 
269  __pantilt_if->set_pan(pan);
270  __pantilt_if->set_tilt(tilt);
271  __pantilt_if->set_pan_velocity(panvel);
272  __pantilt_if->set_tilt_velocity(tiltvel);
273  __pantilt_if->set_enabled(__wt->is_enabled());
274  __pantilt_if->set_final(__wt->is_final());
275  __pantilt_if->write();
276 
277 #ifdef HAVE_TF
278  // Always publish updated transforms
279  tf::Quaternion pr; pr.setEulerZYX(pan, 0, 0);
280  tf::Transform ptr(pr, __translation_pan);
281  tf_publisher->send_transform(ptr, time, __cfg_base_frame, __cfg_pan_link);
282 
283  tf::Quaternion tr; tr.setEulerZYX(0, tilt, 0);
284  tf::Transform ttr(tr, __translation_tilt);
285  tf_publisher->send_transform(ttr, time, __cfg_pan_link, __cfg_tilt_link);
286 #endif
287  }
288 }
289 
290 
291 void
293 {
294  __pantilt_if->set_final(__wt->is_final());
295 
296  while (! __pantilt_if->msgq_empty() ) {
297  if (__pantilt_if->msgq_first_is<PanTiltInterface::CalibrateMessage>()) {
298  // ignored
299 
300  } else if (__pantilt_if->msgq_first_is<PanTiltInterface::GotoMessage>()) {
301  PanTiltInterface::GotoMessage *msg = __pantilt_if->msgq_first(msg);
302 
303  __wt->goto_pantilt(msg->pan(), msg->tilt());
304  __pantilt_if->set_msgid(msg->id());
305  __pantilt_if->set_final(false);
306 
307  } else if (__pantilt_if->msgq_first_is<PanTiltInterface::TimedGotoMessage>()) {
308  PanTiltInterface::TimedGotoMessage *msg = __pantilt_if->msgq_first(msg);
309 
310  __wt->goto_pantilt_timed(msg->pan(), msg->tilt(), msg->time_sec());
311  __pantilt_if->set_msgid(msg->id());
312  __pantilt_if->set_final(false);
313 
314  } else if (__pantilt_if->msgq_first_is<PanTiltInterface::ParkMessage>()) {
315  PanTiltInterface::ParkMessage *msg = __pantilt_if->msgq_first(msg);
316 
317  __wt->goto_pantilt(0, 0);
318  __pantilt_if->set_msgid(msg->id());
319  __pantilt_if->set_final(false);
320 
321  } else if (__pantilt_if->msgq_first_is<PanTiltInterface::SetEnabledMessage>()) {
322  PanTiltInterface::SetEnabledMessage *msg = __pantilt_if->msgq_first(msg);
323 
324  __wt->set_enabled(msg->is_enabled());
325 
326  } else if (__pantilt_if->msgq_first_is<PanTiltInterface::SetVelocityMessage>()) {
327  PanTiltInterface::SetVelocityMessage *msg = __pantilt_if->msgq_first(msg);
328 
329  if (msg->pan_velocity() > __pantilt_if->max_pan_velocity()) {
330  logger->log_warn(name(), "Desired pan velocity %f too high, max is %f",
331  msg->pan_velocity(), __pantilt_if->max_pan_velocity());
332  } else if (msg->tilt_velocity() > __pantilt_if->max_tilt_velocity()) {
333  logger->log_warn(name(), "Desired tilt velocity %f too high, max is %f",
334  msg->tilt_velocity(), __pantilt_if->max_tilt_velocity());
335  } else {
336  __wt->set_velocities(msg->pan_velocity(), msg->tilt_velocity());
337  }
338 
339  } else if (__pantilt_if->msgq_first_is<PanTiltInterface::SetMarginMessage>()) {
340  PanTiltInterface::SetMarginMessage *msg = __pantilt_if->msgq_first(msg);
341 
342  __wt->set_margins(msg->pan_margin(), msg->tilt_margin());
343  __pantilt_if->set_pan_margin(msg->pan_margin());
344  __pantilt_if->set_tilt_margin(msg->tilt_margin());
345 
346  } else {
347  logger->log_warn(name(), "Unknown message received");
348  }
349 
350  __pantilt_if->msgq_pop();
351  }
352 
353  __pantilt_if->write();
354 
355  bool write_led_if = false;
356  while (! __led_if->msgq_empty() ) {
357  write_led_if = true;
359  LedInterface::SetIntensityMessage *msg = __led_if->msgq_first(msg);
360  __wt->set_led_enabled((msg->intensity() >= 0.5));
361  __led_if->set_intensity((msg->intensity() >= 0.5) ? LedInterface::ON : LedInterface::OFF);
362  } else if (__led_if->msgq_first_is<LedInterface::TurnOnMessage>()) {
363  __wt->set_led_enabled(true);
364  __led_if->set_intensity(LedInterface::ON);
365  } else if (__led_if->msgq_first_is<LedInterface::TurnOffMessage>()) {
366  __wt->set_led_enabled(false);
367  __led_if->set_intensity(LedInterface::OFF);
368  }
369 
370  __led_if->msgq_pop();
371  }
372  if (write_led_if) __led_if->write();
373 
374  //__wt->wakeup();
375 }
376 
377 
378 bool
380  Message *message) throw()
381 {
382  if (message->is_of_type<PanTiltInterface::StopMessage>()) {
383  __wt->stop_motion();
384  return false; // do not enqueue StopMessage
385  } else if (message->is_of_type<PanTiltInterface::FlushMessage>()) {
386  __wt->stop_motion();
387  logger->log_info(name(), "Flushing message queue");
388  __pantilt_if->msgq_flush();
389  return false;
390  } else {
391  logger->log_info(name(), "Received message of type %s, enqueueing", message->type());
392  return true;
393  }
394 }
395 
396 
397 /** @class PanTiltRX28Thread::WorkerThread "robotis/rx28_thread.h"
398  * Worker thread for the PanTiltRX28Thread.
399  * This continuous thread issues commands to the RX28 chain. In each loop it
400  * will first execute pending operations, and then update the sensor data (lengthy
401  * operation). Sensor data will only be updated while either a servo in the chain
402  * is still moving or torque is disabled (so the motor can be move manually).
403  * @author Tim Niemueller
404  */
405 
406 
407 /** Constructor.
408  * @param ptu_name name of the pan/tilt unit
409  * @param logger logger
410  * @param rx28 RX28 chain
411  * @param pan_servo_id servo ID of the pan servo
412  * @param tilt_servo_id servo ID of the tilt servo
413  * @param pan_min minimum pan in rad
414  * @param pan_max maximum pan in rad
415  * @param tilt_min minimum tilt in rad
416  * @param tilt_max maximum tilt in rad
417  * @param pan_offset pan offset from zero in servo ticks
418  * @param tilt_offset tilt offset from zero in servo ticks
419  */
420 PanTiltRX28Thread::WorkerThread::WorkerThread(std::string ptu_name,
421  fawkes::Logger *logger,
423  unsigned char pan_servo_id,
424  unsigned char tilt_servo_id,
425  float &pan_min, float &pan_max,
426  float &tilt_min, float &tilt_max,
427  float &pan_offset, float &tilt_offset)
428  : Thread("", Thread::OPMODE_WAITFORWAKEUP)
429 {
430  set_name("RX28WorkerThread(%s)", ptu_name.c_str());
431  set_coalesce_wakeups(true);
432 
433  __logger = logger;
434 
435  __value_rwlock = new ReadWriteLock();
436  __rx28_rwlock = new ReadWriteLock();
437  __fresh_data_mutex = new Mutex();
438  __update_waitcond = new WaitCondition();
439 
440  __rx28 = rx28;
441  __move_pending = false;
442  __target_pan = 0;
443  __target_tilt = 0;
444  __pan_servo_id = pan_servo_id;
445  __tilt_servo_id = tilt_servo_id;
446 
447  __pan_min = pan_min;
448  __pan_max = pan_max;
449  __tilt_min = tilt_min;
450  __tilt_max = tilt_max;
451  __pan_offset = pan_offset;
452  __tilt_offset = tilt_offset;
453  __enable = false;
454  __disable = false;
455  __led_enable = false;
456  __led_disable = false;
457 
458  __max_pan_speed = __rx28->get_max_supported_speed(__pan_servo_id);
459  __max_tilt_speed = __rx28->get_max_supported_speed(__tilt_servo_id);
460 }
461 
462 
463 /** Destructor. */
464 PanTiltRX28Thread::WorkerThread::~WorkerThread()
465 {
466  delete __value_rwlock;
467  delete __rx28_rwlock;
468  delete __fresh_data_mutex;
469  delete __update_waitcond;
470 }
471 
472 
473 /** Enable or disable servo.
474  * @param enabled true to enable servos, false to turn them off
475  */
476 void
477 PanTiltRX28Thread::WorkerThread::set_enabled(bool enabled)
478 {
479  ScopedRWLock lock(__value_rwlock);
480  if (enabled) {
481  __enable = true;
482  __disable = false;
483  } else {
484  __enable = false;
485  __disable = true;
486  }
487  wakeup();
488 }
489 
490 
491 /** Enable or disable LED.
492  * @param enabled true to enable LED, false to turn it off
493  */
494 void
495 PanTiltRX28Thread::WorkerThread::set_led_enabled(bool enabled)
496 {
497  ScopedRWLock lock(__value_rwlock);
498  if (enabled) {
499  __led_enable = true;
500  __led_disable = false;
501  } else {
502  __led_enable = false;
503  __led_disable = true;
504  }
505  wakeup();
506 }
507 
508 
509 /** Stop currently running motion. */
510 void
511 PanTiltRX28Thread::WorkerThread::stop_motion()
512 {
513  float pan = 0, tilt = 0;
514  get_pantilt(pan, tilt);
515  goto_pantilt(pan, tilt);
516 }
517 
518 
519 /** Goto desired pan/tilt values.
520  * @param pan pan in radians
521  * @param tilt tilt in radians
522  */
523 void
524 PanTiltRX28Thread::WorkerThread::goto_pantilt(float pan, float tilt)
525 {
526  ScopedRWLock lock(__value_rwlock);
527  __target_pan = pan;
528  __target_tilt = tilt;
529  __move_pending = true;
530  wakeup();
531 }
532 
533 
534 /** Goto desired pan/tilt values in a specified time.
535  * @param pan pan in radians
536  * @param tilt tilt in radians
537  * @param time_sec time when to reach the desired pan/tilt values
538  */
539 void
540 PanTiltRX28Thread::WorkerThread::goto_pantilt_timed(float pan, float tilt, float time_sec)
541 {
542  __target_pan = pan;
543  __target_tilt = tilt;
544  __move_pending = true;
545 
546  float cpan=0, ctilt=0;
547  get_pantilt(cpan, ctilt);
548 
549  float pan_diff = fabs(pan - cpan);
550  float tilt_diff = fabs(tilt - ctilt);
551 
552  float req_pan_vel = pan_diff / time_sec;
553  float req_tilt_vel = tilt_diff / time_sec;
554 
555  //__logger->log_debug(name(), "Current: %f/%f Des: %f/%f Time: %f Diff: %f/%f ReqVel: %f/%f",
556  // cpan, ctilt, pan, tilt, time_sec, pan_diff, tilt_diff, req_pan_vel, req_tilt_vel);
557 
558 
559  if (req_pan_vel > __max_pan_speed) {
560  __logger->log_warn(name(), "Requested move to (%f, %f) in %f sec requires a "
561  "pan speed of %f rad/s, which is greater than the maximum "
562  "of %f rad/s, reducing to max", pan, tilt, time_sec,
563  req_pan_vel, __max_pan_speed);
564  req_pan_vel = __max_pan_speed;
565  }
566 
567  if (req_tilt_vel > __max_tilt_speed) {
568  __logger->log_warn(name(), "Requested move to (%f, %f) in %f sec requires a "
569  "tilt speed of %f rad/s, which is greater than the maximum of "
570  "%f rad/s, reducing to max", pan, tilt, time_sec,
571  req_tilt_vel, __max_tilt_speed);
572  req_tilt_vel = __max_tilt_speed;
573  }
574 
575  set_velocities(req_pan_vel, req_tilt_vel);
576 
577  wakeup();
578 }
579 
580 
581 /** Set desired velocities.
582  * @param pan_vel pan velocity
583  * @param tilt_vel tilt velocity
584  */
585 void
586 PanTiltRX28Thread::WorkerThread::set_velocities(float pan_vel, float tilt_vel)
587 {
588  ScopedRWLock lock(__value_rwlock);
589  float pan_tmp = roundf((pan_vel / __max_pan_speed) * RobotisRX28::MAX_SPEED);
590  float tilt_tmp = roundf((tilt_vel / __max_tilt_speed) * RobotisRX28::MAX_SPEED);
591 
592  //__logger->log_debug(name(), "old speed: %u/%u new speed: %f/%f", __pan_vel,
593  // __tilt_vel, pan_tmp, tilt_tmp);
594 
595  if ((pan_tmp >= 0) && (pan_tmp <= RobotisRX28::MAX_SPEED)) {
596  __pan_vel = (unsigned int)pan_tmp;
597  __velo_pending = true;
598  } else {
599  __logger->log_warn(name(), "Calculated pan value out of bounds, min: 0 max: %u des: %u",
600  RobotisRX28::MAX_SPEED, (unsigned int)pan_tmp);
601  }
602 
603  if ((tilt_tmp >= 0) && (tilt_tmp <= RobotisRX28::MAX_SPEED)) {
604  __tilt_vel = (unsigned int)tilt_tmp;
605  __velo_pending = true;
606  } else {
607  __logger->log_warn(name(), "Calculated tilt value out of bounds, min: 0 max: %u des: %u",
608  RobotisRX28::MAX_SPEED, (unsigned int)tilt_tmp);
609  }
610 }
611 
612 
613 /** Get current velocities.
614  * @param pan_vel upon return contains current pan velocity
615  * @param tilt_vel upon return contains current tilt velocity
616  */
617 void
618 PanTiltRX28Thread::WorkerThread::get_velocities(float &pan_vel, float &tilt_vel)
619 {
620  unsigned int pan_velticks = __rx28->get_goal_speed(__pan_servo_id);
621  unsigned int tilt_velticks = __rx28->get_goal_speed(__tilt_servo_id);
622 
623  pan_velticks = (unsigned int)(((float)pan_velticks / (float)RobotisRX28::MAX_SPEED) * __max_pan_speed);
624  tilt_velticks = (unsigned int)(((float)tilt_velticks / (float)RobotisRX28::MAX_SPEED) * __max_tilt_speed);
625 }
626 
627 
628 /** Set desired velocities.
629  * @param pan_margin pan margin
630  * @param tilt_margin tilt margin
631  */
632 void
633 PanTiltRX28Thread::WorkerThread::set_margins(float pan_margin, float tilt_margin)
634 {
635  if (pan_margin > 0.0) __pan_margin = pan_margin;
636  if (tilt_margin > 0.0) __tilt_margin = tilt_margin;
637  //__logger->log_warn(name(), "Margins set to %f, %f", __pan_margin, __tilt_margin);
638 }
639 
640 
641 /** Get pan/tilt value.
642  * @param pan upon return contains the current pan value
643  * @param tilt upon return contains the current tilt value
644  */
645 void
646 PanTiltRX28Thread::WorkerThread::get_pantilt(float &pan, float &tilt)
647 {
648  ScopedRWLock lock(__rx28_rwlock, ScopedRWLock::LOCK_READ);
649 
650  int pan_ticks = ((int)__rx28->get_position(__pan_servo_id) - (int)RobotisRX28::CENTER_POSITION);
651  int tilt_ticks = ((int)__rx28->get_position(__tilt_servo_id) - (int)RobotisRX28::CENTER_POSITION);
652 
653  pan = pan_ticks * RobotisRX28::RAD_PER_POS_TICK + __pan_offset;
654  tilt = tilt_ticks * RobotisRX28::RAD_PER_POS_TICK + __tilt_offset;
655 }
656 
657 
658 /** Get pan/tilt value with time.
659  * @param pan upon return contains the current pan value
660  * @param tilt upon return contains the current tilt value
661  * @param time upon return contains the time the pan and tilt values were read
662  */
663 void
664 PanTiltRX28Thread::WorkerThread::get_pantilt(float &pan, float &tilt,
665  fawkes::Time &time)
666 {
667  get_pantilt(pan, tilt);
668  time = __pantilt_time;
669 }
670 
671 
672 /** Check if motion is final.
673  * @return true if motion is final, false otherwise
674  */
675 bool
676 PanTiltRX28Thread::WorkerThread::is_final()
677 {
678  float pan, tilt;
679  get_pantilt(pan, tilt);
680 
681  /*
682  __logger->log_debug(name(), "P: %f T: %f TP: %f TT: %f PM: %f TM: %f PMov: %i TMov: %i Final: %s",
683  pan, tilt, __target_pan, __target_tilt, __pan_margin, __tilt_margin,
684  __rx28->is_moving(__pan_servo_id), __rx28->is_moving(__tilt_servo_id),
685  (( (fabs(pan - __target_pan) <= __pan_margin) &&
686  (fabs(tilt - __target_tilt) <= __tilt_margin) ) ||
687  (! __rx28->is_moving(__pan_servo_id) &&
688  ! __rx28->is_moving(__tilt_servo_id))) ? "YES" : "NO");
689  */
690 
691  ScopedRWLock lock(__rx28_rwlock, ScopedRWLock::LOCK_READ);
692 
693  return ( (fabs(pan - __target_pan) <= __pan_margin) &&
694  (fabs(tilt - __target_tilt) <= __tilt_margin) ) ||
695  (! __rx28->is_moving(__pan_servo_id) &&
696  ! __rx28->is_moving(__tilt_servo_id));
697 }
698 
699 
700 /** Check if PTU is enabled.
701  * @return true if torque is enabled for both servos, false otherwise
702  */
703 bool
704 PanTiltRX28Thread::WorkerThread::is_enabled()
705 {
706  return (__rx28->is_torque_enabled(__pan_servo_id) &&
707  __rx28->is_torque_enabled(__tilt_servo_id));
708 }
709 
710 
711 /** Check is fresh sensor data is available.
712  * Note that this method will return true at once per sensor update cycle.
713  * @return true if fresh data is available, false otherwise
714  */
715 bool
716 PanTiltRX28Thread::WorkerThread::has_fresh_data()
717 {
718  MutexLocker lock(__fresh_data_mutex);
719 
720  bool rv = __fresh_data;
721  __fresh_data = false;
722  return rv;
723 }
724 
725 
726 void
727 PanTiltRX28Thread::WorkerThread::loop()
728 {
729  if (__enable) {
730  __value_rwlock->lock_for_write();
731  __enable = false;
732  __value_rwlock->unlock();
733  ScopedRWLock lock(__rx28_rwlock);
734  __rx28->set_led_enabled(__tilt_servo_id, true);
735  __rx28->set_torques_enabled(true, 2, __pan_servo_id, __tilt_servo_id);
736  } else if (__disable) {
737  __value_rwlock->lock_for_write();
738  __disable = false;
739  __value_rwlock->unlock();
740  ScopedRWLock lock(__rx28_rwlock);
741  if (__led_enable || __led_disable || __velo_pending || __move_pending) usleep(3000);
742  }
743 
744  if (__led_enable) {
745  __value_rwlock->lock_for_write();
746  __led_enable = false;
747  __value_rwlock->unlock();
748  ScopedRWLock lock(__rx28_rwlock);
749  __rx28->set_led_enabled(__pan_servo_id, true);
750  if (__velo_pending || __move_pending) usleep(3000);
751  } else if (__led_disable) {
752  __value_rwlock->lock_for_write();
753  __led_disable = false;
754  __value_rwlock->unlock();
755  ScopedRWLock lock(__rx28_rwlock);
756  __rx28->set_led_enabled(__pan_servo_id, false);
757  if (__velo_pending || __move_pending) usleep(3000);
758  }
759 
760  if (__velo_pending) {
761  __value_rwlock->lock_for_write();
762  __velo_pending = false;
763  unsigned int pan_vel = __pan_vel;
764  unsigned int tilt_vel = __tilt_vel;
765  __value_rwlock->unlock();
766  ScopedRWLock lock(__rx28_rwlock);
767  __rx28->set_goal_speeds(2, __pan_servo_id, pan_vel, __tilt_servo_id, tilt_vel);
768  if (__move_pending) usleep(3000);
769  }
770 
771  if (__move_pending) {
772  __value_rwlock->lock_for_write();
773  __move_pending = false;
774  float target_pan = __target_pan;
775  float target_tilt = __target_tilt;
776  __value_rwlock->unlock();
777  exec_goto_pantilt(target_pan, target_tilt);
778  }
779 
780  try {
781  ScopedRWLock lock(__rx28_rwlock, ScopedRWLock::LOCK_READ);
782  __rx28->read_table_values(__pan_servo_id);
783  __rx28->read_table_values(__tilt_servo_id);
784  {
785  MutexLocker lock_fresh_data(__fresh_data_mutex);
786  __fresh_data = true;
787  __pantilt_time.stamp();
788  }
789  } catch (Exception &e) {
790  // usually just a timeout, too noisy
791  //__logger->log_warn(name(), "Error while reading table values from servos, exception follows");
792  //__logger->log_warn(name(), e);
793  }
794 
795  //if (! is_final() ||
796  // ! __rx28->is_torque_enabled(__pan_servo_id) ||
797  // ! __rx28->is_torque_enabled(__tilt_servo_id)) {
798  // while moving, and while the motor is off, wake us up to get new servo
799  // position data
800  //wakeup();
801  //}
802 
803  __update_waitcond->wake_all();
804 
805  // Wakeup ourselves for faster updates
806  wakeup();
807 }
808 
809 
810 /** Execute pan/tilt motion.
811  * @param pan_rad pan in rad to move to
812  * @param tilt_rad tilt in rad to move to
813  */
814 void
815 PanTiltRX28Thread::WorkerThread::exec_goto_pantilt(float pan_rad, float tilt_rad)
816 {
817  if ( (pan_rad < __pan_min) || (pan_rad > __pan_max) ) {
818  __logger->log_warn(name(), "Pan value out of bounds, min: %f max: %f des: %f",
819  __pan_min, __pan_max, pan_rad);
820  return;
821  }
822  if ( (tilt_rad < __tilt_min) || (tilt_rad > __tilt_max) ) {
823  __logger->log_warn(name(), "Tilt value out of bounds, min: %f max: %f des: %f",
824  __tilt_min, __tilt_max, tilt_rad);
825  return;
826  }
827 
828  unsigned int pan_min = 0, pan_max = 0, tilt_min = 0, tilt_max = 0;
829 
830  __rx28->get_angle_limits(__pan_servo_id, pan_min, pan_max);
831  __rx28->get_angle_limits(__tilt_servo_id, tilt_min, tilt_max);
832 
833 
834  int pan_pos = (int)roundf(RobotisRX28::POS_TICKS_PER_RAD * (pan_rad - __pan_offset))
836  int tilt_pos = (int)roundf(RobotisRX28::POS_TICKS_PER_RAD * (tilt_rad - __tilt_offset))
838 
839  if ( (pan_pos < 0) || ((unsigned int)pan_pos < pan_min) || ((unsigned int)pan_pos > pan_max) ) {
840  __logger->log_warn(name(), "Pan position out of bounds, min: %u max: %u des: %i",
841  pan_min, pan_max, pan_pos);
842  return;
843  }
844 
845  if ( (tilt_pos < 0) || ((unsigned int)tilt_pos < tilt_min) || ((unsigned int)tilt_pos > tilt_max) ) {
846  __logger->log_warn(name(), "Tilt position out of bounds, min: %u max: %u des: %i",
847  tilt_min, tilt_max, tilt_pos);
848  return;
849  }
850 
851  ScopedRWLock lock(__rx28_rwlock);
852  __rx28->goto_positions(2, __pan_servo_id, pan_pos, __tilt_servo_id, tilt_pos);
853 }
854 
855 
856 /** Wait for fresh data to be received.
857  * Blocks the calling thread.
858  */
859 void
860 PanTiltRX28Thread::WorkerThread::wait_for_fresh_data()
861 {
862  __update_waitcond->wait();
863 }