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 }
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:41
unsigned int id() const
Get message ID.
Definition: message.cpp:190
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1026
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message)
BlackBoard message received notification.
Definition: dp_thread.cpp:226
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:49
SetEnabledMessage Fawkes BlackBoard Interface Message.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
void update_sensor_values()
Update sensor values as necessary.
Definition: dp_thread.cpp:146
Thread class encapsulation of pthreads.
Definition: thread.h:45
DirectedPerception PTU implementation.
Definition: dp_ptu.h:30
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:78
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
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:333
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:185
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1179
ParkMessage Fawkes BlackBoard Interface Message.
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:748
Base class for exceptions in Fawkes.
Definition: exception.h:35
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1164
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:36
const char * name() const
Get name of thread.
Definition: thread.h:100
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:729
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:126
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:195
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:165
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:41
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.
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:41