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 
24 #include "rx28.h"
25 
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/JointInterface.h>
31 #include <interfaces/LedInterface.h>
32 #include <interfaces/PanTiltInterface.h>
33 #include <utils/math/angle.h>
34 
35 #include <cmath>
36 #include <cstdarg>
37 #include <unistd.h>
38 
39 using namespace fawkes;
40 
41 /** @class PanTiltRX28Thread "rx28_thread.h"
42  * PanTilt act thread for RX28 PTU.
43  * This thread integrates into the Fawkes main loop at the ACT_EXEC hook and
44  * interacts with the controller of the RX28 PTU.
45  * @author Tim Niemueller
46  */
47 
48 /** Constructor.
49  * @param pantilt_cfg_prefix pantilt plugin configuration prefix
50  * @param ptu_cfg_prefix configuration prefix specific for the PTU
51  * @param ptu_name name of the PTU configuration
52  */
53 PanTiltRX28Thread::PanTiltRX28Thread(std::string &pantilt_cfg_prefix,
54  std::string &ptu_cfg_prefix,
55  std::string &ptu_name)
56 : PanTiltActThread("PanTiltRX28Thread"),
57 #ifdef HAVE_TF
58  TransformAspect(TransformAspect::ONLY_PUBLISHER, (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 void
73 {
74  last_pan_ = last_tilt_ = 0.f;
75  float init_pan_velocity = 0.f;
76  float init_tilt_velocity = 0.f;
77 
78  // Note: due to the use of auto_ptr and RefPtr resources are automatically
79  // freed on destruction, therefore no special handling is necessary in init()
80  // itself!
81 
82  cfg_device_ = config->get_string((ptu_cfg_prefix_ + "device").c_str());
83  cfg_read_timeout_ms_ = config->get_uint((ptu_cfg_prefix_ + "read_timeout_ms").c_str());
84  cfg_disc_timeout_ms_ = config->get_uint((ptu_cfg_prefix_ + "discover_timeout_ms").c_str());
85  cfg_pan_servo_id_ = config->get_uint((ptu_cfg_prefix_ + "pan_servo_id").c_str());
86  cfg_tilt_servo_id_ = config->get_uint((ptu_cfg_prefix_ + "tilt_servo_id").c_str());
87  cfg_pan_offset_ = deg2rad(config->get_float((ptu_cfg_prefix_ + "pan_offset").c_str()));
88  cfg_tilt_offset_ = deg2rad(config->get_float((ptu_cfg_prefix_ + "tilt_offset").c_str()));
89  cfg_goto_zero_start_ = config->get_bool((ptu_cfg_prefix_ + "goto_zero_start").c_str());
90  cfg_turn_off_ = config->get_bool((ptu_cfg_prefix_ + "turn_off").c_str());
91  cfg_cw_compl_margin_ = config->get_uint((ptu_cfg_prefix_ + "cw_compl_margin").c_str());
92  cfg_ccw_compl_margin_ = config->get_uint((ptu_cfg_prefix_ + "ccw_compl_margin").c_str());
93  cfg_cw_compl_slope_ = config->get_uint((ptu_cfg_prefix_ + "cw_compl_slope").c_str());
94  cfg_ccw_compl_slope_ = config->get_uint((ptu_cfg_prefix_ + "ccw_compl_slope").c_str());
95  cfg_pan_min_ = config->get_float((ptu_cfg_prefix_ + "pan_min").c_str());
96  cfg_pan_max_ = config->get_float((ptu_cfg_prefix_ + "pan_max").c_str());
97  cfg_tilt_min_ = config->get_float((ptu_cfg_prefix_ + "tilt_min").c_str());
98  cfg_tilt_max_ = config->get_float((ptu_cfg_prefix_ + "tilt_max").c_str());
99  cfg_pan_margin_ = config->get_float((ptu_cfg_prefix_ + "pan_margin").c_str());
100  cfg_tilt_margin_ = config->get_float((ptu_cfg_prefix_ + "tilt_margin").c_str());
101  cfg_pan_start_ = config->get_float((ptu_cfg_prefix_ + "pan_start").c_str());
102  cfg_tilt_start_ = config->get_float((ptu_cfg_prefix_ + "tilt_start").c_str());
103 #ifdef HAVE_TF
104  cfg_publish_transforms_ = config->get_bool((ptu_cfg_prefix_ + "publish_transforms").c_str());
105 #endif
106 
107 #ifdef HAVE_TF
108  if (cfg_publish_transforms_) {
109  float pan_trans_x = config->get_float((ptu_cfg_prefix_ + "pan_trans_x").c_str());
110  float pan_trans_y = config->get_float((ptu_cfg_prefix_ + "pan_trans_y").c_str());
111  float pan_trans_z = config->get_float((ptu_cfg_prefix_ + "pan_trans_z").c_str());
112  float tilt_trans_x = config->get_float((ptu_cfg_prefix_ + "tilt_trans_x").c_str());
113  float tilt_trans_y = config->get_float((ptu_cfg_prefix_ + "tilt_trans_y").c_str());
114  float tilt_trans_z = config->get_float((ptu_cfg_prefix_ + "tilt_trans_z").c_str());
115 
116  std::string frame_id_prefix = std::string("") + ptu_name_;
117  try {
118  frame_id_prefix = config->get_string((ptu_cfg_prefix_ + "frame_id_prefix").c_str());
119  } catch (Exception &e) {
120  } // ignore, use default
121 
122  cfg_base_frame_ = frame_id_prefix + "/base";
123  cfg_pan_link_ = frame_id_prefix + "/pan";
124  cfg_tilt_link_ = frame_id_prefix + "/tilt";
125 
126  translation_pan_.setValue(pan_trans_x, pan_trans_y, pan_trans_z);
127  translation_tilt_.setValue(tilt_trans_x, tilt_trans_y, tilt_trans_z);
128  }
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(),
142  "Servo %u in PTU servo chain, but neither "
143  "configured as pan nor as tilt servo",
144  *i);
145  }
146  }
147 
148  // We only want responses to be sent on explicit READ to speed up communication
150  // set compliance values
152  cfg_cw_compl_margin_,
153  cfg_cw_compl_slope_,
154  cfg_ccw_compl_margin_,
155  cfg_ccw_compl_slope_);
156  rx28_->set_led_enabled(cfg_pan_servo_id_, false);
157 
158  if (!(pan_servo_found && tilt_servo_found)) {
159  throw Exception("Pan and/or tilt servo not found: pan: %i tilt: %i",
160  pan_servo_found,
161  tilt_servo_found);
162  }
163 
164  // If you have more than one interface: catch exception and close them!
165  std::string bbid = "PanTilt " + ptu_name_;
166  pantilt_if_ = blackboard->open_for_writing<PanTiltInterface>(bbid.c_str());
167  pantilt_if_->set_calibrated(true);
168  pantilt_if_->set_min_pan(cfg_pan_min_);
169  pantilt_if_->set_max_pan(cfg_pan_max_);
170  pantilt_if_->set_min_tilt(cfg_tilt_min_);
171  pantilt_if_->set_max_tilt(cfg_tilt_max_);
172  pantilt_if_->set_pan_margin(cfg_pan_margin_);
173  pantilt_if_->set_tilt_margin(cfg_tilt_margin_);
174  pantilt_if_->set_max_pan_velocity(rx28_->get_max_supported_speed(cfg_pan_servo_id_));
175  pantilt_if_->set_max_tilt_velocity(rx28_->get_max_supported_speed(cfg_tilt_servo_id_));
176  pantilt_if_->set_pan_velocity(init_pan_velocity);
177  pantilt_if_->set_tilt_velocity(init_tilt_velocity);
178  pantilt_if_->write();
179 
180  led_if_ = blackboard->open_for_writing<LedInterface>(bbid.c_str());
181 
182  std::string panid = ptu_name_ + " pan";
183  panjoint_if_ = blackboard->open_for_writing<JointInterface>(panid.c_str());
184  panjoint_if_->set_position(last_pan_);
185  panjoint_if_->set_velocity(init_pan_velocity);
186  panjoint_if_->write();
187 
188  std::string tiltid = ptu_name_ + " tilt";
189  tiltjoint_if_ = blackboard->open_for_writing<JointInterface>(tiltid.c_str());
190  tiltjoint_if_->set_position(last_tilt_);
191  tiltjoint_if_->set_velocity(init_tilt_velocity);
192  tiltjoint_if_->write();
193 
194  wt_ = new WorkerThread(ptu_name_,
195  logger,
196  rx28_,
197  cfg_pan_servo_id_,
198  cfg_tilt_servo_id_,
199  cfg_pan_min_,
200  cfg_pan_max_,
201  cfg_tilt_min_,
202  cfg_tilt_max_,
203  cfg_pan_offset_,
204  cfg_tilt_offset_);
205  wt_->set_margins(cfg_pan_margin_, cfg_tilt_margin_);
206  wt_->start();
207  wt_->set_enabled(true);
208  if (cfg_goto_zero_start_) {
209  wt_->goto_pantilt_timed(cfg_pan_start_, cfg_tilt_start_, 3.0);
210  }
211 
212  bbil_add_message_interface(pantilt_if_);
213  bbil_add_message_interface(panjoint_if_);
214  bbil_add_message_interface(tiltjoint_if_);
216 
217 #ifdef USE_TIMETRACKER
218  tt_.reset(new TimeTracker());
219  tt_count_ = 0;
220  ttc_read_sensor_ = tt_->add_class("Read Sensor");
221 #endif
222 }
223 
224 bool
226 {
227  if (cfg_turn_off_) {
228  logger->log_info(name(), "Moving to park position");
229  wt_->goto_pantilt_timed(0, cfg_tilt_max_, 2.0);
230  // we need to wait twice, because the first wakeup is likely to happen
231  // before the command is actually send
232  wt_->wait_for_fresh_data();
233  wt_->wait_for_fresh_data();
234 
235  while (!wt_->is_final()) {
236  //wt_->wakeup();
237  wt_->wait_for_fresh_data();
238  }
239  }
240  return true;
241 }
242 
243 void
245 {
247  blackboard->close(pantilt_if_);
248  blackboard->close(led_if_);
249  blackboard->close(panjoint_if_);
250  blackboard->close(tiltjoint_if_);
251 
252  wt_->cancel();
253  wt_->join();
254  delete wt_;
255 
256  if (cfg_turn_off_) {
257  logger->log_info(name(), "Turning off PTU");
258  try {
259  rx28_->set_led_enabled(cfg_pan_servo_id_, false);
260  rx28_->set_led_enabled(cfg_tilt_servo_id_, false);
261  rx28_->set_torques_enabled(false, 2, cfg_pan_servo_id_, cfg_tilt_servo_id_);
262  } catch (Exception &e) {
263  logger->log_warn(name(), "Failed to turn of PTU: %s", e.what());
264  }
265  }
266 
267  // Setting to NULL deletes instance (RefPtr)
268  rx28_ = NULL;
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;
313  pr.setEulerZYX(pan, 0, 0);
314  tf::Transform ptr(pr, translation_pan_);
315  tf_publisher->send_transform(ptr, time, cfg_base_frame_, cfg_pan_link_);
316 
317  tf::Quaternion tr;
318  tr.setEulerZYX(0, tilt, 0);
319  tf::Transform ttr(tr, translation_tilt_);
320  tf_publisher->send_transform(ttr, time, cfg_pan_link_, cfg_tilt_link_);
321  }
322 #endif
323  }
324 }
325 
326 void
328 {
329  pantilt_if_->set_final(wt_->is_final());
330 
331  while (!pantilt_if_->msgq_empty()) {
333  // ignored
334 
335  } else if (pantilt_if_->msgq_first_is<PanTiltInterface::GotoMessage>()) {
336  PanTiltInterface::GotoMessage *msg = pantilt_if_->msgq_first(msg);
337 
338  wt_->goto_pantilt(msg->pan(), msg->tilt());
339  pantilt_if_->set_msgid(msg->id());
340  pantilt_if_->set_final(false);
341 
342  } else if (pantilt_if_->msgq_first_is<PanTiltInterface::TimedGotoMessage>()) {
343  PanTiltInterface::TimedGotoMessage *msg = pantilt_if_->msgq_first(msg);
344 
345  wt_->goto_pantilt_timed(msg->pan(), msg->tilt(), msg->time_sec());
346  pantilt_if_->set_msgid(msg->id());
347  pantilt_if_->set_final(false);
348 
349  } else if (pantilt_if_->msgq_first_is<PanTiltInterface::ParkMessage>()) {
350  PanTiltInterface::ParkMessage *msg = pantilt_if_->msgq_first(msg);
351 
352  wt_->goto_pantilt(0, 0);
353  pantilt_if_->set_msgid(msg->id());
354  pantilt_if_->set_final(false);
355 
356  } else if (pantilt_if_->msgq_first_is<PanTiltInterface::SetEnabledMessage>()) {
357  PanTiltInterface::SetEnabledMessage *msg = pantilt_if_->msgq_first(msg);
358 
359  wt_->set_enabled(msg->is_enabled());
360 
361  } else if (pantilt_if_->msgq_first_is<PanTiltInterface::SetVelocityMessage>()) {
362  PanTiltInterface::SetVelocityMessage *msg = pantilt_if_->msgq_first(msg);
363 
364  if (msg->pan_velocity() > pantilt_if_->max_pan_velocity()) {
365  logger->log_warn(name(),
366  "Desired pan velocity %f too high, max is %f",
367  msg->pan_velocity(),
368  pantilt_if_->max_pan_velocity());
369  } else if (msg->tilt_velocity() > pantilt_if_->max_tilt_velocity()) {
370  logger->log_warn(name(),
371  "Desired tilt velocity %f too high, max is %f",
372  msg->tilt_velocity(),
373  pantilt_if_->max_tilt_velocity());
374  } else {
375  wt_->set_velocities(msg->pan_velocity(), msg->tilt_velocity());
376  }
377 
378  } else if (pantilt_if_->msgq_first_is<PanTiltInterface::SetMarginMessage>()) {
379  PanTiltInterface::SetMarginMessage *msg = pantilt_if_->msgq_first(msg);
380 
381  wt_->set_margins(msg->pan_margin(), msg->tilt_margin());
382  pantilt_if_->set_pan_margin(msg->pan_margin());
383  pantilt_if_->set_tilt_margin(msg->tilt_margin());
384 
385  } else {
386  logger->log_warn(name(), "Unknown message received");
387  }
388 
389  pantilt_if_->msgq_pop();
390  }
391 
392  pantilt_if_->write();
393 
394  bool write_led_if = false;
395  while (!led_if_->msgq_empty()) {
396  write_led_if = true;
398  LedInterface::SetIntensityMessage *msg = led_if_->msgq_first(msg);
399  wt_->set_led_enabled((msg->intensity() >= 0.5));
400  led_if_->set_intensity((msg->intensity() >= 0.5) ? LedInterface::ON : LedInterface::OFF);
401  } else if (led_if_->msgq_first_is<LedInterface::TurnOnMessage>()) {
402  wt_->set_led_enabled(true);
403  led_if_->set_intensity(LedInterface::ON);
404  } else if (led_if_->msgq_first_is<LedInterface::TurnOffMessage>()) {
405  wt_->set_led_enabled(false);
406  led_if_->set_intensity(LedInterface::OFF);
407  }
408 
409  led_if_->msgq_pop();
410  }
411  if (write_led_if)
412  led_if_->write();
413 
414  //wt_->wakeup();
415 }
416 
417 bool
419 {
420  if (message->is_of_type<PanTiltInterface::StopMessage>()) {
421  wt_->stop_motion();
422  return false; // do not enqueue StopMessage
423  } else if (message->is_of_type<PanTiltInterface::FlushMessage>()) {
424  wt_->stop_motion();
425  logger->log_info(name(), "Flushing message queue");
426  pantilt_if_->msgq_flush();
427  return false;
428  } else {
429  logger->log_info(name(), "Received message of type %s, enqueueing", message->type());
430  return true;
431  }
432 }
433 
434 /** @class PanTiltRX28Thread::WorkerThread "robotis/rx28_thread.h"
435  * Worker thread for the PanTiltRX28Thread.
436  * This continuous thread issues commands to the RX28 chain. In each loop it
437  * will first execute pending operations, and then update the sensor data (lengthy
438  * operation). Sensor data will only be updated while either a servo in the chain
439  * is still moving or torque is disabled (so the motor can be move manually).
440  * @author Tim Niemueller
441  */
442 
443 /** Constructor.
444  * @param ptu_name name of the pan/tilt unit
445  * @param logger logger
446  * @param rx28 RX28 chain
447  * @param pan_servo_id servo ID of the pan servo
448  * @param tilt_servo_id servo ID of the tilt servo
449  * @param pan_min minimum pan in rad
450  * @param pan_max maximum pan in rad
451  * @param tilt_min minimum tilt in rad
452  * @param tilt_max maximum tilt in rad
453  * @param pan_offset pan offset from zero in servo ticks
454  * @param tilt_offset tilt offset from zero in servo ticks
455  */
456 PanTiltRX28Thread::WorkerThread::WorkerThread(std::string ptu_name,
457  fawkes::Logger * logger,
459  unsigned char pan_servo_id,
460  unsigned char tilt_servo_id,
461  float & pan_min,
462  float & pan_max,
463  float & tilt_min,
464  float & tilt_max,
465  float & pan_offset,
466  float & tilt_offset)
467 : Thread("", Thread::OPMODE_WAITFORWAKEUP)
468 {
469  set_name("RX28WorkerThread(%s)", ptu_name.c_str());
470  set_coalesce_wakeups(true);
471 
472  logger_ = logger;
473 
474  value_rwlock_ = new ReadWriteLock();
475  rx28_rwlock_ = new ReadWriteLock();
476  fresh_data_mutex_ = new Mutex();
477  update_waitcond_ = new WaitCondition();
478 
479  rx28_ = rx28;
480  move_pending_ = false;
481  target_pan_ = 0;
482  target_tilt_ = 0;
483  pan_servo_id_ = pan_servo_id;
484  tilt_servo_id_ = tilt_servo_id;
485 
486  pan_min_ = pan_min;
487  pan_max_ = pan_max;
488  tilt_min_ = tilt_min;
489  tilt_max_ = tilt_max;
490  pan_offset_ = pan_offset;
491  tilt_offset_ = tilt_offset;
492  enable_ = false;
493  disable_ = false;
494  led_enable_ = false;
495  led_disable_ = false;
496 
497  max_pan_speed_ = rx28_->get_max_supported_speed(pan_servo_id_);
498  max_tilt_speed_ = rx28_->get_max_supported_speed(tilt_servo_id_);
499 }
500 
501 /** Destructor. */
502 PanTiltRX28Thread::WorkerThread::~WorkerThread()
503 {
504  delete value_rwlock_;
505  delete rx28_rwlock_;
506  delete fresh_data_mutex_;
507  delete update_waitcond_;
508 }
509 
510 /** Enable or disable servo.
511  * @param enabled true to enable servos, false to turn them off
512  */
513 void
514 PanTiltRX28Thread::WorkerThread::set_enabled(bool enabled)
515 {
516  ScopedRWLock lock(value_rwlock_);
517  if (enabled) {
518  enable_ = true;
519  disable_ = false;
520  } else {
521  enable_ = false;
522  disable_ = true;
523  }
524  wakeup();
525 }
526 
527 /** Enable or disable LED.
528  * @param enabled true to enable LED, false to turn it off
529  */
530 void
531 PanTiltRX28Thread::WorkerThread::set_led_enabled(bool enabled)
532 {
533  ScopedRWLock lock(value_rwlock_);
534  if (enabled) {
535  led_enable_ = true;
536  led_disable_ = false;
537  } else {
538  led_enable_ = false;
539  led_disable_ = true;
540  }
541  wakeup();
542 }
543 
544 /** Stop currently running motion. */
545 void
546 PanTiltRX28Thread::WorkerThread::stop_motion()
547 {
548  float pan = 0, tilt = 0;
549  get_pantilt(pan, tilt);
550  goto_pantilt(pan, tilt);
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 /** Goto desired pan/tilt values in a specified time.
568  * @param pan pan in radians
569  * @param tilt tilt in radians
570  * @param time_sec time when to reach the desired pan/tilt values
571  */
572 void
573 PanTiltRX28Thread::WorkerThread::goto_pantilt_timed(float pan, float tilt, float time_sec)
574 {
575  target_pan_ = pan;
576  target_tilt_ = tilt;
577  move_pending_ = true;
578 
579  float cpan = 0, ctilt = 0;
580  get_pantilt(cpan, ctilt);
581 
582  float pan_diff = fabs(pan - cpan);
583  float tilt_diff = fabs(tilt - ctilt);
584 
585  float req_pan_vel = pan_diff / time_sec;
586  float req_tilt_vel = tilt_diff / time_sec;
587 
588  //logger_->log_debug(name(), "Current: %f/%f Des: %f/%f Time: %f Diff: %f/%f ReqVel: %f/%f",
589  // cpan, ctilt, pan, tilt, time_sec, pan_diff, tilt_diff, req_pan_vel, req_tilt_vel);
590 
591  if (req_pan_vel > max_pan_speed_) {
592  logger_->log_warn(name(),
593  "Requested move to (%f, %f) in %f sec requires a "
594  "pan speed of %f rad/s, which is greater than the maximum "
595  "of %f rad/s, reducing to max",
596  pan,
597  tilt,
598  time_sec,
599  req_pan_vel,
600  max_pan_speed_);
601  req_pan_vel = max_pan_speed_;
602  }
603 
604  if (req_tilt_vel > max_tilt_speed_) {
605  logger_->log_warn(name(),
606  "Requested move to (%f, %f) in %f sec requires a "
607  "tilt speed of %f rad/s, which is greater than the maximum of "
608  "%f rad/s, reducing to max",
609  pan,
610  tilt,
611  time_sec,
612  req_tilt_vel,
613  max_tilt_speed_);
614  req_tilt_vel = max_tilt_speed_;
615  }
616 
617  set_velocities(req_pan_vel, req_tilt_vel);
618 
619  wakeup();
620 }
621 
622 /** Set desired velocities.
623  * @param pan_vel pan velocity
624  * @param tilt_vel tilt velocity
625  */
626 void
627 PanTiltRX28Thread::WorkerThread::set_velocities(float pan_vel, float tilt_vel)
628 {
629  ScopedRWLock lock(value_rwlock_);
630  float pan_tmp = roundf((pan_vel / max_pan_speed_) * RobotisRX28::MAX_SPEED);
631  float tilt_tmp = roundf((tilt_vel / max_tilt_speed_) * RobotisRX28::MAX_SPEED);
632 
633  //logger_->log_debug(name(), "old speed: %u/%u new speed: %f/%f", pan_vel_,
634  // tilt_vel_, pan_tmp, tilt_tmp);
635 
636  if ((pan_tmp >= 0) && (pan_tmp <= RobotisRX28::MAX_SPEED)) {
637  pan_vel_ = (unsigned int)pan_tmp;
638  velo_pending_ = true;
639  } else {
640  logger_->log_warn(name(),
641  "Calculated pan value out of bounds, min: 0 max: %u des: %u",
643  (unsigned int)pan_tmp);
644  }
645 
646  if ((tilt_tmp >= 0) && (tilt_tmp <= RobotisRX28::MAX_SPEED)) {
647  tilt_vel_ = (unsigned int)tilt_tmp;
648  velo_pending_ = true;
649  } else {
650  logger_->log_warn(name(),
651  "Calculated tilt value out of bounds, min: 0 max: %u des: %u",
653  (unsigned int)tilt_tmp);
654  }
655 }
656 
657 /** Get current velocities.
658  * @param pan_vel upon return contains current pan velocity
659  * @param tilt_vel upon return contains current tilt velocity
660  */
661 void
662 PanTiltRX28Thread::WorkerThread::get_velocities(float &pan_vel, float &tilt_vel)
663 {
664  unsigned int pan_velticks = rx28_->get_goal_speed(pan_servo_id_);
665  unsigned int tilt_velticks = rx28_->get_goal_speed(tilt_servo_id_);
666 
667  pan_velticks =
668  (unsigned int)(((float)pan_velticks / (float)RobotisRX28::MAX_SPEED) * max_pan_speed_);
669  tilt_velticks =
670  (unsigned int)(((float)tilt_velticks / (float)RobotisRX28::MAX_SPEED) * max_tilt_speed_);
671 }
672 
673 /** Set desired velocities.
674  * @param pan_margin pan margin
675  * @param tilt_margin tilt margin
676  */
677 void
678 PanTiltRX28Thread::WorkerThread::set_margins(float pan_margin, float tilt_margin)
679 {
680  if (pan_margin > 0.0)
681  pan_margin_ = pan_margin;
682  if (tilt_margin > 0.0)
683  tilt_margin_ = tilt_margin;
684  //logger_->log_warn(name(), "Margins set to %f, %f", pan_margin_, tilt_margin_);
685 }
686 
687 /** Get pan/tilt value.
688  * @param pan upon return contains the current pan value
689  * @param tilt upon return contains the current tilt value
690  */
691 void
692 PanTiltRX28Thread::WorkerThread::get_pantilt(float &pan, float &tilt)
693 {
694  ScopedRWLock lock(rx28_rwlock_, ScopedRWLock::LOCK_READ);
695 
696  int pan_ticks = ((int)rx28_->get_position(pan_servo_id_) - (int)RobotisRX28::CENTER_POSITION);
697  int tilt_ticks = ((int)rx28_->get_position(tilt_servo_id_) - (int)RobotisRX28::CENTER_POSITION);
698 
699  pan = pan_ticks * RobotisRX28::RAD_PER_POS_TICK + pan_offset_;
700  tilt = tilt_ticks * RobotisRX28::RAD_PER_POS_TICK + tilt_offset_;
701 }
702 
703 /** Get pan/tilt value with time.
704  * @param pan upon return contains the current pan value
705  * @param tilt upon return contains the current tilt value
706  * @param time upon return contains the time the pan and tilt values were read
707  */
708 void
709 PanTiltRX28Thread::WorkerThread::get_pantilt(float &pan, float &tilt, fawkes::Time &time)
710 {
711  get_pantilt(pan, tilt);
712  time = pantilt_time_;
713 }
714 
715 /** Check if motion is final.
716  * @return true if motion is final, false otherwise
717  */
718 bool
719 PanTiltRX28Thread::WorkerThread::is_final()
720 {
721  float pan, tilt;
722  get_pantilt(pan, tilt);
723 
724  /*
725  logger_->log_debug(name(), "P: %f T: %f TP: %f TT: %f PM: %f TM: %f PMov: %i TMov: %i Final: %s",
726  pan, tilt, target_pan_, target_tilt_, pan_margin_, tilt_margin_,
727  rx28_->is_moving(pan_servo_id_), rx28_->is_moving(tilt_servo_id_),
728  (( (fabs(pan - target_pan_) <= pan_margin_) &&
729  (fabs(tilt - target_tilt_) <= tilt_margin_) ) ||
730  (! rx28_->is_moving(pan_servo_id_) &&
731  ! rx28_->is_moving(tilt_servo_id_))) ? "YES" : "NO");
732  */
733 
734  ScopedRWLock lock(rx28_rwlock_, ScopedRWLock::LOCK_READ);
735 
736  return ((fabs(pan - target_pan_) <= pan_margin_) && (fabs(tilt - target_tilt_) <= tilt_margin_))
737  || (!rx28_->is_moving(pan_servo_id_) && !rx28_->is_moving(tilt_servo_id_));
738 }
739 
740 /** Check if PTU is enabled.
741  * @return true if torque is enabled for both servos, false otherwise
742  */
743 bool
744 PanTiltRX28Thread::WorkerThread::is_enabled()
745 {
746  return (rx28_->is_torque_enabled(pan_servo_id_) && rx28_->is_torque_enabled(tilt_servo_id_));
747 }
748 
749 /** Check is fresh sensor data is available.
750  * Note that this method will return true at once per sensor update cycle.
751  * @return true if fresh data is available, false otherwise
752  */
753 bool
754 PanTiltRX28Thread::WorkerThread::has_fresh_data()
755 {
756  MutexLocker lock(fresh_data_mutex_);
757 
758  bool rv = fresh_data_;
759  fresh_data_ = false;
760  return rv;
761 }
762 
763 void
764 PanTiltRX28Thread::WorkerThread::loop()
765 {
766  if (enable_) {
767  value_rwlock_->lock_for_write();
768  enable_ = false;
769  value_rwlock_->unlock();
770  ScopedRWLock lock(rx28_rwlock_);
771  rx28_->set_led_enabled(tilt_servo_id_, true);
772  rx28_->set_torques_enabled(true, 2, pan_servo_id_, tilt_servo_id_);
773  } else if (disable_) {
774  value_rwlock_->lock_for_write();
775  disable_ = false;
776  value_rwlock_->unlock();
777  ScopedRWLock lock(rx28_rwlock_);
778  if (led_enable_ || led_disable_ || velo_pending_ || move_pending_)
779  usleep(3000);
780  }
781 
782  if (led_enable_) {
783  value_rwlock_->lock_for_write();
784  led_enable_ = false;
785  value_rwlock_->unlock();
786  ScopedRWLock lock(rx28_rwlock_);
787  rx28_->set_led_enabled(pan_servo_id_, true);
788  if (velo_pending_ || move_pending_)
789  usleep(3000);
790  } else if (led_disable_) {
791  value_rwlock_->lock_for_write();
792  led_disable_ = false;
793  value_rwlock_->unlock();
794  ScopedRWLock lock(rx28_rwlock_);
795  rx28_->set_led_enabled(pan_servo_id_, false);
796  if (velo_pending_ || move_pending_)
797  usleep(3000);
798  }
799 
800  if (velo_pending_) {
801  value_rwlock_->lock_for_write();
802  velo_pending_ = false;
803  unsigned int pan_vel = pan_vel_;
804  unsigned int tilt_vel = tilt_vel_;
805  value_rwlock_->unlock();
806  ScopedRWLock lock(rx28_rwlock_);
807  rx28_->set_goal_speeds(2, pan_servo_id_, pan_vel, tilt_servo_id_, tilt_vel);
808  if (move_pending_)
809  usleep(3000);
810  }
811 
812  if (move_pending_) {
813  value_rwlock_->lock_for_write();
814  move_pending_ = false;
815  float target_pan = target_pan_;
816  float target_tilt = target_tilt_;
817  value_rwlock_->unlock();
818  exec_goto_pantilt(target_pan, target_tilt);
819  }
820 
821  try {
822  ScopedRWLock lock(rx28_rwlock_, ScopedRWLock::LOCK_READ);
823  rx28_->read_table_values(pan_servo_id_);
824  rx28_->read_table_values(tilt_servo_id_);
825  {
826  MutexLocker lock_fresh_data(fresh_data_mutex_);
827  fresh_data_ = true;
828  pantilt_time_.stamp();
829  }
830  } catch (Exception &e) {
831  // usually just a timeout, too noisy
832  //logger_->log_warn(name(), "Error while reading table values from servos, exception follows");
833  //logger_->log_warn(name(), e);
834  }
835 
836  //if (! is_final() ||
837  // ! rx28_->is_torque_enabled(pan_servo_id_) ||
838  // ! rx28_->is_torque_enabled(tilt_servo_id_)) {
839  // while moving, and while the motor is off, wake us up to get new servo
840  // position data
841  //wakeup();
842  //}
843 
844  update_waitcond_->wake_all();
845 
846  // Wakeup ourselves for faster updates
847  wakeup();
848 }
849 
850 /** Execute pan/tilt motion.
851  * @param pan_rad pan in rad to move to
852  * @param tilt_rad tilt in rad to move to
853  */
854 void
855 PanTiltRX28Thread::WorkerThread::exec_goto_pantilt(float pan_rad, float tilt_rad)
856 {
857  if ((pan_rad < pan_min_) || (pan_rad > pan_max_)) {
858  logger_->log_warn(
859  name(), "Pan value out of bounds, min: %f max: %f des: %f", pan_min_, pan_max_, pan_rad);
860  return;
861  }
862  if ((tilt_rad < tilt_min_) || (tilt_rad > tilt_max_)) {
863  logger_->log_warn(name(),
864  "Tilt value out of bounds, min: %f max: %f des: %f",
865  tilt_min_,
866  tilt_max_,
867  tilt_rad);
868  return;
869  }
870 
871  unsigned int pan_min = 0, pan_max = 0, tilt_min = 0, tilt_max = 0;
872 
873  rx28_->get_angle_limits(pan_servo_id_, pan_min, pan_max);
874  rx28_->get_angle_limits(tilt_servo_id_, tilt_min, tilt_max);
875 
876  int pan_pos = (int)roundf(RobotisRX28::POS_TICKS_PER_RAD * (pan_rad - pan_offset_))
878  int tilt_pos = (int)roundf(RobotisRX28::POS_TICKS_PER_RAD * (tilt_rad - tilt_offset_))
880 
881  if ((pan_pos < 0) || ((unsigned int)pan_pos < pan_min) || ((unsigned int)pan_pos > pan_max)) {
882  logger_->log_warn(
883  name(), "Pan position out of bounds, min: %u max: %u des: %i", pan_min, pan_max, pan_pos);
884  return;
885  }
886 
887  if ((tilt_pos < 0) || ((unsigned int)tilt_pos < tilt_min)
888  || ((unsigned int)tilt_pos > tilt_max)) {
889  logger_->log_warn(name(),
890  "Tilt position out of bounds, min: %u max: %u des: %i",
891  tilt_min,
892  tilt_max,
893  tilt_pos);
894  return;
895  }
896 
897  ScopedRWLock lock(rx28_rwlock_);
898  rx28_->goto_positions(2, pan_servo_id_, pan_pos, tilt_servo_id_, tilt_pos);
899 }
900 
901 /** Wait for fresh data to be received.
902  * Blocks the calling thread.
903  */
904 void
905 PanTiltRX28Thread::WorkerThread::wait_for_fresh_data()
906 {
907  update_waitcond_->wait();
908 }
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.
std::list< unsigned char > DeviceList
List of servo IDs.
Definition: rx28.h:47
TimedGotoMessage Fawkes BlackBoard Interface Message.
Wait until a given condition holds.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:41
unsigned int id() const
Get message ID.
Definition: message.cpp:190
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1026
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:962
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:1225
Mutex locking helper.
Definition: mutex_locker.h:33
void set_max_pan_velocity(const float new_max_pan_velocity)
Set max_pan_velocity value.
SetEnabledMessage Fawkes BlackBoard Interface Message.
Scoped read/write lock.
Definition: scoped_rwlock.h:33
A class for handling time.
Definition: time.h:92
virtual const char * what() const
Get primary string.
Definition: exception.cpp:639
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
Thread class encapsulation of pthreads.
Definition: thread.h:45
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:78
PanTiltRX28Thread(std::string &pantilt_cfg_prefix, std::string &ptu_cfg_prefix, std::string &ptu_name)
Constructor.
Definition: rx28_thread.cpp:53
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:41
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:38
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:185
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1179
void set_intensity(const float new_intensity)
Set intensity value.
ParkMessage Fawkes BlackBoard Interface Message.
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:748
DeviceList discover(unsigned int total_timeout_ms=50)
Discover devices on the bus.
Definition: rx28.cpp:466
Base class for exceptions in Fawkes.
Definition: exception.h:35
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1164
virtual void init()
Initialize the thread.
Definition: rx28_thread.cpp:72
static const float POS_TICKS_PER_RAD
POS_TICKS_PER_RAD.
Definition: rx28.h:153
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:156
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:36
SetIntensityMessage Fawkes BlackBoard Interface Message.
Definition: LedInterface.h:55
const char * name() const
Get name of thread.
Definition: thread.h:100
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:1238
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:729
void set_max_tilt(const float new_max_tilt)
Set max_tilt value.
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:195
Class to access a chain of Robotis RX28 servos.
Definition: rx28.h:43
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:1152
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:149
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.
static const float RAD_PER_POS_TICK
RAD_PER_POS_TICK.
Definition: rx28.h:152
void set_velocity(const float new_velocity)
Set velocity value.
static const unsigned char SRL_RESPOND_READ
SRL_RESPOND_READ.
Definition: rx28.h:144
JointInterface Fawkes BlackBoard Interface.
static const unsigned char BROADCAST_ID
BROADCAST_ID.
Definition: rx28.h:147
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:36
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:41
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.
float time_sec() const
Get time_sec value.
void set_torques_enabled(bool enabled, unsigned int num_servos,...)
Enable or disable torque for multiple (selected) servos at once.
Definition: rx28.cpp:1196
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
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
virtual void close(Interface *interface)=0
Close interface.
Interface for logging.
Definition: logger.h:41