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
40namespace fawkes {
41class PanTiltInterface;
42class LedInterface;
43class JointInterface;
44class ReadWriteLock;
45class WaitCondition;
46} // namespace fawkes
47
48class RobotisRX28;
49
51#ifdef HAVE_TF
53#endif
55{
56public:
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
68 fawkes::Message * message) noexcept;
69
71
72 /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
73protected:
74 virtual void
76 {
77 Thread::run();
78 }
79
80private:
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.