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 }
Pan/tilt act thread.
Definition: act_thread.h:41
void update_sensor_values()
Update sensor values as necessary.
PanTiltRX28Thread(std::string &pantilt_cfg_prefix, std::string &ptu_cfg_prefix, std::string &ptu_name)
Constructor.
Definition: rx28_thread.cpp:53
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
Definition: rx28_thread.cpp:72
virtual void finalize()
Finalize the thread.
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message) noexcept
BlackBoard message received notification.
virtual bool prepare_finalize_user()
Prepare finalization user implementation.
Class to access a chain of Robotis RX28 servos.
Definition: rx28.h:44
std::list< unsigned char > DeviceList
List of servo IDs.
Definition: rx28.h:47
static const unsigned int MAX_SPEED
MAX_SPEED.
Definition: rx28.h:156
float get_max_supported_speed(unsigned char id, bool refresh=false)
Get maximum supported speed.
Definition: rx28.cpp:962
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
static const float RAD_PER_POS_TICK
RAD_PER_POS_TICK.
Definition: rx28.h:152
void set_status_return_level(unsigned char id, unsigned char status_return_level)
Set status return level.
Definition: rx28.cpp:1152
static const float POS_TICKS_PER_RAD
POS_TICKS_PER_RAD.
Definition: rx28.h:153
static const unsigned char BROADCAST_ID
BROADCAST_ID.
Definition: rx28.h:147
void set_torques_enabled(bool enabled, unsigned int num_servos,...)
Enable or disable torque for multiple (selected) servos at once.
Definition: rx28.cpp:1196
static const unsigned char SRL_RESPOND_READ
SRL_RESPOND_READ.
Definition: rx28.h:144
void set_led_enabled(unsigned char id, bool enabled)
Turn LED on or off.
Definition: rx28.cpp:1225
static const unsigned int CENTER_POSITION
CENTER_POSITION.
Definition: rx28.h:149
DeviceList discover(unsigned int total_timeout_ms=50)
Discover devices on the bus.
Definition: rx28.cpp:466
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
BlackBoard interface listener.
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
virtual void close(Interface *interface)=0
Close interface.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const noexcept
Get primary string.
Definition: exception.cpp:639
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:351
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1200
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
JointInterface Fawkes BlackBoard Interface.
void set_position(const float new_position)
Set position value.
void set_velocity(const float new_velocity)
Set velocity value.
SetIntensityMessage Fawkes BlackBoard Interface Message.
Definition: LedInterface.h:56
float intensity() const
Get intensity value.
TurnOffMessage Fawkes BlackBoard Interface Message.
Definition: LedInterface.h:107
TurnOnMessage Fawkes BlackBoard Interface Message.
Definition: LedInterface.h:87
LedInterface Fawkes BlackBoard Interface.
Definition: LedInterface.h:34
void set_intensity(const float new_intensity)
Set intensity value.
Interface for logging.
Definition: logger.h:42
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
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:181
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:195
Mutex locking helper.
Definition: mutex_locker.h:34
Mutex mutual exclusion lock.
Definition: mutex.h:33
CalibrateMessage Fawkes BlackBoard Interface Message.
FlushMessage Fawkes BlackBoard Interface Message.
GotoMessage Fawkes BlackBoard Interface Message.
float tilt() const
Get tilt value.
ParkMessage Fawkes BlackBoard Interface Message.
SetEnabledMessage Fawkes BlackBoard Interface Message.
SetMarginMessage Fawkes BlackBoard Interface Message.
float tilt_margin() const
Get tilt_margin value.
float pan_margin() const
Get pan_margin value.
SetVelocityMessage Fawkes BlackBoard Interface Message.
float tilt_velocity() const
Get tilt_velocity value.
float pan_velocity() const
Get pan_velocity value.
StopMessage Fawkes BlackBoard Interface Message.
TimedGotoMessage Fawkes BlackBoard Interface Message.
float time_sec() const
Get time_sec value.
PanTiltInterface Fawkes BlackBoard Interface.
void set_enabled(const bool new_enabled)
Set enabled value.
float max_tilt_velocity() const
Get max_tilt_velocity value.
void set_tilt_velocity(const float new_tilt_velocity)
Set tilt_velocity value.
void set_min_pan(const float new_min_pan)
Set min_pan value.
void set_final(const bool new_final)
Set final value.
void set_min_tilt(const float new_min_tilt)
Set min_tilt value.
void set_max_tilt_velocity(const float new_max_tilt_velocity)
Set max_tilt_velocity value.
void set_max_tilt(const float new_max_tilt)
Set max_tilt value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
void set_max_pan(const float new_max_pan)
Set max_pan value.
void set_pan_velocity(const float new_pan_velocity)
Set pan_velocity value.
void set_pan_margin(const float new_pan_margin)
Set pan_margin value.
void set_tilt(const float new_tilt)
Set tilt value.
void set_max_pan_velocity(const float new_max_pan_velocity)
Set max_pan_velocity value.
void set_calibrated(const bool new_calibrated)
Set calibrated value.
float max_pan_velocity() const
Get max_pan_velocity value.
void set_tilt_margin(const float new_tilt_margin)
Set tilt_margin value.
void set_pan(const float new_pan)
Set pan value.
Read/write lock to allow multiple readers but only a single writer on the resource at a time.
Scoped read/write lock.
Definition: scoped_rwlock.h:34
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:748
void set_coalesce_wakeups(bool coalesce=true)
Set wakeup coalescing.
Definition: thread.cpp:729
Time tracking utility.
Definition: tracker.h:37
A class for handling time.
Definition: time.h:93
Thread aspect to access the transform system.
Definition: tf.h:39
Wait until a given condition holds.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36