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