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