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
34using 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
62void
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
125void
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 */
145void
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
164void
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
225bool
227 Message * message) noexcept
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 */
257PanTiltDirectedPerceptionThread::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());
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. */
280PanTiltDirectedPerceptionThread::WorkerThread::~WorkerThread()
281{
282 delete move_mutex_;
283}
284
285/** Stop currently running motion. */
286void
287PanTiltDirectedPerceptionThread::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 */
298void
299PanTiltDirectedPerceptionThread::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 */
312void
313PanTiltDirectedPerceptionThread::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. */
320void
321PanTiltDirectedPerceptionThread::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 */
329bool
330PanTiltDirectedPerceptionThread::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 */
340bool
341PanTiltDirectedPerceptionThread::WorkerThread::has_fresh_data()
342{
343 bool rv = fresh_data_;
344 fresh_data_ = false;
345 return rv;
346}
347
348void
349PanTiltDirectedPerceptionThread::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 */
382void
383PanTiltDirectedPerceptionThread::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 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
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message) noexcept
BlackBoard message received notification.
Definition: dp_thread.cpp:226
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:80
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:351
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1200
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
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:44
unsigned int id() const
Get message ID.
Definition: message.cpp:181
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.