Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * rx28_thread.cpp - RX28 pan/tilt unit act thread 00004 * 00005 * Created: Thu Jun 18 09:53:49 2009 00006 * Copyright 2006-2011 Tim Niemueller [www.niemueller.de] 00007 ****************************************************************************/ 00008 00009 /* This program is free software; you can redistribute it and/or modify 00010 * it under the terms of the GNU General Public License as published by 00011 * the Free Software Foundation; either version 2 of the License, or 00012 * (at your option) any later version. 00013 * 00014 * This program is distributed in the hope that it will be useful, 00015 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00017 * GNU Library General Public License for more details. 00018 * 00019 * Read the full text in the LICENSE.GPL file in the doc directory. 00020 */ 00021 00022 #include "rx28_thread.h" 00023 #include "rx28.h" 00024 00025 #include <utils/math/angle.h> 00026 #include <core/threading/mutex_locker.h> 00027 #include <core/threading/read_write_lock.h> 00028 #include <core/threading/scoped_rwlock.h> 00029 #include <core/threading/wait_condition.h> 00030 #include <interfaces/PanTiltInterface.h> 00031 #include <interfaces/LedInterface.h> 00032 00033 #include <cstdarg> 00034 #include <cmath> 00035 #include <unistd.h> 00036 00037 using namespace fawkes; 00038 00039 /** @class PanTiltRX28Thread "rx28_thread.h" 00040 * PanTilt act thread for RX28 PTU. 00041 * This thread integrates into the Fawkes main loop at the ACT_EXEC hook and 00042 * interacts with the controller of the RX28 PTU. 00043 * @author Tim Niemueller 00044 */ 00045 00046 /** Constructor. 00047 * @param pantilt_cfg_prefix pantilt plugin configuration prefix 00048 * @param ptu_cfg_prefix configuration prefix specific for the PTU 00049 * @param ptu_name name of the PTU configuration 00050 */ 00051 PanTiltRX28Thread::PanTiltRX28Thread(std::string &pantilt_cfg_prefix, 00052 std::string &ptu_cfg_prefix, 00053 std::string &ptu_name) 00054 : PanTiltActThread("PanTiltRX28Thread"), 00055 #ifdef HAVE_TF 00056 TransformAspect(TransformAspect::ONLY_PUBLISHER, 00057 (std::string("PTU ") + ptu_name).c_str()), 00058 #endif 00059 BlackBoardInterfaceListener("PanTiltRX28Thread(%s)", ptu_name.c_str()) 00060 { 00061 set_name("PanTiltRX28Thread(%s)", ptu_name.c_str()); 00062 00063 __pantilt_cfg_prefix = pantilt_cfg_prefix; 00064 __ptu_cfg_prefix = ptu_cfg_prefix; 00065 __ptu_name = ptu_name; 00066 00067 __rx28 = NULL; 00068 } 00069 00070 00071 void 00072 PanTiltRX28Thread::init() 00073 { 00074 __last_pan = __last_tilt = 0.f; 00075 00076 // Note: due to the use of auto_ptr and RefPtr resources are automatically 00077 // freed on destruction, therefore no special handling is necessary in init() 00078 // itself! 00079 00080 __cfg_device = config->get_string((__ptu_cfg_prefix + "device").c_str()); 00081 __cfg_read_timeout_ms = config->get_uint((__ptu_cfg_prefix + "read_timeout_ms").c_str()); 00082 __cfg_disc_timeout_ms = config->get_uint((__ptu_cfg_prefix + "discover_timeout_ms").c_str()); 00083 __cfg_pan_servo_id = config->get_uint((__ptu_cfg_prefix + "pan_servo_id").c_str()); 00084 __cfg_tilt_servo_id = config->get_uint((__ptu_cfg_prefix + "tilt_servo_id").c_str()); 00085 __cfg_pan_offset = deg2rad(config->get_float((__ptu_cfg_prefix + "pan_offset").c_str())); 00086 __cfg_tilt_offset = deg2rad(config->get_float((__ptu_cfg_prefix + "tilt_offset").c_str())); 00087 __cfg_goto_zero_start = config->get_bool((__ptu_cfg_prefix + "goto_zero_start").c_str()); 00088 __cfg_turn_off = config->get_bool((__ptu_cfg_prefix + "turn_off").c_str()); 00089 __cfg_cw_compl_margin = config->get_uint((__ptu_cfg_prefix + "cw_compl_margin").c_str()); 00090 __cfg_ccw_compl_margin = config->get_uint((__ptu_cfg_prefix + "ccw_compl_margin").c_str()); 00091 __cfg_cw_compl_slope = config->get_uint((__ptu_cfg_prefix + "cw_compl_slope").c_str()); 00092 __cfg_ccw_compl_slope = config->get_uint((__ptu_cfg_prefix + "ccw_compl_slope").c_str()); 00093 __cfg_pan_min = config->get_float((__ptu_cfg_prefix + "pan_min").c_str()); 00094 __cfg_pan_max = config->get_float((__ptu_cfg_prefix + "pan_max").c_str()); 00095 __cfg_tilt_min = config->get_float((__ptu_cfg_prefix + "tilt_min").c_str()); 00096 __cfg_tilt_max = config->get_float((__ptu_cfg_prefix + "tilt_max").c_str()); 00097 __cfg_pan_margin = config->get_float((__ptu_cfg_prefix + "pan_margin").c_str()); 00098 __cfg_tilt_margin = config->get_float((__ptu_cfg_prefix + "tilt_margin").c_str()); 00099 __cfg_pan_start = config->get_float((__ptu_cfg_prefix + "pan_start").c_str()); 00100 __cfg_tilt_start = config->get_float((__ptu_cfg_prefix + "tilt_start").c_str()); 00101 00102 #ifdef HAVE_TF 00103 float pan_trans_x = 00104 config->get_float((__ptu_cfg_prefix + "pan_trans_x").c_str()); 00105 float pan_trans_y = 00106 config->get_float((__ptu_cfg_prefix + "pan_trans_y").c_str()); 00107 float pan_trans_z = 00108 config->get_float((__ptu_cfg_prefix + "pan_trans_z").c_str()); 00109 float tilt_trans_x = 00110 config->get_float((__ptu_cfg_prefix + "tilt_trans_x").c_str()); 00111 float tilt_trans_y = 00112 config->get_float((__ptu_cfg_prefix + "tilt_trans_y").c_str()); 00113 float tilt_trans_z = 00114 config->get_float((__ptu_cfg_prefix + "tilt_trans_z").c_str()); 00115 00116 00117 std::string frame_id_prefix = std::string("/") + __ptu_name; 00118 try { 00119 frame_id_prefix = 00120 config->get_string((__ptu_cfg_prefix + "frame_id_prefix").c_str()); 00121 } catch (Exception &e) {} // ignore, use default 00122 00123 __cfg_base_frame = frame_id_prefix + "/base"; 00124 __cfg_pan_link = frame_id_prefix + "/pan"; 00125 __cfg_tilt_link = frame_id_prefix + "/tilt"; 00126 00127 __translation_pan.setValue(pan_trans_x, pan_trans_y, pan_trans_z); 00128 __translation_tilt.setValue(tilt_trans_x, tilt_trans_y, tilt_trans_z); 00129 #endif 00130 00131 bool pan_servo_found = false, tilt_servo_found = false; 00132 00133 __rx28 = new RobotisRX28(__cfg_device.c_str(), __cfg_read_timeout_ms); 00134 RobotisRX28::DeviceList devl = __rx28->discover(); 00135 for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) { 00136 if (__cfg_pan_servo_id == *i) { 00137 pan_servo_found = true; 00138 } else if (__cfg_tilt_servo_id == *i) { 00139 tilt_servo_found = true; 00140 } else { 00141 logger->log_warn(name(), "Servo %u in PTU servo chain, but neither " 00142 "configured as pan nor as tilt servo", *i); 00143 } 00144 } 00145 00146 // We only want responses to be sent on explicit READ to speed up communication 00147 __rx28->set_status_return_level(RobotisRX28::BROADCAST_ID, RobotisRX28::SRL_RESPOND_READ); 00148 // set compliance values 00149 __rx28->set_compliance_values(RobotisRX28::BROADCAST_ID, 00150 __cfg_cw_compl_margin, __cfg_cw_compl_slope, 00151 __cfg_ccw_compl_margin, __cfg_ccw_compl_slope); 00152 __rx28->set_led_enabled(__cfg_pan_servo_id, false); 00153 00154 00155 if (! (pan_servo_found && tilt_servo_found)) { 00156 throw Exception("Pan and/or tilt servo not found: pan: %i tilt: %i", 00157 pan_servo_found, tilt_servo_found); 00158 } 00159 00160 // If you have more than one interface: catch exception and close them! 00161 std::string bbid = "PanTilt " + __ptu_name; 00162 __pantilt_if = blackboard->open_for_writing<PanTiltInterface>(bbid.c_str()); 00163 __pantilt_if->set_calibrated(true); 00164 __pantilt_if->set_min_pan(__cfg_pan_min); 00165 __pantilt_if->set_max_pan(__cfg_pan_max); 00166 __pantilt_if->set_min_tilt(__cfg_tilt_min); 00167 __pantilt_if->set_max_tilt(__cfg_tilt_max); 00168 __pantilt_if->set_pan_margin(__cfg_pan_margin); 00169 __pantilt_if->set_tilt_margin(__cfg_tilt_margin); 00170 __pantilt_if->set_max_pan_velocity(__rx28->get_max_supported_speed(__cfg_pan_servo_id)); 00171 __pantilt_if->set_max_tilt_velocity(__rx28->get_max_supported_speed(__cfg_tilt_servo_id)); 00172 __pantilt_if->set_pan_velocity(__rx28->get_max_supported_speed(__cfg_pan_servo_id)); 00173 __pantilt_if->set_tilt_velocity(__rx28->get_max_supported_speed(__cfg_tilt_servo_id)); 00174 __pantilt_if->write(); 00175 00176 __led_if = blackboard->open_for_writing<LedInterface>(bbid.c_str()); 00177 00178 __wt = new WorkerThread(__ptu_name, logger, __rx28, 00179 __cfg_pan_servo_id, __cfg_tilt_servo_id, 00180 __cfg_pan_min, __cfg_pan_max, __cfg_tilt_min, __cfg_tilt_max, 00181 __cfg_pan_offset, __cfg_tilt_offset); 00182 __wt->set_margins(__cfg_pan_margin, __cfg_tilt_margin); 00183 __wt->start(); 00184 __wt->set_enabled(true); 00185 if ( __cfg_goto_zero_start ) { 00186 __wt->goto_pantilt_timed(__cfg_pan_start, __cfg_tilt_start, 3.0); 00187 } 00188 00189 bbil_add_message_interface(__pantilt_if); 00190 blackboard->register_listener(this); 00191 00192 #ifdef USE_TIMETRACKER 00193 __tt.reset(new TimeTracker()); 00194 __tt_count = 0; 00195 __ttc_read_sensor = __tt->add_class("Read Sensor"); 00196 #endif 00197 00198 } 00199 00200 00201 bool 00202 PanTiltRX28Thread::prepare_finalize_user() 00203 { 00204 if (__cfg_turn_off) { 00205 logger->log_info(name(), "Moving to park position"); 00206 __wt->goto_pantilt_timed(0, __cfg_tilt_max, 2.0); 00207 // we need to wait twice, because the first wakeup is likely to happen 00208 // before the command is actually send 00209 __wt->wait_for_fresh_data(); 00210 __wt->wait_for_fresh_data(); 00211 00212 while (! __wt->is_final()) { 00213 //__wt->wakeup(); 00214 __wt->wait_for_fresh_data(); 00215 } 00216 } 00217 return true; 00218 } 00219 00220 void 00221 PanTiltRX28Thread::finalize() 00222 { 00223 blackboard->unregister_listener(this); 00224 blackboard->close(__pantilt_if); 00225 blackboard->close(__led_if); 00226 00227 __wt->cancel(); 00228 __wt->join(); 00229 delete __wt; 00230 00231 if (__cfg_turn_off) { 00232 logger->log_info(name(), "Turning off PTU"); 00233 try { 00234 __rx28->set_led_enabled(__cfg_pan_servo_id, false); 00235 __rx28->set_led_enabled(__cfg_tilt_servo_id, false); 00236 __rx28->set_torques_enabled(false, 2, __cfg_pan_servo_id, __cfg_tilt_servo_id); 00237 } catch (Exception &e) { 00238 logger->log_warn(name(), "Failed to turn of PTU: %s", e.what()); 00239 } 00240 } 00241 00242 // Setting to NULL deletes instance (RefPtr) 00243 __rx28 = NULL; 00244 } 00245 00246 00247 /** Update sensor values as necessary. 00248 * To be called only from PanTiltSensorThread. Writes the current pan/tilt 00249 * data into the interface. 00250 */ 00251 void 00252 PanTiltRX28Thread::update_sensor_values() 00253 { 00254 if (__wt->has_fresh_data()) { 00255 float pan = 0, tilt = 0, panvel=0, tiltvel=0; 00256 fawkes::Time time; 00257 __wt->get_pantilt(pan, tilt, time); 00258 __wt->get_velocities(panvel, tiltvel); 00259 00260 // poor man's filter: only update if we get a change of least half a degree 00261 if (fabs(__last_pan - pan) >= 0.009 || fabs(__last_tilt - tilt) >= 0.009) { 00262 __last_pan = pan; 00263 __last_tilt = tilt; 00264 } else { 00265 pan = __last_pan; 00266 tilt = __last_tilt; 00267 } 00268 00269 __pantilt_if->set_pan(pan); 00270 __pantilt_if->set_tilt(tilt); 00271 __pantilt_if->set_pan_velocity(panvel); 00272 __pantilt_if->set_tilt_velocity(tiltvel); 00273 __pantilt_if->set_enabled(__wt->is_enabled()); 00274 __pantilt_if->set_final(__wt->is_final()); 00275 __pantilt_if->write(); 00276 00277 #ifdef HAVE_TF 00278 // Always publish updated transforms 00279 tf::Quaternion pr; pr.setEulerZYX(pan, 0, 0); 00280 tf::Transform ptr(pr, __translation_pan); 00281 tf_publisher->send_transform(ptr, time, __cfg_base_frame, __cfg_pan_link); 00282 00283 tf::Quaternion tr; tr.setEulerZYX(0, tilt, 0); 00284 tf::Transform ttr(tr, __translation_tilt); 00285 tf_publisher->send_transform(ttr, time, __cfg_pan_link, __cfg_tilt_link); 00286 #endif 00287 } 00288 } 00289 00290 00291 void 00292 PanTiltRX28Thread::loop() 00293 { 00294 __pantilt_if->set_final(__wt->is_final()); 00295 00296 while (! __pantilt_if->msgq_empty() ) { 00297 if (__pantilt_if->msgq_first_is<PanTiltInterface::CalibrateMessage>()) { 00298 // ignored 00299 00300 } else if (__pantilt_if->msgq_first_is<PanTiltInterface::GotoMessage>()) { 00301 PanTiltInterface::GotoMessage *msg = __pantilt_if->msgq_first(msg); 00302 00303 __wt->goto_pantilt(msg->pan(), msg->tilt()); 00304 __pantilt_if->set_msgid(msg->id()); 00305 __pantilt_if->set_final(false); 00306 00307 } else if (__pantilt_if->msgq_first_is<PanTiltInterface::TimedGotoMessage>()) { 00308 PanTiltInterface::TimedGotoMessage *msg = __pantilt_if->msgq_first(msg); 00309 00310 __wt->goto_pantilt_timed(msg->pan(), msg->tilt(), msg->time_sec()); 00311 __pantilt_if->set_msgid(msg->id()); 00312 __pantilt_if->set_final(false); 00313 00314 } else if (__pantilt_if->msgq_first_is<PanTiltInterface::ParkMessage>()) { 00315 PanTiltInterface::ParkMessage *msg = __pantilt_if->msgq_first(msg); 00316 00317 __wt->goto_pantilt(0, 0); 00318 __pantilt_if->set_msgid(msg->id()); 00319 __pantilt_if->set_final(false); 00320 00321 } else if (__pantilt_if->msgq_first_is<PanTiltInterface::SetEnabledMessage>()) { 00322 PanTiltInterface::SetEnabledMessage *msg = __pantilt_if->msgq_first(msg); 00323 00324 __wt->set_enabled(msg->is_enabled()); 00325 00326 } else if (__pantilt_if->msgq_first_is<PanTiltInterface::SetVelocityMessage>()) { 00327 PanTiltInterface::SetVelocityMessage *msg = __pantilt_if->msgq_first(msg); 00328 00329 if (msg->pan_velocity() > __pantilt_if->max_pan_velocity()) { 00330 logger->log_warn(name(), "Desired pan velocity %f too high, max is %f", 00331 msg->pan_velocity(), __pantilt_if->max_pan_velocity()); 00332 } else if (msg->tilt_velocity() > __pantilt_if->max_tilt_velocity()) { 00333 logger->log_warn(name(), "Desired tilt velocity %f too high, max is %f", 00334 msg->tilt_velocity(), __pantilt_if->max_tilt_velocity()); 00335 } else { 00336 __wt->set_velocities(msg->pan_velocity(), msg->tilt_velocity()); 00337 } 00338 00339 } else if (__pantilt_if->msgq_first_is<PanTiltInterface::SetMarginMessage>()) { 00340 PanTiltInterface::SetMarginMessage *msg = __pantilt_if->msgq_first(msg); 00341 00342 __wt->set_margins(msg->pan_margin(), msg->tilt_margin()); 00343 __pantilt_if->set_pan_margin(msg->pan_margin()); 00344 __pantilt_if->set_tilt_margin(msg->tilt_margin()); 00345 00346 } else { 00347 logger->log_warn(name(), "Unknown message received"); 00348 } 00349 00350 __pantilt_if->msgq_pop(); 00351 } 00352 00353 __pantilt_if->write(); 00354 00355 bool write_led_if = false; 00356 while (! __led_if->msgq_empty() ) { 00357 write_led_if = true; 00358 if (__led_if->msgq_first_is<LedInterface::SetIntensityMessage>()) { 00359 LedInterface::SetIntensityMessage *msg = __led_if->msgq_first(msg); 00360 __wt->set_led_enabled((msg->intensity() >= 0.5)); 00361 __led_if->set_intensity((msg->intensity() >= 0.5) ? LedInterface::ON : LedInterface::OFF); 00362 } else if (__led_if->msgq_first_is<LedInterface::TurnOnMessage>()) { 00363 __wt->set_led_enabled(true); 00364 __led_if->set_intensity(LedInterface::ON); 00365 } else if (__led_if->msgq_first_is<LedInterface::TurnOffMessage>()) { 00366 __wt->set_led_enabled(false); 00367 __led_if->set_intensity(LedInterface::OFF); 00368 } 00369 00370 __led_if->msgq_pop(); 00371 } 00372 if (write_led_if) __led_if->write(); 00373 00374 //__wt->wakeup(); 00375 } 00376 00377 00378 bool 00379 PanTiltRX28Thread::bb_interface_message_received(Interface *interface, 00380 Message *message) throw() 00381 { 00382 if (message->is_of_type<PanTiltInterface::StopMessage>()) { 00383 __wt->stop_motion(); 00384 return false; // do not enqueue StopMessage 00385 } else if (message->is_of_type<PanTiltInterface::FlushMessage>()) { 00386 __wt->stop_motion(); 00387 logger->log_info(name(), "Flushing message queue"); 00388 __pantilt_if->msgq_flush(); 00389 return false; 00390 } else { 00391 logger->log_info(name(), "Received message of type %s, enqueueing", message->type()); 00392 return true; 00393 } 00394 } 00395 00396 00397 /** @class PanTiltRX28Thread::WorkerThread "robotis/rx28_thread.h" 00398 * Worker thread for the PanTiltRX28Thread. 00399 * This continuous thread issues commands to the RX28 chain. In each loop it 00400 * will first execute pending operations, and then update the sensor data (lengthy 00401 * operation). Sensor data will only be updated while either a servo in the chain 00402 * is still moving or torque is disabled (so the motor can be move manually). 00403 * @author Tim Niemueller 00404 */ 00405 00406 00407 /** Constructor. 00408 * @param ptu_name name of the pan/tilt unit 00409 * @param logger logger 00410 * @param rx28 RX28 chain 00411 * @param pan_servo_id servo ID of the pan servo 00412 * @param tilt_servo_id servo ID of the tilt servo 00413 * @param pan_min minimum pan in rad 00414 * @param pan_max maximum pan in rad 00415 * @param tilt_min minimum tilt in rad 00416 * @param tilt_max maximum tilt in rad 00417 * @param pan_offset pan offset from zero in servo ticks 00418 * @param tilt_offset tilt offset from zero in servo ticks 00419 */ 00420 PanTiltRX28Thread::WorkerThread::WorkerThread(std::string ptu_name, 00421 fawkes::Logger *logger, 00422 fawkes::RefPtr<RobotisRX28> rx28, 00423 unsigned char pan_servo_id, 00424 unsigned char tilt_servo_id, 00425 float &pan_min, float &pan_max, 00426 float &tilt_min, float &tilt_max, 00427 float &pan_offset, float &tilt_offset) 00428 : Thread("", Thread::OPMODE_WAITFORWAKEUP) 00429 { 00430 set_name("RX28WorkerThread(%s)", ptu_name.c_str()); 00431 set_coalesce_wakeups(true); 00432 00433 __logger = logger; 00434 00435 __value_rwlock = new ReadWriteLock(); 00436 __rx28_rwlock = new ReadWriteLock(); 00437 __fresh_data_mutex = new Mutex(); 00438 __update_waitcond = new WaitCondition(); 00439 00440 __rx28 = rx28; 00441 __move_pending = false; 00442 __target_pan = 0; 00443 __target_tilt = 0; 00444 __pan_servo_id = pan_servo_id; 00445 __tilt_servo_id = tilt_servo_id; 00446 00447 __pan_min = pan_min; 00448 __pan_max = pan_max; 00449 __tilt_min = tilt_min; 00450 __tilt_max = tilt_max; 00451 __pan_offset = pan_offset; 00452 __tilt_offset = tilt_offset; 00453 __enable = false; 00454 __disable = false; 00455 __led_enable = false; 00456 __led_disable = false; 00457 00458 __max_pan_speed = __rx28->get_max_supported_speed(__pan_servo_id); 00459 __max_tilt_speed = __rx28->get_max_supported_speed(__tilt_servo_id); 00460 } 00461 00462 00463 /** Destructor. */ 00464 PanTiltRX28Thread::WorkerThread::~WorkerThread() 00465 { 00466 delete __value_rwlock; 00467 delete __rx28_rwlock; 00468 delete __fresh_data_mutex; 00469 delete __update_waitcond; 00470 } 00471 00472 00473 /** Enable or disable servo. 00474 * @param enabled true to enable servos, false to turn them off 00475 */ 00476 void 00477 PanTiltRX28Thread::WorkerThread::set_enabled(bool enabled) 00478 { 00479 ScopedRWLock lock(__value_rwlock); 00480 if (enabled) { 00481 __enable = true; 00482 __disable = false; 00483 } else { 00484 __enable = false; 00485 __disable = true; 00486 } 00487 wakeup(); 00488 } 00489 00490 00491 /** Enable or disable LED. 00492 * @param enabled true to enable LED, false to turn it off 00493 */ 00494 void 00495 PanTiltRX28Thread::WorkerThread::set_led_enabled(bool enabled) 00496 { 00497 ScopedRWLock lock(__value_rwlock); 00498 if (enabled) { 00499 __led_enable = true; 00500 __led_disable = false; 00501 } else { 00502 __led_enable = false; 00503 __led_disable = true; 00504 } 00505 wakeup(); 00506 } 00507 00508 00509 /** Stop currently running motion. */ 00510 void 00511 PanTiltRX28Thread::WorkerThread::stop_motion() 00512 { 00513 float pan = 0, tilt = 0; 00514 get_pantilt(pan, tilt); 00515 goto_pantilt(pan, tilt); 00516 } 00517 00518 00519 /** Goto desired pan/tilt values. 00520 * @param pan pan in radians 00521 * @param tilt tilt in radians 00522 */ 00523 void 00524 PanTiltRX28Thread::WorkerThread::goto_pantilt(float pan, float tilt) 00525 { 00526 ScopedRWLock lock(__value_rwlock); 00527 __target_pan = pan; 00528 __target_tilt = tilt; 00529 __move_pending = true; 00530 wakeup(); 00531 } 00532 00533 00534 /** Goto desired pan/tilt values in a specified time. 00535 * @param pan pan in radians 00536 * @param tilt tilt in radians 00537 * @param time_sec time when to reach the desired pan/tilt values 00538 */ 00539 void 00540 PanTiltRX28Thread::WorkerThread::goto_pantilt_timed(float pan, float tilt, float time_sec) 00541 { 00542 __target_pan = pan; 00543 __target_tilt = tilt; 00544 __move_pending = true; 00545 00546 float cpan=0, ctilt=0; 00547 get_pantilt(cpan, ctilt); 00548 00549 float pan_diff = fabs(pan - cpan); 00550 float tilt_diff = fabs(tilt - ctilt); 00551 00552 float req_pan_vel = pan_diff / time_sec; 00553 float req_tilt_vel = tilt_diff / time_sec; 00554 00555 //__logger->log_debug(name(), "Current: %f/%f Des: %f/%f Time: %f Diff: %f/%f ReqVel: %f/%f", 00556 // cpan, ctilt, pan, tilt, time_sec, pan_diff, tilt_diff, req_pan_vel, req_tilt_vel); 00557 00558 00559 if (req_pan_vel > __max_pan_speed) { 00560 __logger->log_warn(name(), "Requested move to (%f, %f) in %f sec requires a " 00561 "pan speed of %f rad/s, which is greater than the maximum " 00562 "of %f rad/s, reducing to max", pan, tilt, time_sec, 00563 req_pan_vel, __max_pan_speed); 00564 req_pan_vel = __max_pan_speed; 00565 } 00566 00567 if (req_tilt_vel > __max_tilt_speed) { 00568 __logger->log_warn(name(), "Requested move to (%f, %f) in %f sec requires a " 00569 "tilt speed of %f rad/s, which is greater than the maximum of " 00570 "%f rad/s, reducing to max", pan, tilt, time_sec, 00571 req_tilt_vel, __max_tilt_speed); 00572 req_tilt_vel = __max_tilt_speed; 00573 } 00574 00575 set_velocities(req_pan_vel, req_tilt_vel); 00576 00577 wakeup(); 00578 } 00579 00580 00581 /** Set desired velocities. 00582 * @param pan_vel pan velocity 00583 * @param tilt_vel tilt velocity 00584 */ 00585 void 00586 PanTiltRX28Thread::WorkerThread::set_velocities(float pan_vel, float tilt_vel) 00587 { 00588 ScopedRWLock lock(__value_rwlock); 00589 float pan_tmp = roundf((pan_vel / __max_pan_speed) * RobotisRX28::MAX_SPEED); 00590 float tilt_tmp = roundf((tilt_vel / __max_tilt_speed) * RobotisRX28::MAX_SPEED); 00591 00592 //__logger->log_debug(name(), "old speed: %u/%u new speed: %f/%f", __pan_vel, 00593 // __tilt_vel, pan_tmp, tilt_tmp); 00594 00595 if ((pan_tmp >= 0) && (pan_tmp <= RobotisRX28::MAX_SPEED)) { 00596 __pan_vel = (unsigned int)pan_tmp; 00597 __velo_pending = true; 00598 } else { 00599 __logger->log_warn(name(), "Calculated pan value out of bounds, min: 0 max: %u des: %u", 00600 RobotisRX28::MAX_SPEED, (unsigned int)pan_tmp); 00601 } 00602 00603 if ((tilt_tmp >= 0) && (tilt_tmp <= RobotisRX28::MAX_SPEED)) { 00604 __tilt_vel = (unsigned int)tilt_tmp; 00605 __velo_pending = true; 00606 } else { 00607 __logger->log_warn(name(), "Calculated tilt value out of bounds, min: 0 max: %u des: %u", 00608 RobotisRX28::MAX_SPEED, (unsigned int)tilt_tmp); 00609 } 00610 } 00611 00612 00613 /** Get current velocities. 00614 * @param pan_vel upon return contains current pan velocity 00615 * @param tilt_vel upon return contains current tilt velocity 00616 */ 00617 void 00618 PanTiltRX28Thread::WorkerThread::get_velocities(float &pan_vel, float &tilt_vel) 00619 { 00620 unsigned int pan_velticks = __rx28->get_goal_speed(__pan_servo_id); 00621 unsigned int tilt_velticks = __rx28->get_goal_speed(__tilt_servo_id); 00622 00623 pan_velticks = (unsigned int)(((float)pan_velticks / (float)RobotisRX28::MAX_SPEED) * __max_pan_speed); 00624 tilt_velticks = (unsigned int)(((float)tilt_velticks / (float)RobotisRX28::MAX_SPEED) * __max_tilt_speed); 00625 } 00626 00627 00628 /** Set desired velocities. 00629 * @param pan_margin pan margin 00630 * @param tilt_margin tilt margin 00631 */ 00632 void 00633 PanTiltRX28Thread::WorkerThread::set_margins(float pan_margin, float tilt_margin) 00634 { 00635 if (pan_margin > 0.0) __pan_margin = pan_margin; 00636 if (tilt_margin > 0.0) __tilt_margin = tilt_margin; 00637 //__logger->log_warn(name(), "Margins set to %f, %f", __pan_margin, __tilt_margin); 00638 } 00639 00640 00641 /** Get pan/tilt value. 00642 * @param pan upon return contains the current pan value 00643 * @param tilt upon return contains the current tilt value 00644 */ 00645 void 00646 PanTiltRX28Thread::WorkerThread::get_pantilt(float &pan, float &tilt) 00647 { 00648 ScopedRWLock lock(__rx28_rwlock, ScopedRWLock::LOCK_READ); 00649 00650 int pan_ticks = ((int)__rx28->get_position(__pan_servo_id) - (int)RobotisRX28::CENTER_POSITION); 00651 int tilt_ticks = ((int)__rx28->get_position(__tilt_servo_id) - (int)RobotisRX28::CENTER_POSITION); 00652 00653 pan = pan_ticks * RobotisRX28::RAD_PER_POS_TICK + __pan_offset; 00654 tilt = tilt_ticks * RobotisRX28::RAD_PER_POS_TICK + __tilt_offset; 00655 } 00656 00657 00658 /** Get pan/tilt value with time. 00659 * @param pan upon return contains the current pan value 00660 * @param tilt upon return contains the current tilt value 00661 * @param time upon return contains the time the pan and tilt values were read 00662 */ 00663 void 00664 PanTiltRX28Thread::WorkerThread::get_pantilt(float &pan, float &tilt, 00665 fawkes::Time &time) 00666 { 00667 get_pantilt(pan, tilt); 00668 time = __pantilt_time; 00669 } 00670 00671 00672 /** Check if motion is final. 00673 * @return true if motion is final, false otherwise 00674 */ 00675 bool 00676 PanTiltRX28Thread::WorkerThread::is_final() 00677 { 00678 float pan, tilt; 00679 get_pantilt(pan, tilt); 00680 00681 /* 00682 __logger->log_debug(name(), "P: %f T: %f TP: %f TT: %f PM: %f TM: %f PMov: %i TMov: %i Final: %s", 00683 pan, tilt, __target_pan, __target_tilt, __pan_margin, __tilt_margin, 00684 __rx28->is_moving(__pan_servo_id), __rx28->is_moving(__tilt_servo_id), 00685 (( (fabs(pan - __target_pan) <= __pan_margin) && 00686 (fabs(tilt - __target_tilt) <= __tilt_margin) ) || 00687 (! __rx28->is_moving(__pan_servo_id) && 00688 ! __rx28->is_moving(__tilt_servo_id))) ? "YES" : "NO"); 00689 */ 00690 00691 ScopedRWLock lock(__rx28_rwlock, ScopedRWLock::LOCK_READ); 00692 00693 return ( (fabs(pan - __target_pan) <= __pan_margin) && 00694 (fabs(tilt - __target_tilt) <= __tilt_margin) ) || 00695 (! __rx28->is_moving(__pan_servo_id) && 00696 ! __rx28->is_moving(__tilt_servo_id)); 00697 } 00698 00699 00700 /** Check if PTU is enabled. 00701 * @return true if torque is enabled for both servos, false otherwise 00702 */ 00703 bool 00704 PanTiltRX28Thread::WorkerThread::is_enabled() 00705 { 00706 return (__rx28->is_torque_enabled(__pan_servo_id) && 00707 __rx28->is_torque_enabled(__tilt_servo_id)); 00708 } 00709 00710 00711 /** Check is fresh sensor data is available. 00712 * Note that this method will return true at once per sensor update cycle. 00713 * @return true if fresh data is available, false otherwise 00714 */ 00715 bool 00716 PanTiltRX28Thread::WorkerThread::has_fresh_data() 00717 { 00718 MutexLocker lock(__fresh_data_mutex); 00719 00720 bool rv = __fresh_data; 00721 __fresh_data = false; 00722 return rv; 00723 } 00724 00725 00726 void 00727 PanTiltRX28Thread::WorkerThread::loop() 00728 { 00729 if (__enable) { 00730 __value_rwlock->lock_for_write(); 00731 __enable = false; 00732 __value_rwlock->unlock(); 00733 ScopedRWLock lock(__rx28_rwlock); 00734 __rx28->set_led_enabled(__tilt_servo_id, true); 00735 __rx28->set_torques_enabled(true, 2, __pan_servo_id, __tilt_servo_id); 00736 } else if (__disable) { 00737 __value_rwlock->lock_for_write(); 00738 __disable = false; 00739 __value_rwlock->unlock(); 00740 ScopedRWLock lock(__rx28_rwlock); 00741 if (__led_enable || __led_disable || __velo_pending || __move_pending) usleep(3000); 00742 } 00743 00744 if (__led_enable) { 00745 __value_rwlock->lock_for_write(); 00746 __led_enable = false; 00747 __value_rwlock->unlock(); 00748 ScopedRWLock lock(__rx28_rwlock); 00749 __rx28->set_led_enabled(__pan_servo_id, true); 00750 if (__velo_pending || __move_pending) usleep(3000); 00751 } else if (__led_disable) { 00752 __value_rwlock->lock_for_write(); 00753 __led_disable = false; 00754 __value_rwlock->unlock(); 00755 ScopedRWLock lock(__rx28_rwlock); 00756 __rx28->set_led_enabled(__pan_servo_id, false); 00757 if (__velo_pending || __move_pending) usleep(3000); 00758 } 00759 00760 if (__velo_pending) { 00761 __value_rwlock->lock_for_write(); 00762 __velo_pending = false; 00763 unsigned int pan_vel = __pan_vel; 00764 unsigned int tilt_vel = __tilt_vel; 00765 __value_rwlock->unlock(); 00766 ScopedRWLock lock(__rx28_rwlock); 00767 __rx28->set_goal_speeds(2, __pan_servo_id, pan_vel, __tilt_servo_id, tilt_vel); 00768 if (__move_pending) usleep(3000); 00769 } 00770 00771 if (__move_pending) { 00772 __value_rwlock->lock_for_write(); 00773 __move_pending = false; 00774 float target_pan = __target_pan; 00775 float target_tilt = __target_tilt; 00776 __value_rwlock->unlock(); 00777 exec_goto_pantilt(target_pan, target_tilt); 00778 } 00779 00780 try { 00781 ScopedRWLock lock(__rx28_rwlock, ScopedRWLock::LOCK_READ); 00782 __rx28->read_table_values(__pan_servo_id); 00783 __rx28->read_table_values(__tilt_servo_id); 00784 { 00785 MutexLocker lock_fresh_data(__fresh_data_mutex); 00786 __fresh_data = true; 00787 __pantilt_time.stamp(); 00788 } 00789 } catch (Exception &e) { 00790 // usually just a timeout, too noisy 00791 //__logger->log_warn(name(), "Error while reading table values from servos, exception follows"); 00792 //__logger->log_warn(name(), e); 00793 } 00794 00795 //if (! is_final() || 00796 // ! __rx28->is_torque_enabled(__pan_servo_id) || 00797 // ! __rx28->is_torque_enabled(__tilt_servo_id)) { 00798 // while moving, and while the motor is off, wake us up to get new servo 00799 // position data 00800 //wakeup(); 00801 //} 00802 00803 __update_waitcond->wake_all(); 00804 00805 // Wakeup ourselves for faster updates 00806 wakeup(); 00807 } 00808 00809 00810 /** Execute pan/tilt motion. 00811 * @param pan_rad pan in rad to move to 00812 * @param tilt_rad tilt in rad to move to 00813 */ 00814 void 00815 PanTiltRX28Thread::WorkerThread::exec_goto_pantilt(float pan_rad, float tilt_rad) 00816 { 00817 if ( (pan_rad < __pan_min) || (pan_rad > __pan_max) ) { 00818 __logger->log_warn(name(), "Pan value out of bounds, min: %f max: %f des: %f", 00819 __pan_min, __pan_max, pan_rad); 00820 return; 00821 } 00822 if ( (tilt_rad < __tilt_min) || (tilt_rad > __tilt_max) ) { 00823 __logger->log_warn(name(), "Tilt value out of bounds, min: %f max: %f des: %f", 00824 __tilt_min, __tilt_max, tilt_rad); 00825 return; 00826 } 00827 00828 unsigned int pan_min = 0, pan_max = 0, tilt_min = 0, tilt_max = 0; 00829 00830 __rx28->get_angle_limits(__pan_servo_id, pan_min, pan_max); 00831 __rx28->get_angle_limits(__tilt_servo_id, tilt_min, tilt_max); 00832 00833 00834 int pan_pos = (int)roundf(RobotisRX28::POS_TICKS_PER_RAD * (pan_rad - __pan_offset)) 00835 + RobotisRX28::CENTER_POSITION; 00836 int tilt_pos = (int)roundf(RobotisRX28::POS_TICKS_PER_RAD * (tilt_rad - __tilt_offset)) 00837 + RobotisRX28::CENTER_POSITION; 00838 00839 if ( (pan_pos < 0) || ((unsigned int)pan_pos < pan_min) || ((unsigned int)pan_pos > pan_max) ) { 00840 __logger->log_warn(name(), "Pan position out of bounds, min: %u max: %u des: %i", 00841 pan_min, pan_max, pan_pos); 00842 return; 00843 } 00844 00845 if ( (tilt_pos < 0) || ((unsigned int)tilt_pos < tilt_min) || ((unsigned int)tilt_pos > tilt_max) ) { 00846 __logger->log_warn(name(), "Tilt position out of bounds, min: %u max: %u des: %i", 00847 tilt_min, tilt_max, tilt_pos); 00848 return; 00849 } 00850 00851 ScopedRWLock lock(__rx28_rwlock); 00852 __rx28->goto_positions(2, __pan_servo_id, pan_pos, __tilt_servo_id, tilt_pos); 00853 } 00854 00855 00856 /** Wait for fresh data to be received. 00857 * Blocks the calling thread. 00858 */ 00859 void 00860 PanTiltRX28Thread::WorkerThread::wait_for_fresh_data() 00861 { 00862 __update_waitcond->wait(); 00863 }