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
39using 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 */
53PanTiltRX28Thread::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
71void
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 {
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
224bool
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
243void
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 */
275void
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
326void
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()) {
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()) {
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;
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
417bool
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 */
456PanTiltRX28Thread::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());
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. */
502PanTiltRX28Thread::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 */
513void
514PanTiltRX28Thread::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 */
530void
531PanTiltRX28Thread::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. */
545void
546PanTiltRX28Thread::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 */
557void
558PanTiltRX28Thread::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 */
572void
573PanTiltRX28Thread::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 */
626void
627PanTiltRX28Thread::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 */
661void
662PanTiltRX28Thread::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 */
677void
678PanTiltRX28Thread::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 */
691void
692PanTiltRX28Thread::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 */
708void
709PanTiltRX28Thread::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 */
718bool
719PanTiltRX28Thread::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 */
743bool
744PanTiltRX28Thread::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 */
753bool
754PanTiltRX28Thread::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
763void
764PanTiltRX28Thread::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 */
854void
855PanTiltRX28Thread::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 */
904void
905PanTiltRX28Thread::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