Fawkes API  Fawkes Development Version
evid100p_thread.cpp
1 
2 /***************************************************************************
3  * evid100p_thread.h - Sony EviD100P pan/tilt unit act thread
4  *
5  * Created: Sun Jun 21 12:38:34 2009
6  * Copyright 2006-2009 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "evid100p_thread.h"
24 #include "evid100p.h"
25 
26 #include <core/threading/mutex_locker.h>
27 #include <interfaces/PanTiltInterface.h>
28 
29 #include <cstdarg>
30 #include <cmath>
31 
32 using namespace fawkes;
33 
34 /** @class PanTiltSonyEviD100PThread "evid100p_thread.h"
35  * PanTilt act thread for the PTU part of the Sony EviD100P camera.
36  * This thread integrates into the Fawkes main loop at the ACT_EXEC hook and
37  * interacts via the Visca protocol with the controller of the Sony EviD100P.
38  * @author Tim Niemueller
39  */
40 
41 /** Constructor.
42  * @param pantilt_cfg_prefix pantilt plugin configuration prefix
43  * @param ptu_cfg_prefix configuration prefix specific for the PTU
44  * @param ptu_name name of the PTU configuration
45  */
47  std::string &ptu_cfg_prefix,
48  std::string &ptu_name)
49  : PanTiltActThread("PanTiltSonyEviD100PThread"),
50  BlackBoardInterfaceListener("PanTiltSonyEviD100PThread")
51 {
52  set_name("PanTiltSonyEviD100PThread(%s)", ptu_name.c_str());
53 
54  __pantilt_cfg_prefix = pantilt_cfg_prefix;
55  __ptu_cfg_prefix = ptu_cfg_prefix;
56  __ptu_name = ptu_name;
57 
58  __cam = NULL;
59 }
60 
61 
62 void
64 {
65  // Note: due to the use of auto_ptr and RefPtr resources are automatically
66  // freed on destruction, therefore no special handling is necessary in init()
67  // itself!
68 
69  __cfg_device = config->get_string((__ptu_cfg_prefix + "device").c_str());
70  __cfg_read_timeout_ms = config->get_uint((__ptu_cfg_prefix + "read_timeout_ms").c_str());
71 
72  try {
73  __cam = new SonyEviD100PVisca(__cfg_device.c_str(), __cfg_read_timeout_ms);
74  } catch (Exception &e) {
75  e.print_trace();
76  e.print_backtrace();
77  throw;
78  }
79 
80  // If you have more than one interface: catch exception and close them!
81  std::string bbid = "PanTilt " + __ptu_name;
82  __pantilt_if = blackboard->open_for_writing<PanTiltInterface>(bbid.c_str());
83  __pantilt_if->set_calibrated(true);
88  __pantilt_if->set_enabled(true); // Cannot be turned off
89 
90  float pan_smin, pan_smax, tilt_smin, tilt_smax;
91  __cam->get_speed_limits(pan_smin, pan_smax, tilt_smin, tilt_smax);
92  __pantilt_if->set_max_pan_velocity(pan_smax);
93  __pantilt_if->set_max_tilt_velocity(tilt_smax);
94  __pantilt_if->set_pan_velocity(pan_smax);
95  __pantilt_if->set_tilt_velocity(tilt_smax);
96  __pantilt_if->write();
97 
98  __wt = new WorkerThread(__ptu_name, logger, __cam,
101  __wt->start();
102 
103  __wt->set_velocities(pan_smax, tilt_smax);
104 
105  bbil_add_message_interface(__pantilt_if);
107 
108 #ifdef USE_TIMETRACKER
109  __tt.reset(new TimeTracker());
110  __tt_count = 0;
111  __ttc_read_sensor = __tt->add_class("Read Sensor");
112 #endif
113 
114 }
115 
116 
117 void
119 {
121  blackboard->close(__pantilt_if);
122 
123  __wt->cancel();
124  __wt->join();
125  delete __wt;
126 
127  // Setting to NULL deletes instance (RefPtr)
128  __cam = NULL;
129 }
130 
131 
132 /** Update sensor values as necessary.
133  * To be called only from PanTiltSensorThread. Writes the current pan/tilt
134  * data into the interface.
135  */
136 void
138 {
139  if (__wt->has_fresh_data()) {
140  float pan = 0, tilt = 0;
141  __wt->get_pantilt(pan, tilt);
142  __pantilt_if->set_pan(pan);
143  __pantilt_if->set_tilt(tilt);
144  __pantilt_if->set_final(__wt->is_final());
145  __pantilt_if->write();
146  }
147 }
148 
149 
150 void
152 {
153  __pantilt_if->set_final(__wt->is_final());
154 
155  while (! __pantilt_if->msgq_empty() ) {
156  if (__pantilt_if->msgq_first_is<PanTiltInterface::CalibrateMessage>()) {
157  // ignored
158 
159  } else if (__pantilt_if->msgq_first_is<PanTiltInterface::GotoMessage>()) {
160  PanTiltInterface::GotoMessage *msg = __pantilt_if->msgq_first(msg);
161 
162  __wt->goto_pantilt(msg->pan(), msg->tilt());
163  __pantilt_if->set_msgid(msg->id());
164  __pantilt_if->set_final(false);
165 
166  } else if (__pantilt_if->msgq_first_is<PanTiltInterface::ParkMessage>()) {
167  PanTiltInterface::ParkMessage *msg = __pantilt_if->msgq_first(msg);
168 
169  __wt->goto_pantilt(0, 0);
170  __pantilt_if->set_msgid(msg->id());
171  __pantilt_if->set_final(false);
172 
173  } else if (__pantilt_if->msgq_first_is<PanTiltInterface::SetEnabledMessage>()) {
174  PanTiltInterface::SetEnabledMessage *msg = __pantilt_if->msgq_first(msg);
175 
176  logger->log_warn(name(), "SetEnabledMessage ignored for Sony EviD100P");
177 
178  } else if (__pantilt_if->msgq_first_is<PanTiltInterface::SetVelocityMessage>()) {
179  PanTiltInterface::SetVelocityMessage *msg = __pantilt_if->msgq_first(msg);
180 
181  logger->log_warn(name(), "SetVelocityMessage ignored for Sony EviD100P");
182 
183  if ((msg->pan_velocity() < 0) || (msg->tilt_velocity() < 0) ) {
184  logger->log_warn(name(), "Ignoring pan/tilt velocities %f/%f, at least one "
185  " is negative", msg->pan_velocity(), msg->tilt_velocity());
186  } else if (msg->pan_velocity() > __pantilt_if->max_pan_velocity()) {
187  logger->log_warn(name(), "Desired pan velocity %f too high, max is %f",
188  msg->pan_velocity(), __pantilt_if->max_pan_velocity());
189  } else if (msg->tilt_velocity() > __pantilt_if->max_tilt_velocity()) {
190  logger->log_warn(name(), "Desired tilt velocity %f too high, max is %f",
191  msg->tilt_velocity(), __pantilt_if->max_tilt_velocity());
192  } else {
193  __wt->set_velocities(msg->pan_velocity(), msg->tilt_velocity());
194  __pantilt_if->set_pan_velocity(msg->pan_velocity());
195  __pantilt_if->set_tilt_velocity(msg->tilt_velocity());
196  }
197 
198  } else {
199  logger->log_warn(name(), "Unknown message received");
200  }
201 
202  __pantilt_if->msgq_pop();
203  }
204 
205  __pantilt_if->write();
206 
207 }
208 
209 
210 bool
212  Message *message) throw()
213 {
214  if (message->is_of_type<PanTiltInterface::StopMessage>()) {
215  __wt->stop_motion();
216  return false; // do not enqueue StopMessage
217  } else if (message->is_of_type<PanTiltInterface::FlushMessage>()) {
218  __wt->stop_motion();
219  logger->log_info(name(), "Flushing message queue");
220  __pantilt_if->msgq_flush();
221  return false;
222  } else {
223  logger->log_info(name(), "Received message of type %s, enqueueing", message->type());
224  return true;
225  }
226 }
227 
228 
229 /** @class PanTiltSonyEviD100PThread::WorkerThread "sony/evid100p_thread.h"
230  * Worker thread for the PanTiltSonyEviD100PThread.
231  * This continuous thread issues commands to the camera. In each loop it
232  * will first execute pending operations, and then update the sensor data (lengthy
233  * operation). Sensor data will only be updated while either a servo in the chain
234  * is still moving or torque is disabled (so the motor can be move manually).
235  * @author Tim Niemueller
236  */
237 
238 
239 /** Constructor.
240  * @param ptu_name name of the pan/tilt unit
241  * @param logger logger
242  * @param cam Visca controller object
243  * @param pan_min minimum pan in rad
244  * @param pan_max maximum pan in rad
245  * @param tilt_min minimum tilt in rad
246  * @param tilt_max maximum tilt in rad
247  */
248 PanTiltSonyEviD100PThread::WorkerThread::WorkerThread(std::string ptu_name,
249  fawkes::Logger *logger,
251  const float &pan_min,
252  const float &pan_max,
253  const float &tilt_min,
254  const float &tilt_max)
255  : Thread("", Thread::OPMODE_WAITFORWAKEUP)
256 {
257  set_name("SonyEviD100PWorkerThread(%s)", ptu_name.c_str());
258  set_coalesce_wakeups(true);
259 
260  __logger = logger;
261 
262  __move_mutex = new Mutex();
263 
264  __cam = cam;
265  __move_pending = false;
266  __target_pan = 0;
267  __target_tilt = 0;
268 
269  __velo_pending = false;
270  __pan_vel = 0;
271  __tilt_vel = 0;
272 
273  __pan_min = pan_min;
274  __pan_max = pan_max;
275  __tilt_min = tilt_min;
276  __tilt_max = tilt_max;
277 }
278 
279 
280 /** Destructor. */
281 PanTiltSonyEviD100PThread::WorkerThread::~WorkerThread()
282 {
283  delete __move_mutex;
284 }
285 
286 
287 /** Stop currently running motion. */
288 void
289 PanTiltSonyEviD100PThread::WorkerThread::stop_motion()
290 {
291  float pan = 0, tilt = 0;
292  get_pantilt(pan, tilt);
293  goto_pantilt(pan, tilt);
294 }
295 
296 
297 /** Goto desired pan/tilt values.
298  * @param pan pan in radians
299  * @param tilt tilt in radians
300  */
301 void
302 PanTiltSonyEviD100PThread::WorkerThread::goto_pantilt(float pan, float tilt)
303 {
304  MutexLocker lock(__move_mutex);
305  __target_pan = pan;
306  __target_tilt = tilt;
307  __move_pending = true;
308  wakeup();
309 }
310 
311 
312 /** Get pan/tilt value.
313  * @param pan upon return contains the current pan value
314  * @param tilt upon return contains the current tilt value
315  */
316 void
317 PanTiltSonyEviD100PThread::WorkerThread::get_pantilt(float &pan, float &tilt)
318 {
319  pan = __cur_pan;
320  tilt = __cur_tilt;
321 }
322 
323 
324 /** Set desired velocities.
325  * @param pan_vel pan velocity
326  * @param tilt_vel tilt velocity
327  */
328 void
329 PanTiltSonyEviD100PThread::WorkerThread::set_velocities(float pan_vel, float tilt_vel)
330 {
331  __pan_vel = pan_vel;
332  __tilt_vel = tilt_vel;
333  __velo_pending = true;
334 }
335 
336 
337 /** Check if motion is final.
338  * @return true if motion is final, false otherwise
339  */
340 bool
341 PanTiltSonyEviD100PThread::WorkerThread::is_final()
342 {
343  MutexLocker lock(__move_mutex);
344  return __cam->is_nonblocking_finished(SonyEviD100PVisca::NONBLOCKING_PANTILT);
345 }
346 
347 
348 /** Check is fresh sensor data is available.
349  * Note that this method will return true at once per sensor update cycle.
350  * @return true if fresh data is available, false otherwise
351  */
352 bool
353 PanTiltSonyEviD100PThread::WorkerThread::has_fresh_data()
354 {
355  bool rv = __fresh_data;
356  __fresh_data = false;
357  return rv;
358 }
359 
360 
361 void
362 PanTiltSonyEviD100PThread::WorkerThread::once()
363 {
364  // do some process cycles to process data returning back from set_address()
365  // and clear calls
366  for (int i = 0; i < 20; ++i) {
367  try {
368  __cam->process();
369  } catch (Exception &e) { /* ignored */ }
370  }
371 }
372 
373 
374 void
375 PanTiltSonyEviD100PThread::WorkerThread::loop()
376 {
377  try {
378  __cam->process();
379  } catch (Exception &e) {
380  __logger->log_warn(name(), "Data processing failed, exception follows");
381  __logger->log_warn(name(), e);
382  }
383 
384  if (__velo_pending) {
385  try {
386  __cam->set_speed_radsec(__pan_vel, __tilt_vel);
387  } catch (Exception &e) {
388  __logger->log_warn(name(), "Setting pan/tilt values failed, exception follows");
389  __logger->log_warn(name(), e);
390  }
391  __velo_pending = false;
392  }
393 
394  if (__move_pending) {
395  __move_mutex->lock();
396  __logger->log_debug(name(), "Executing goto to %f, %f", __target_pan, __target_tilt);
397  exec_goto_pantilt(__target_pan, __target_tilt);
398  __move_mutex->unlock();
399  }
400 
401  //__cam->start_get_pan_tilt();
402  try {
403  __cam->get_pan_tilt_rad(__cur_pan, __cur_tilt);
404  __fresh_data = true;
405  } catch (Exception &e) {
406  __logger->log_warn(name(), "Failed to get new pan/tilt data, exception follows");
407  __logger->log_warn(name(), e);
408  }
409 
410  if (! is_final() || ! __fresh_data) {
411  // while moving or if data reception failed wake us up to get new servo data
412  wakeup();
413  }
414 }
415 
416 
417 /** Execute pan/tilt motion.
418  * @param pan_rad pan in rad to move to
419  * @param tilt_rad tilt in rad to move to
420  */
421 void
422 PanTiltSonyEviD100PThread::WorkerThread::exec_goto_pantilt(float pan_rad, float tilt_rad)
423 {
424  if ( (pan_rad < __pan_min) || (pan_rad > __pan_max) ) {
425  __logger->log_warn(name(), "Pan value out of bounds, min: %f max: %f des: %f",
426  __pan_min, __pan_max, pan_rad);
427  return;
428  }
429  if ( (tilt_rad < __tilt_min) || (tilt_rad > __tilt_max) ) {
430  __logger->log_warn(name(), "Tilt value out of bounds, min: %f max: %f des: %f",
431  __tilt_min, __tilt_max, tilt_rad);
432  return;
433  }
434 
435  try {
436  __cam->set_pan_tilt_rad(pan_rad, tilt_rad);
437  } catch (Exception &e) {
438  __logger->log_warn(name(), "Failed to execute pan/tilt to %f, %f, exception "
439  "follows", pan_rad, tilt_rad);
440  __logger->log_warn(name(), e);
441  }
442  __move_pending = false;
443 }
static const float MIN_TILT_RAD
Min tilt in rad.
Definition: evid100p.h:61
float pan() const
Get pan value.
static const unsigned int NONBLOCKING_PANTILT
Non-blocking pan/tilt item.
Definition: visca.h:61
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:43
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:972
virtual void finalize()
Finalize the thread.
static const float MIN_PAN_RAD
Min pan in rad.
Definition: evid100p.h:59
virtual void loop()
Code to execute in the thread.
float tilt_velocity() const
Get tilt_velocity value.
static const float MAX_PAN_RAD
Max pan in rad.
Definition: evid100p.h:58
void set_pan_velocity(const float new_pan_velocity)
Set pan_velocity value.
Fawkes library namespace.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
Mutex locking helper.
Definition: mutex_locker.h:33
void update_sensor_values()
Update sensor values as necessary.
void set_max_pan_velocity(const float new_max_pan_velocity)
Set max_pan_velocity value.
SetEnabledMessage Fawkes BlackBoard Interface Message.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:200
Thread class encapsulation of pthreads.
Definition: thread.h:42
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:495
unsigned int id() const
Get message ID.
Definition: message.cpp:197
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
float pan_velocity() const
Get pan_velocity value.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
SetVelocityMessage Fawkes BlackBoard Interface Message.
void set_min_pan(const float new_min_pan)
Set min_pan value.
void get_speed_limits(float &pan_min, float &pan_max, float &tilt_min, float &tilt_max)
Get speed limits.
Definition: evid100p.cpp:243
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:174
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1118
ParkMessage Fawkes BlackBoard Interface Message.
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:749
Base class for exceptions in Fawkes.
Definition: exception.h:36
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1104
void print_backtrace() const
Prints a backtrace.
Definition: exception.cpp:563
FlushMessage Fawkes BlackBoard Interface Message.
void set_min_tilt(const float new_min_tilt)
Set min_tilt value.
void set_tilt(const float new_tilt)
Set tilt value.
Time tracking utility.
Definition: tracker.h:38
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:305
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
void set_final(const bool new_final)
Set final value.
void set_calibrated(const bool new_calibrated)
Set calibrated value.
Pan/tilt act thread.
Definition: act_thread.h:36
GotoMessage Fawkes BlackBoard Interface Message.
void set_coalesce_wakeups(bool coalesce=true)
Set wakeup coalescing.
Definition: thread.cpp:729
void set_max_tilt(const float new_max_tilt)
Set max_tilt value.
const char * name() const
Get name of thread.
Definition: thread.h:95
float max_pan_velocity() const
Get max_pan_velocity value.
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:203
void print_trace()
Prints trace to stderr.
Definition: exception.cpp:619
float max_tilt_velocity() const
Get max_tilt_velocity value.
void set_max_tilt_velocity(const float new_max_tilt_velocity)
Set max_tilt_velocity value.
float tilt() const
Get tilt value.
void set_pan(const float new_pan)
Set pan value.
CalibrateMessage Fawkes BlackBoard Interface Message.
static const float MAX_TILT_RAD
Max tilt in rad.
Definition: evid100p.h:60
Sony EviD100P Visca controller.
Definition: evid100p.h:32
PanTiltInterface Fawkes BlackBoard Interface.
void set_max_pan(const float new_max_pan)
Set max_pan value.
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message)
BlackBoard message received notification.
PanTiltSonyEviD100PThread(std::string &pantilt_cfg_prefix, std::string &ptu_cfg_prefix, std::string &ptu_name)
Constructor.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
void set_enabled(const bool new_enabled)
Set enabled value.
Mutex mutual exclusion lock.
Definition: mutex.h:32
void set_tilt_velocity(const float new_tilt_velocity)
Set tilt_velocity value.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard interface listener.
StopMessage Fawkes BlackBoard Interface Message.
virtual void init()
Initialize the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:43
virtual void close(Interface *interface)=0
Close interface.
Interface for logging.
Definition: logger.h:34