Fawkes API  Fawkes Development Version
rx28_thread.h
1 
2 /***************************************************************************
3  * rx28_thread.h - RX28 pan/tilt unit act thread
4  *
5  * Created: Thu Jun 18 09:52:16 2009
6  * Copyright 2006-2011 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 #ifndef __PLUGINS_PANTILT_ROBOTIS_RX28_THREAD_H_
24 #define __PLUGINS_PANTILT_ROBOTIS_RX28_THREAD_H_
25 
26 #include "../act_thread.h"
27 
28 #ifdef HAVE_TF
29 # include <aspect/tf.h>
30 #endif
31 #include <blackboard/interface_listener.h>
32 #include <utils/time/time.h>
33 
34 #ifdef USE_TIMETRACKER
35 # include <utils/time/tracker.h>
36 #endif
37 #include <string>
38 #include <memory>
39 
40 namespace fawkes {
41  class PanTiltInterface;
42  class LedInterface;
43  class ReadWriteLock;
44  class WaitCondition;
45 }
46 
47 class RobotisRX28;
48 
50 : public PanTiltActThread,
51 #ifdef HAVE_TF
53 #endif
55 {
56  public:
57  PanTiltRX28Thread(std::string &pantilt_cfg_prefix,
58  std::string &ptu_cfg_prefix,
59  std::string &ptu_name);
60 
61  virtual void init();
62  virtual bool prepare_finalize_user();
63  virtual void finalize();
64  virtual void loop();
65 
66  // For BlackBoardInterfaceListener
67  virtual bool bb_interface_message_received(fawkes::Interface *interface,
68  fawkes::Message *message) throw();
69 
70  void update_sensor_values();
71 
72  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
73  protected: virtual void run() { Thread::run(); }
74 
75  private:
76  fawkes::PanTiltInterface *__pantilt_if;
77  fawkes::LedInterface *__led_if;
78 
80 
81  std::string __pantilt_cfg_prefix;
82  std::string __ptu_cfg_prefix;
83  std::string __ptu_name;
84  std::string __cfg_device;
85  unsigned int __cfg_read_timeout_ms;
86  unsigned int __cfg_disc_timeout_ms;
87  unsigned int __cfg_pan_servo_id;
88  unsigned int __cfg_tilt_servo_id;
89  bool __cfg_goto_zero_start;
90  bool __cfg_turn_off;
91  unsigned int __cfg_cw_compl_margin;
92  unsigned int __cfg_ccw_compl_margin;
93  unsigned int __cfg_cw_compl_slope;
94  unsigned int __cfg_ccw_compl_slope;
95  float __cfg_pan_min;
96  float __cfg_pan_max;
97  float __cfg_tilt_min;
98  float __cfg_tilt_max;
99  float __cfg_pan_margin;
100  float __cfg_tilt_margin;
101  float __cfg_pan_offset;
102  float __cfg_tilt_offset;
103  float __cfg_pan_start;
104  float __cfg_tilt_start;
105 #ifdef HAVE_TF
106  std::string __cfg_base_frame;
107  std::string __cfg_pan_link;
108  std::string __cfg_tilt_link;
109 
110  fawkes::tf::Vector3 __translation_pan;
111  fawkes::tf::Vector3 __translation_tilt;
112 #endif
113 
114  float __last_pan;
115  float __last_tilt;
116 
117  class WorkerThread : public fawkes::Thread
118  {
119  public:
120  WorkerThread(std::string ptu_name, fawkes::Logger *logger,
122  unsigned char pan_servo_id, unsigned char tilt_servo_id,
123  float &pan_min, float &pan_max, float &tilt_min, float &tilt_max,
124  float &pan_offset, float &tilt_offset);
125 
126  ~WorkerThread();
127  void goto_pantilt(float pan, float tilt);
128  void goto_pantilt_timed(float pan, float tilt, float time_sec);
129  void get_pantilt(float &pan, float &tilt);
130  void get_pantilt(float &pan, float &tilt, fawkes::Time &time);
131  void set_velocities(float pan_vel, float tilt_vel);
132  void get_velocities(float &pan_vel, float &tilt_vel);
133  void set_margins(float pan_margin, float tilt_margin);
134  bool is_final();
135  bool is_enabled();
136  void set_enabled(bool enabled);
137  void set_led_enabled(bool enabled);
138  void stop_motion();
139  bool has_fresh_data();
140  void wait_for_fresh_data();
141 
142  virtual void loop();
143 
144  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
145  protected: virtual void run() { Thread::run(); }
146 
147  private:
148  void exec_goto_pantilt(float pan, float tilt);
149 
150  private:
151  fawkes::ReadWriteLock *__rx28_rwlock;
153  fawkes::Logger *__logger;
154  fawkes::WaitCondition *__update_waitcond;
155 
156  unsigned char __pan_servo_id;
157  unsigned char __tilt_servo_id;
158 
159  float __pan_min;
160  float __pan_max;
161  float __tilt_min;
162  float __tilt_max;
163  float __pan_offset;
164  float __tilt_offset;
165  float __max_pan_speed;
166  float __max_tilt_speed;
167  float __pan_margin;
168  float __tilt_margin;
169 
170  fawkes::ReadWriteLock *__value_rwlock;
171  bool __move_pending;
172  float __target_pan;
173  float __target_tilt;
174  bool __enable;
175  bool __disable;
176  bool __velo_pending;
177  unsigned int __pan_vel;
178  unsigned int __tilt_vel;
179  bool __led_enable;
180  bool __led_disable;
181  fawkes::Time __pantilt_time;
182 
183  bool __fresh_data;
184  fawkes::Mutex *__fresh_data_mutex;
185 
186  };
187 
188  WorkerThread *__wt;
189 };
190 
191 #endif
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message)
BlackBoard message received notification.
virtual void loop()
Code to execute in the thread.
Wait until a given condition holds.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:43
void update_sensor_values()
Update sensor values as necessary.
Fawkes library namespace.
A class for handling time.
Definition: time.h:91
Thread class encapsulation of pthreads.
Definition: thread.h:42
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: rx28_thread.h:73
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
PanTiltRX28Thread(std::string &pantilt_cfg_prefix, std::string &ptu_cfg_prefix, std::string &ptu_name)
Constructor.
Definition: rx28_thread.cpp:51
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
Thread aspect to access the transform system.
Definition: tf.h:38
virtual void init()
Initialize the thread.
Definition: rx28_thread.cpp:72
PanTilt act thread for RX28 PTU.
Definition: rx28_thread.h:49
Read/write lock to allow multiple readers but only a single writer on the resource at a time...
Pan/tilt act thread.
Definition: act_thread.h:36
Class to access a chain of Robotis RX28 servos.
Definition: rx28.h:42
LedInterface Fawkes BlackBoard Interface.
Definition: LedInterface.h:33
virtual bool prepare_finalize_user()
Prepare finalization user implementation.
PanTiltInterface Fawkes BlackBoard Interface.
Mutex mutual exclusion lock.
Definition: mutex.h:32
virtual void finalize()
Finalize the thread.
BlackBoard interface listener.
Interface for logging.
Definition: logger.h:34