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 JointInterface;
44  class ReadWriteLock;
45  class WaitCondition;
46 }
47 
48 class RobotisRX28;
49 
51 : public PanTiltActThread,
52 #ifdef HAVE_TF
54 #endif
56 {
57  public:
58  PanTiltRX28Thread(std::string &pantilt_cfg_prefix,
59  std::string &ptu_cfg_prefix,
60  std::string &ptu_name);
61 
62  virtual void init();
63  virtual bool prepare_finalize_user();
64  virtual void finalize();
65  virtual void loop();
66 
67  // For BlackBoardInterfaceListener
68  virtual bool bb_interface_message_received(fawkes::Interface *interface,
69  fawkes::Message *message) throw();
70 
71  void update_sensor_values();
72 
73  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
74  protected: virtual void run() { Thread::run(); }
75 
76  private:
77  fawkes::PanTiltInterface *__pantilt_if;
78  fawkes::LedInterface *__led_if;
79  fawkes::JointInterface *__panjoint_if;
80  fawkes::JointInterface *__tiltjoint_if;
81 
83 
84  std::string __pantilt_cfg_prefix;
85  std::string __ptu_cfg_prefix;
86  std::string __ptu_name;
87  std::string __cfg_device;
88  unsigned int __cfg_read_timeout_ms;
89  unsigned int __cfg_disc_timeout_ms;
90  unsigned int __cfg_pan_servo_id;
91  unsigned int __cfg_tilt_servo_id;
92  bool __cfg_goto_zero_start;
93  bool __cfg_turn_off;
94  unsigned int __cfg_cw_compl_margin;
95  unsigned int __cfg_ccw_compl_margin;
96  unsigned int __cfg_cw_compl_slope;
97  unsigned int __cfg_ccw_compl_slope;
98  float __cfg_pan_min;
99  float __cfg_pan_max;
100  float __cfg_tilt_min;
101  float __cfg_tilt_max;
102  float __cfg_pan_margin;
103  float __cfg_tilt_margin;
104  float __cfg_pan_offset;
105  float __cfg_tilt_offset;
106  float __cfg_pan_start;
107  float __cfg_tilt_start;
108 #ifdef HAVE_TF
109  std::string __cfg_base_frame;
110  std::string __cfg_pan_link;
111  std::string __cfg_tilt_link;
112 
113  fawkes::tf::Vector3 __translation_pan;
114  fawkes::tf::Vector3 __translation_tilt;
115 
116  bool __cfg_publish_transforms;
117 #endif
118 
119  float __last_pan;
120  float __last_tilt;
121 
122  class WorkerThread : public fawkes::Thread
123  {
124  public:
125  WorkerThread(std::string ptu_name, fawkes::Logger *logger,
127  unsigned char pan_servo_id, unsigned char tilt_servo_id,
128  float &pan_min, float &pan_max, float &tilt_min, float &tilt_max,
129  float &pan_offset, float &tilt_offset);
130 
131  ~WorkerThread();
132  void goto_pantilt(float pan, float tilt);
133  void goto_pantilt_timed(float pan, float tilt, float time_sec);
134  void get_pantilt(float &pan, float &tilt);
135  void get_pantilt(float &pan, float &tilt, fawkes::Time &time);
136  void set_velocities(float pan_vel, float tilt_vel);
137  void get_velocities(float &pan_vel, float &tilt_vel);
138  void set_margins(float pan_margin, float tilt_margin);
139  bool is_final();
140  bool is_enabled();
141  void set_enabled(bool enabled);
142  void set_led_enabled(bool enabled);
143  void stop_motion();
144  bool has_fresh_data();
145  void wait_for_fresh_data();
146 
147  virtual void loop();
148 
149  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
150  protected: virtual void run() { Thread::run(); }
151 
152  private:
153  void exec_goto_pantilt(float pan, float tilt);
154 
155  private:
156  fawkes::ReadWriteLock *__rx28_rwlock;
158  fawkes::Logger *__logger;
159  fawkes::WaitCondition *__update_waitcond;
160 
161  unsigned char __pan_servo_id;
162  unsigned char __tilt_servo_id;
163 
164  float __pan_min;
165  float __pan_max;
166  float __tilt_min;
167  float __tilt_max;
168  float __pan_offset;
169  float __tilt_offset;
170  float __max_pan_speed;
171  float __max_tilt_speed;
172  float __pan_margin;
173  float __tilt_margin;
174 
175  fawkes::ReadWriteLock *__value_rwlock;
176  bool __move_pending;
177  float __target_pan;
178  float __target_tilt;
179  bool __enable;
180  bool __disable;
181  bool __velo_pending;
182  unsigned int __pan_vel;
183  unsigned int __tilt_vel;
184  bool __led_enable;
185  bool __led_disable;
186  fawkes::Time __pantilt_time;
187 
188  bool __fresh_data;
189  fawkes::Mutex *__fresh_data_mutex;
190 
191  };
192 
193  WorkerThread *__wt;
194 };
195 
196 #endif
Wait until a given condition holds.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
Fawkes library namespace.
virtual void run()
Code to execute in the thread.
Definition: thread.cpp:939
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:74
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
Thread aspect to access the transform system.
Definition: tf.h:42
PanTilt act thread for RX28 PTU.
Definition: rx28_thread.h:50
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
PanTiltInterface Fawkes BlackBoard Interface.
JointInterface Fawkes BlackBoard Interface.
Mutex mutual exclusion lock.
Definition: mutex.h:32
BlackBoard interface listener.
Interface for logging.
Definition: logger.h:34