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 
25 #include "dp_ptu.h"
26 
27 #include <core/threading/mutex_locker.h>
28 #include <interfaces/JointInterface.h>
29 #include <interfaces/PanTiltInterface.h>
30 
31 #include <cmath>
32 #include <cstdarg>
33 
34 using namespace fawkes;
35 
36 /** @class PanTiltDirectedPerceptionThread "dp_thread.h"
37  * PanTilt act thread for PTUs from DirectedPerception employing the ASCII
38  * protocol.
39  * This thread integrates into the Fawkes main loop at the ACT_EXEC hook and
40  * interacts via the Visca protocol with the controller of the Sony EviD100P.
41  * @author Tim Niemueller
42  */
43 
44 /** Constructor.
45  * @param pantilt_cfg_prefix pantilt plugin configuration prefix
46  * @param ptu_cfg_prefix configuration prefix specific for the PTU
47  * @param ptu_name name of the PTU configuration
48  */
50  std::string &ptu_cfg_prefix,
51  std::string &ptu_name)
52 : PanTiltActThread("PanTiltDirectedPerceptionThread"),
53  BlackBoardInterfaceListener("PanTiltDirectedPerceptionThread")
54 {
55  set_name("PanTiltDirectedPerceptionThread(%s)", ptu_name.c_str());
56 
57  pantilt_cfg_prefix_ = pantilt_cfg_prefix;
58  ptu_cfg_prefix_ = ptu_cfg_prefix;
59  ptu_name_ = ptu_name;
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 void
127 {
129  blackboard->close(pantilt_if_);
130  blackboard->close(panjoint_if_);
131  blackboard->close(tiltjoint_if_);
132 
133  wt_->cancel();
134  wt_->join();
135  delete wt_;
136 
137  // Setting to NULL deletes instance (RefPtr)
138  ptu_ = NULL;
139 }
140 
141 /** Update sensor values as necessary.
142  * To be called only from PanTiltSensorThread. Writes the current pan/tilt
143  * data into the interface.
144  */
145 void
147 {
148  if (wt_->has_fresh_data()) {
149  float pan = 0, tilt = 0;
150  wt_->get_pantilt(pan, tilt);
151  pantilt_if_->set_pan(pan);
152  pantilt_if_->set_tilt(tilt);
153  pantilt_if_->set_final(wt_->is_final());
154  pantilt_if_->write();
155 
156  panjoint_if_->set_position(pan);
157  panjoint_if_->write();
158 
159  tiltjoint_if_->set_position(tilt);
160  tiltjoint_if_->write();
161  }
162 }
163 
164 void
166 {
167  pantilt_if_->set_final(wt_->is_final());
168 
169  while (!pantilt_if_->msgq_empty()) {
171  wt_->reset();
172 
173  } else if (pantilt_if_->msgq_first_is<PanTiltInterface::GotoMessage>()) {
174  PanTiltInterface::GotoMessage *msg = pantilt_if_->msgq_first(msg);
175 
176  wt_->goto_pantilt(msg->pan(), msg->tilt());
177  pantilt_if_->set_msgid(msg->id());
178  pantilt_if_->set_final(false);
179 
180  } else if (pantilt_if_->msgq_first_is<PanTiltInterface::ParkMessage>()) {
181  PanTiltInterface::ParkMessage *msg = pantilt_if_->msgq_first(msg);
182 
183  wt_->goto_pantilt(0, 0);
184  pantilt_if_->set_msgid(msg->id());
185  pantilt_if_->set_final(false);
186 
187  } else if (pantilt_if_->msgq_first_is<PanTiltInterface::SetEnabledMessage>()) {
188  PanTiltInterface::SetEnabledMessage *msg = pantilt_if_->msgq_first(msg);
189 
190  logger->log_warn(name(), "SetEnabledMessage ignored for Sony EviD100P");
191 
192  } else if (pantilt_if_->msgq_first_is<PanTiltInterface::SetVelocityMessage>()) {
193  PanTiltInterface::SetVelocityMessage *msg = pantilt_if_->msgq_first(msg);
194 
195  logger->log_warn(name(), "SetVelocityMessage ignored for Sony EviD100P");
196 
197  /* ignored for now
198  if (msg->pan_velocity() > pantilt_if_->max_pan_velocity()) {
199  logger->log_warn(name(), "Desired pan velocity %f too high, max is %f",
200  msg->pan_velocity(), pantilt_if_->max_pan_velocity());
201  } else if (msg->tilt_velocity() > pantilt_if_->max_tilt_velocity()) {
202  logger->log_warn(name(), "Desired tilt velocity %f too high, max is %f",
203  msg->tilt_velocity(), pantilt_if_->max_tilt_velocity());
204  } else {
205  wt_->set_velocities(msg->pan_velocity(), msg->tilt_velocity());
206  pantilt_if_->set_pan_velocity(msg->pan_velocity());
207  pantilt_if_->set_tilt_velocity(msg->tilt_velocity());
208  panjoint_if_->set_velocity(msg->pan_velocity());
209  panjoint_if_->write();
210  tiltjoint_if_->set_velocity(msg->tilt_velocity());
211  tiltjoint_if_->write();
212  }
213  */
214 
215  } else {
216  logger->log_warn(name(), "Unknown message received");
217  }
218 
219  pantilt_if_->msgq_pop();
220  }
221 
222  pantilt_if_->write();
223 }
224 
225 bool
227  Message * message) throw()
228 {
229  if (message->is_of_type<PanTiltInterface::StopMessage>()) {
230  wt_->stop_motion();
231  return false; // do not enqueue StopMessage
232  } else if (message->is_of_type<PanTiltInterface::FlushMessage>()) {
233  wt_->stop_motion();
234  logger->log_info(name(), "Flushing message queue");
235  pantilt_if_->msgq_flush();
236  return false;
237  } else {
238  logger->log_info(name(), "Received message of type %s, enqueueing", message->type());
239  return true;
240  }
241 }
242 
243 /** @class PanTiltDirectedPerceptionThread::WorkerThread "sony/evid100p_thread.h"
244  * Worker thread for the PanTiltDirectedPerceptionThread.
245  * This continuous thread issues commands to the camera. In each loop it
246  * will first execute pending operations, and then update the sensor data (lengthy
247  * operation). Sensor data will only be updated while either a servo in the chain
248  * is still moving or torque is disabled (so the motor can be move manually).
249  * @author Tim Niemueller
250  */
251 
252 /** Constructor.
253  * @param ptu_name name of the pan/tilt unit
254  * @param logger logger
255  * @param ptu ptu controller
256  */
257 PanTiltDirectedPerceptionThread::WorkerThread::WorkerThread(
258  std::string ptu_name,
259  fawkes::Logger * logger,
261 : Thread("", Thread::OPMODE_WAITFORWAKEUP)
262 {
263  set_name("SonyDirectedPerceptionWorkerThread(%s)", ptu_name.c_str());
264  set_coalesce_wakeups(true);
265 
266  logger_ = logger;
267 
268  move_mutex_ = new Mutex();
269 
270  ptu_ = ptu;
271  move_pending_ = false;
272  reset_pending_ = false;
273  target_pan_ = 0;
274  target_tilt_ = 0;
275 
276  ptu_->get_limits(pan_min_, pan_max_, tilt_min_, tilt_max_);
277 }
278 
279 /** Destructor. */
280 PanTiltDirectedPerceptionThread::WorkerThread::~WorkerThread()
281 {
282  delete move_mutex_;
283 }
284 
285 /** Stop currently running motion. */
286 void
287 PanTiltDirectedPerceptionThread::WorkerThread::stop_motion()
288 {
289  float pan = 0, tilt = 0;
290  get_pantilt(pan, tilt);
291  goto_pantilt(pan, tilt);
292 }
293 
294 /** Goto desired pan/tilt values.
295  * @param pan pan in radians
296  * @param tilt tilt in radians
297  */
298 void
299 PanTiltDirectedPerceptionThread::WorkerThread::goto_pantilt(float pan, float tilt)
300 {
301  MutexLocker lock(move_mutex_);
302  target_pan_ = pan;
303  target_tilt_ = tilt;
304  move_pending_ = true;
305  wakeup();
306 }
307 
308 /** Get pan/tilt value.
309  * @param pan upon return contains the current pan value
310  * @param tilt upon return contains the current tilt value
311  */
312 void
313 PanTiltDirectedPerceptionThread::WorkerThread::get_pantilt(float &pan, float &tilt)
314 {
315  pan = cur_pan_;
316  tilt = cur_tilt_;
317 }
318 
319 /** Trigger a reset of the PTU. */
320 void
321 PanTiltDirectedPerceptionThread::WorkerThread::reset()
322 {
323  reset_pending_ = true;
324 }
325 
326 /** Check if motion is final.
327  * @return true if motion is final, false otherwise
328  */
329 bool
330 PanTiltDirectedPerceptionThread::WorkerThread::is_final()
331 {
332  MutexLocker lock(move_mutex_);
333  return ((fabs(cur_pan_ - target_pan_) < 0.01) && (fabs(cur_tilt_ - target_tilt_) < 0.01));
334 }
335 
336 /** Check is fresh sensor data is available.
337  * Note that this method will return true at once per sensor update cycle.
338  * @return true if fresh data is available, false otherwise
339  */
340 bool
341 PanTiltDirectedPerceptionThread::WorkerThread::has_fresh_data()
342 {
343  bool rv = fresh_data_;
344  fresh_data_ = false;
345  return rv;
346 }
347 
348 void
349 PanTiltDirectedPerceptionThread::WorkerThread::loop()
350 {
351  if (move_pending_) {
352  move_mutex_->lock();
353  exec_goto_pantilt(target_pan_, target_tilt_);
354  move_mutex_->unlock();
355  }
356 
357  if (reset_pending_) {
358  move_mutex_->lock();
359  reset_pending_ = false;
360  move_mutex_->unlock();
361  ptu_->reset();
362  }
363 
364  try {
365  ptu_->get_pan_tilt_rad(cur_pan_, cur_tilt_);
366  fresh_data_ = true;
367  } catch (Exception &e) {
368  logger_->log_warn(name(), "Failed to get new pan/tilt data, exception follows");
369  logger_->log_warn(name(), e);
370  }
371 
372  if (!is_final()) {
373  // while moving wake us up to get new servo position data
374  wakeup();
375  }
376 }
377 
378 /** Execute pan/tilt motion.
379  * @param pan_rad pan in rad to move to
380  * @param tilt_rad tilt in rad to move to
381  */
382 void
383 PanTiltDirectedPerceptionThread::WorkerThread::exec_goto_pantilt(float pan_rad, float tilt_rad)
384 {
385  if ((pan_rad < pan_min_) || (pan_rad > pan_max_)) {
386  logger_->log_warn(
387  name(), "Pan value out of bounds, min: %f max: %f des: %f", pan_min_, pan_max_, pan_rad);
388  return;
389  }
390  if ((tilt_rad < tilt_min_) || (tilt_rad > tilt_max_)) {
391  logger_->log_warn(name(),
392  "Tilt value out of bounds, min: %f max: %f des: %f",
393  tilt_min_,
394  tilt_max_,
395  tilt_rad);
396  return;
397  }
398 
399  ptu_->set_pan_tilt_rad(pan_rad, tilt_rad);
400  move_pending_ = false;
401 }
DirectedPerception PTU implementation.
Definition: dp_ptu.h:31
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:333
Pan/tilt act thread.
Definition: act_thread.h:41
virtual void loop()
Code to execute in the thread.
Definition: dp_thread.cpp:165
PanTiltDirectedPerceptionThread(std::string &pantilt_cfg_prefix, std::string &ptu_cfg_prefix, std::string &ptu_name)
Constructor.
Definition: dp_thread.cpp:49
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message)
BlackBoard message received notification.
Definition: dp_thread.cpp:226
virtual void finalize()
Finalize the thread.
Definition: dp_thread.cpp:126
virtual void init()
Initialize the thread.
Definition: dp_thread.cpp:63
void update_sensor_values()
Update sensor values as necessary.
Definition: dp_thread.cpp:146
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 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
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1182
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1167
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1029
JointInterface Fawkes BlackBoard Interface.
void set_position(const float new_position)
Set position value.
void set_velocity(const float new_velocity)
Set velocity value.
Interface for logging.
Definition: logger.h:42
virtual void log_warn(const char *component, const char *format,...)=0
Log warning 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:45
unsigned int id() const
Get message ID.
Definition: message.cpp:180
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.
SetVelocityMessage Fawkes BlackBoard Interface Message.
StopMessage Fawkes BlackBoard Interface Message.
PanTiltInterface Fawkes BlackBoard Interface.
void set_enabled(const bool new_enabled)
Set enabled 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(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_tilt(const float new_tilt)
Set tilt value.
void set_calibrated(const bool new_calibrated)
Set calibrated value.
void set_pan(const float new_pan)
Set pan value.
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
Fawkes library namespace.