Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
dp_thread.cpp
1 
2 /***************************************************************************
3  * dp_thread.h - DirectedPerception pan/tilt unit act thread
4  *
5  * Created: Sun Jun 21 17:31:50 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 "dp_thread.h"
24 #include "dp_ptu.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 PanTiltDirectedPerceptionThread "dp_thread.h"
35  * PanTilt act thread for PTUs from DirectedPerception employing the ASCII
36  * protocol.
37  * This thread integrates into the Fawkes main loop at the ACT_EXEC hook and
38  * interacts via the Visca protocol with the controller of the Sony EviD100P.
39  * @author Tim Niemueller
40  */
41 
42 /** Constructor.
43  * @param pantilt_cfg_prefix pantilt plugin configuration prefix
44  * @param ptu_cfg_prefix configuration prefix specific for the PTU
45  * @param ptu_name name of the PTU configuration
46  */
48  std::string &ptu_cfg_prefix,
49  std::string &ptu_name)
50  : PanTiltActThread("PanTiltDirectedPerceptionThread"),
51  BlackBoardInterfaceListener("PanTiltDirectedPerceptionThread")
52 {
53  set_name("PanTiltDirectedPerceptionThread(%s)", ptu_name.c_str());
54 
55  __pantilt_cfg_prefix = pantilt_cfg_prefix;
56  __ptu_cfg_prefix = ptu_cfg_prefix;
57  __ptu_name = ptu_name;
58 }
59 
60 
61 void
63 {
64  // Note: due to the use of auto_ptr and RefPtr resources are automatically
65  // freed on destruction, therefore no special handling is necessary in init()
66  // itself!
67 
68  __cfg_device = config->get_string((__ptu_cfg_prefix + "device").c_str());
69  __cfg_read_timeout_ms = config->get_uint((__ptu_cfg_prefix + "read_timeout_ms").c_str());
70 
71  __ptu = new DirectedPerceptionPTU(__cfg_device.c_str(), __cfg_read_timeout_ms);
72 
73  // If you have more than one interface: catch exception and close them!
74  std::string bbid = "PanTilt " + __ptu_name;
75  __pantilt_if = blackboard->open_for_writing<PanTiltInterface>(bbid.c_str());
76 
77  float min_pan=0, max_pan=0, min_tilt=0, max_tilt=0;
78  __ptu->get_limits(min_pan, max_pan, min_tilt, max_tilt);
79 
80  __pantilt_if->set_calibrated(true);
81  __pantilt_if->set_min_pan(min_pan);
82  __pantilt_if->set_max_pan(max_pan);
83  __pantilt_if->set_min_tilt(min_tilt);
84  __pantilt_if->set_max_tilt(max_tilt);
85  __pantilt_if->set_enabled(true); // Cannot be turned off
86  //__pantilt_if->set_max_pan_velocity(0);
87  //__pantilt_if->set_max_tilt_velocity(0);
88  //__pantilt_if->set_pan_velocity(0);
89  //__pantilt_if->set_tilt_velocity(0);
90  __pantilt_if->write();
91 
92  __wt = new WorkerThread(__ptu_name, logger, __ptu);
93  __wt->start();
94 
95  bbil_add_message_interface(__pantilt_if);
97 
98 #ifdef USE_TIMETRACKER
99  __tt.reset(new TimeTracker());
100  __tt_count = 0;
101  __ttc_read_sensor = __tt->add_class("Read Sensor");
102 #endif
103 
104 }
105 
106 
107 void
109 {
111  blackboard->close(__pantilt_if);
112 
113  __wt->cancel();
114  __wt->join();
115  delete __wt;
116 
117  // Setting to NULL deletes instance (RefPtr)
118  __ptu = NULL;
119 }
120 
121 
122 /** Update sensor values as necessary.
123  * To be called only from PanTiltSensorThread. Writes the current pan/tilt
124  * data into the interface.
125  */
126 void
128 {
129  if (__wt->has_fresh_data()) {
130  float pan = 0, tilt = 0;
131  __wt->get_pantilt(pan, tilt);
132  __pantilt_if->set_pan(pan);
133  __pantilt_if->set_tilt(tilt);
134  __pantilt_if->set_final(__wt->is_final());
135  __pantilt_if->write();
136  }
137 }
138 
139 
140 void
142 {
143  __pantilt_if->set_final(__wt->is_final());
144 
145  while (! __pantilt_if->msgq_empty() ) {
146  if (__pantilt_if->msgq_first_is<PanTiltInterface::CalibrateMessage>()) {
147  __wt->reset();
148 
149  } else if (__pantilt_if->msgq_first_is<PanTiltInterface::GotoMessage>()) {
150  PanTiltInterface::GotoMessage *msg = __pantilt_if->msgq_first(msg);
151 
152  __wt->goto_pantilt(msg->pan(), msg->tilt());
153  __pantilt_if->set_msgid(msg->id());
154  __pantilt_if->set_final(false);
155 
156  } else if (__pantilt_if->msgq_first_is<PanTiltInterface::ParkMessage>()) {
157  PanTiltInterface::ParkMessage *msg = __pantilt_if->msgq_first(msg);
158 
159  __wt->goto_pantilt(0, 0);
160  __pantilt_if->set_msgid(msg->id());
161  __pantilt_if->set_final(false);
162 
163  } else if (__pantilt_if->msgq_first_is<PanTiltInterface::SetEnabledMessage>()) {
164  PanTiltInterface::SetEnabledMessage *msg = __pantilt_if->msgq_first(msg);
165 
166  logger->log_warn(name(), "SetEnabledMessage ignored for Sony EviD100P");
167 
168  } else if (__pantilt_if->msgq_first_is<PanTiltInterface::SetVelocityMessage>()) {
169  PanTiltInterface::SetVelocityMessage *msg = __pantilt_if->msgq_first(msg);
170 
171  logger->log_warn(name(), "SetVelocityMessage ignored for Sony EviD100P");
172 
173  /* ignored for now
174  if (msg->pan_velocity() > __pantilt_if->max_pan_velocity()) {
175  logger->log_warn(name(), "Desired pan velocity %f too high, max is %f",
176  msg->pan_velocity(), __pantilt_if->max_pan_velocity());
177  } else if (msg->tilt_velocity() > __pantilt_if->max_tilt_velocity()) {
178  logger->log_warn(name(), "Desired tilt velocity %f too high, max is %f",
179  msg->tilt_velocity(), __pantilt_if->max_tilt_velocity());
180  } else {
181  __wt->set_velocities(msg->pan_velocity(), msg->tilt_velocity());
182  __pantilt_if->set_pan_velocity(msg->pan_velocity());
183  __pantilt_if->set_tilt_velocity(msg->tilt_velocity());
184  }
185  */
186 
187  } else {
188  logger->log_warn(name(), "Unknown message received");
189  }
190 
191  __pantilt_if->msgq_pop();
192  }
193 
194  __pantilt_if->write();
195 
196 }
197 
198 
199 bool
201  Message *message) throw()
202 {
203  if (message->is_of_type<PanTiltInterface::StopMessage>()) {
204  __wt->stop_motion();
205  return false; // do not enqueue StopMessage
206  } else if (message->is_of_type<PanTiltInterface::FlushMessage>()) {
207  __wt->stop_motion();
208  logger->log_info(name(), "Flushing message queue");
209  __pantilt_if->msgq_flush();
210  return false;
211  } else {
212  logger->log_info(name(), "Received message of type %s, enqueueing", message->type());
213  return true;
214  }
215 }
216 
217 
218 /** @class PanTiltDirectedPerceptionThread::WorkerThread "sony/evid100p_thread.h"
219  * Worker thread for the PanTiltDirectedPerceptionThread.
220  * This continuous thread issues commands to the camera. In each loop it
221  * will first execute pending operations, and then update the sensor data (lengthy
222  * operation). Sensor data will only be updated while either a servo in the chain
223  * is still moving or torque is disabled (so the motor can be move manually).
224  * @author Tim Niemueller
225  */
226 
227 
228 /** Constructor.
229  * @param ptu_name name of the pan/tilt unit
230  * @param logger logger
231  * @param ptu ptu controller
232  */
233 PanTiltDirectedPerceptionThread::WorkerThread::WorkerThread(std::string ptu_name,
234  fawkes::Logger *logger,
236  : Thread("", Thread::OPMODE_WAITFORWAKEUP)
237 {
238  set_name("SonyDirectedPerceptionWorkerThread(%s)", ptu_name.c_str());
239  set_coalesce_wakeups(true);
240 
241  __logger = logger;
242 
243  __move_mutex = new Mutex();
244 
245  __ptu = ptu;
246  __move_pending = false;
247  __reset_pending = false;
248  __target_pan = 0;
249  __target_tilt = 0;
250 
251  __ptu->get_limits(__pan_min, __pan_max, __tilt_min, __tilt_max);
252 }
253 
254 
255 /** Destructor. */
256 PanTiltDirectedPerceptionThread::WorkerThread::~WorkerThread()
257 {
258  delete __move_mutex;
259 }
260 
261 
262 /** Stop currently running motion. */
263 void
264 PanTiltDirectedPerceptionThread::WorkerThread::stop_motion()
265 {
266  float pan = 0, tilt = 0;
267  get_pantilt(pan, tilt);
268  goto_pantilt(pan, tilt);
269 }
270 
271 
272 /** Goto desired pan/tilt values.
273  * @param pan pan in radians
274  * @param tilt tilt in radians
275  */
276 void
277 PanTiltDirectedPerceptionThread::WorkerThread::goto_pantilt(float pan, float tilt)
278 {
279  MutexLocker lock(__move_mutex);
280  __target_pan = pan;
281  __target_tilt = tilt;
282  __move_pending = true;
283  wakeup();
284 }
285 
286 
287 /** Get pan/tilt value.
288  * @param pan upon return contains the current pan value
289  * @param tilt upon return contains the current tilt value
290  */
291 void
292 PanTiltDirectedPerceptionThread::WorkerThread::get_pantilt(float &pan, float &tilt)
293 {
294  pan = __cur_pan;
295  tilt = __cur_tilt;
296 }
297 
298 
299 /** Trigger a reset of the PTU. */
300 void
301 PanTiltDirectedPerceptionThread::WorkerThread::reset()
302 {
303  __reset_pending = true;
304 }
305 
306 
307 /** Check if motion is final.
308  * @return true if motion is final, false otherwise
309  */
310 bool
311 PanTiltDirectedPerceptionThread::WorkerThread::is_final()
312 {
313  MutexLocker lock(__move_mutex);
314  return ( (fabs(__cur_pan - __target_pan) < 0.01) &&
315  (fabs(__cur_tilt - __target_tilt) < 0.01));
316 }
317 
318 
319 /** Check is fresh sensor data is available.
320  * Note that this method will return true at once per sensor update cycle.
321  * @return true if fresh data is available, false otherwise
322  */
323 bool
324 PanTiltDirectedPerceptionThread::WorkerThread::has_fresh_data()
325 {
326  bool rv = __fresh_data;
327  __fresh_data = false;
328  return rv;
329 }
330 
331 
332 void
333 PanTiltDirectedPerceptionThread::WorkerThread::loop()
334 {
335  if (__move_pending) {
336  __move_mutex->lock();
337  exec_goto_pantilt(__target_pan, __target_tilt);
338  __move_mutex->unlock();
339  }
340 
341  if (__reset_pending) {
342  __move_mutex->lock();
343  __reset_pending = false;
344  __move_mutex->unlock();
345  __ptu->reset();
346  }
347 
348  try {
349  __ptu->get_pan_tilt_rad(__cur_pan, __cur_tilt);
350  __fresh_data = true;
351  } catch (Exception &e) {
352  __logger->log_warn(name(), "Failed to get new pan/tilt data, exception follows");
353  __logger->log_warn(name(), e);
354  }
355 
356  if (! is_final()) {
357  // while moving wake us up to get new servo position data
358  wakeup();
359  }
360 }
361 
362 
363 /** Execute pan/tilt motion.
364  * @param pan_rad pan in rad to move to
365  * @param tilt_rad tilt in rad to move to
366  */
367 void
368 PanTiltDirectedPerceptionThread::WorkerThread::exec_goto_pantilt(float pan_rad, float tilt_rad)
369 {
370  if ( (pan_rad < __pan_min) || (pan_rad > __pan_max) ) {
371  __logger->log_warn(name(), "Pan value out of bounds, min: %f max: %f des: %f",
372  __pan_min, __pan_max, pan_rad);
373  return;
374  }
375  if ( (tilt_rad < __tilt_min) || (tilt_rad > __tilt_max) ) {
376  __logger->log_warn(name(), "Tilt value out of bounds, min: %f max: %f des: %f",
377  __tilt_min, __tilt_max, tilt_rad);
378  return;
379  }
380 
381  __ptu->set_pan_tilt_rad(pan_rad, tilt_rad);
382  __move_pending = false;
383 }
float pan() const
Get pan value.
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 bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message)
BlackBoard message received notification.
Definition: dp_thread.cpp:200
void set_msgid(const uint32_t new_msgid)
Set msgid value.
Mutex locking helper.
Definition: mutex_locker.h:33
PanTiltDirectedPerceptionThread(std::string &pantilt_cfg_prefix, std::string &ptu_cfg_prefix, std::string &ptu_name)
Constructor.
Definition: dp_thread.cpp:47
SetEnabledMessage Fawkes BlackBoard Interface Message.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:200
void update_sensor_values()
Update sensor values as necessary.
Definition: dp_thread.cpp:127
Thread class encapsulation of pthreads.
Definition: thread.h:42
DirectedPerception PTU implementation.
Definition: dp_ptu.h:30
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
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.
virtual void get_limits(float &pan_min, float &pan_max, float &tilt_min, float &tilt_max)
Get position limits in radians.
Definition: dp_ptu.cpp:345
void set_min_pan(const float new_min_pan)
Set min_pan value.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp: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
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.
virtual void init()
Initialize the thread.
Definition: dp_thread.cpp:62
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual void finalize()
Finalize the thread.
Definition: dp_thread.cpp:108
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:203
float tilt() const
Get tilt value.
void set_pan(const float new_pan)
Set pan value.
CalibrateMessage Fawkes BlackBoard Interface Message.
PanTiltInterface Fawkes BlackBoard Interface.
void set_max_pan(const float new_max_pan)
Set max_pan value.
virtual void loop()
Code to execute in the thread.
Definition: dp_thread.cpp:141
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1104
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
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.
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