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 <memory>
38 #include <string>
39 
40 namespace fawkes {
41 class PanTiltInterface;
42 class LedInterface;
43 class JointInterface;
44 class ReadWriteLock;
45 class WaitCondition;
46 } // namespace fawkes
47 
48 class RobotisRX28;
49 
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) noexcept;
69 
70  void update_sensor_values();
71 
72  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
73 protected:
74  virtual void
75  run()
76  {
77  Thread::run();
78  }
79 
80 private:
81  fawkes::PanTiltInterface *pantilt_if_;
82  fawkes::LedInterface * led_if_;
83  fawkes::JointInterface * panjoint_if_;
84  fawkes::JointInterface * tiltjoint_if_;
85 
87 
88  std::string pantilt_cfg_prefix_;
89  std::string ptu_cfg_prefix_;
90  std::string ptu_name_;
91  std::string cfg_device_;
92  unsigned int cfg_read_timeout_ms_;
93  unsigned int cfg_disc_timeout_ms_;
94  unsigned int cfg_pan_servo_id_;
95  unsigned int cfg_tilt_servo_id_;
96  bool cfg_goto_zero_start_;
97  bool cfg_turn_off_;
98  unsigned int cfg_cw_compl_margin_;
99  unsigned int cfg_ccw_compl_margin_;
100  unsigned int cfg_cw_compl_slope_;
101  unsigned int cfg_ccw_compl_slope_;
102  float cfg_pan_min_;
103  float cfg_pan_max_;
104  float cfg_tilt_min_;
105  float cfg_tilt_max_;
106  float cfg_pan_margin_;
107  float cfg_tilt_margin_;
108  float cfg_pan_offset_;
109  float cfg_tilt_offset_;
110  float cfg_pan_start_;
111  float cfg_tilt_start_;
112 #ifdef HAVE_TF
113  std::string cfg_base_frame_;
114  std::string cfg_pan_link_;
115  std::string cfg_tilt_link_;
116 
117  fawkes::tf::Vector3 translation_pan_;
118  fawkes::tf::Vector3 translation_tilt_;
119 
120  bool cfg_publish_transforms_;
121 #endif
122 
123  float last_pan_;
124  float last_tilt_;
125 
126  class WorkerThread : public fawkes::Thread
127  {
128  public:
129  WorkerThread(std::string ptu_name,
132  unsigned char pan_servo_id,
133  unsigned char tilt_servo_id,
134  float & pan_min,
135  float & pan_max,
136  float & tilt_min,
137  float & tilt_max,
138  float & pan_offset,
139  float & tilt_offset);
140 
141  ~WorkerThread();
142  void goto_pantilt(float pan, float tilt);
143  void goto_pantilt_timed(float pan, float tilt, float time_sec);
144  void get_pantilt(float &pan, float &tilt);
145  void get_pantilt(float &pan, float &tilt, fawkes::Time &time);
146  void set_velocities(float pan_vel, float tilt_vel);
147  void get_velocities(float &pan_vel, float &tilt_vel);
148  void set_margins(float pan_margin, float tilt_margin);
149  bool is_final();
150  bool is_enabled();
151  void set_enabled(bool enabled);
152  void set_led_enabled(bool enabled);
153  void stop_motion();
154  bool has_fresh_data();
155  void wait_for_fresh_data();
156 
157  virtual void loop();
158 
159  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
160  protected:
161  virtual void
162  run()
163  {
164  Thread::run();
165  }
166 
167  private:
168  void exec_goto_pantilt(float pan, float tilt);
169 
170  private:
171  fawkes::ReadWriteLock * rx28_rwlock_;
173  fawkes::Logger * logger_;
174  fawkes::WaitCondition * update_waitcond_;
175 
176  unsigned char pan_servo_id_;
177  unsigned char tilt_servo_id_;
178 
179  float pan_min_;
180  float pan_max_;
181  float tilt_min_;
182  float tilt_max_;
183  float pan_offset_;
184  float tilt_offset_;
185  float max_pan_speed_;
186  float max_tilt_speed_;
187  float pan_margin_;
188  float tilt_margin_;
189 
190  fawkes::ReadWriteLock *value_rwlock_;
191  bool move_pending_;
192  float target_pan_;
193  float target_tilt_;
194  bool enable_;
195  bool disable_;
196  bool velo_pending_;
197  unsigned int pan_vel_;
198  unsigned int tilt_vel_;
199  bool led_enable_;
200  bool led_disable_;
201  fawkes::Time pantilt_time_;
202 
203  bool fresh_data_;
204  fawkes::Mutex *fresh_data_mutex_;
205  };
206 
207  WorkerThread *wt_;
208 };
209 
210 #endif
Pan/tilt act thread.
Definition: act_thread.h:41
PanTilt act thread for RX28 PTU.
Definition: rx28_thread.h:55
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: rx28_thread.h:75
void update_sensor_values()
Update sensor values as necessary.
PanTiltRX28Thread(std::string &pantilt_cfg_prefix, std::string &ptu_cfg_prefix, std::string &ptu_name)
Constructor.
Definition: rx28_thread.cpp:53
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
Definition: rx28_thread.cpp:72
virtual void finalize()
Finalize the thread.
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message) noexcept
BlackBoard message received notification.
virtual bool prepare_finalize_user()
Prepare finalization user implementation.
Class to access a chain of Robotis RX28 servos.
Definition: rx28.h:44
BlackBoard interface listener.
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
JointInterface Fawkes BlackBoard Interface.
LedInterface Fawkes BlackBoard Interface.
Definition: LedInterface.h:34
Interface for logging.
Definition: logger.h:42
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
Mutex mutual exclusion lock.
Definition: mutex.h:33
PanTiltInterface Fawkes BlackBoard Interface.
Read/write lock to allow multiple readers but only a single writer on the resource at a time.
Thread class encapsulation of pthreads.
Definition: thread.h:46
A class for handling time.
Definition: time.h:93
Thread aspect to access the transform system.
Definition: tf.h:39
Wait until a given condition holds.
Fawkes library namespace.