Fawkes API Fawkes Development Version
act_thread.cpp
1
2/***************************************************************************
3 * act_thread.cpp - Robotino act thread
4 *
5 * Created: Sun Nov 13 16:07:40 2011
6 * Copyright 2011-2016 Tim Niemueller [www.niemueller.de]
7 * 2014 Sebastian Reuter
8 * 2014 Tobias Neumann
9 * 2017 Till Hofmann
10 ****************************************************************************/
11
12/* This program is free software; you can redistribute it and/or modify
13 * it under the terms of the GNU General Public License as published by
14 * the Free Software Foundation; either version 2 of the License, or
15 * (at your option) any later version.
16 *
17 * This program is distributed in the hope that it will be useful,
18 * but WITHOUT ANY WARRANTY; without even the implied warranty of
19 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20 * GNU Library General Public License for more details.
21 *
22 * Read the full text in the LICENSE.GPL file in the doc directory.
23 */
24
25#include "act_thread.h"
26
27#include "com_thread.h"
28
29#include <interfaces/GripperInterface.h>
30#include <interfaces/IMUInterface.h>
31#include <interfaces/MotorInterface.h>
32#include <utils/math/angle.h>
33
34using namespace fawkes;
35
36/** @class RobotinoActThread "act_thread.h"
37 * Robotino act hook integration thread.
38 * This thread integrates into the Fawkes main loop at the ACT hook and
39 * executes motion commands.
40 * @author Tim Niemueller
41 */
42
43/** Constructor.
44 * @param com_thread thread that communicates with the hardware base
45 */
47: Thread("RobotinoActThread", Thread::OPMODE_WAITFORWAKEUP),
48#ifdef HAVE_TF
49 TransformAspect(TransformAspect::ONLY_PUBLISHER, "Robotino Odometry"),
50#endif
52{
53 com_ = com_thread;
54}
55
56void
58{
59 last_seqnum_ = 0;
60 last_msg_time_ = clock->now();
61
62 //get config values
63 cfg_deadman_threshold_ = config->get_float("/hardware/robotino/deadman_time_threshold");
64 cfg_gripper_enabled_ = config->get_bool("/hardware/robotino/gripper/enable_gripper");
65 cfg_bumper_estop_enabled_ = config->get_bool("/hardware/robotino/bumper/estop_enabled");
66 cfg_odom_time_offset_ = config->get_float("/hardware/robotino/odometry/time_offset");
67 cfg_odom_frame_ = config->get_string("/frames/odom");
68 cfg_base_frame_ = config->get_string("/frames/base");
69 std::string odom_mode = config->get_string("/hardware/robotino/odometry/mode");
70 cfg_odom_corr_phi_ = config->get_float("/hardware/robotino/odometry/calc/correction/phi");
71 cfg_odom_corr_trans_ = config->get_float("/hardware/robotino/odometry/calc/correction/trans");
72
73 cfg_rb_ = config->get_float("/hardware/robotino/drive/layout/rb");
74 cfg_rw_ = config->get_float("/hardware/robotino/drive/layout/rw");
75 cfg_gear_ = config->get_float("/hardware/robotino/drive/layout/gear");
76
77 cfg_trans_accel_ = config->get_float("/hardware/robotino/drive/trans-acceleration");
78 cfg_trans_decel_ = config->get_float("/hardware/robotino/drive/trans-deceleration");
79 cfg_rot_accel_ = config->get_float("/hardware/robotino/drive/rot-acceleration");
80 cfg_rot_decel_ = config->get_float("/hardware/robotino/drive/rot-deceleration");
81
82#ifdef HAVE_TF
83 cfg_publish_transform_ = true;
84 try {
85 cfg_publish_transform_ = config->get_bool("/hardware/robotino/odometry/publish_transform");
86 } catch (Exception &e) {
87 // ignore, use default
88 }
89#endif // HAVE_TF
90
91 com_->set_drive_layout(cfg_rb_, cfg_rw_, cfg_gear_);
92 com_->set_drive_limits(cfg_trans_accel_, cfg_trans_decel_, cfg_rot_accel_, cfg_rot_decel_);
93
94 std::string imu_if_id;
95
96 imu_if_ = NULL;
97 if (odom_mode == "copy") {
98 cfg_odom_mode_ = ODOM_COPY;
99 } else if (odom_mode == "calc") {
100 cfg_odom_mode_ = ODOM_CALC;
101 imu_if_id = config->get_string("/hardware/robotino/odometry/calc/imu_interface_id");
102 cfg_imu_deadman_loops_ = config->get_uint("/hardware/robotino/odometry/calc/imu_deadman_loops");
103 } else {
104 throw Exception("Invalid odometry mode '%s', must be calc or copy", odom_mode.c_str());
105 }
106
107 gripper_close_ = false;
108
109 msg_received_ = false;
110 msg_zero_vel_ = false;
111
112 odom_x_ = odom_y_ = odom_phi_ = 0.;
113 odom_time_ = new Time(clock);
114
115 motor_if_ = blackboard->open_for_writing<MotorInterface>("Robotino");
116 gripper_if_ = blackboard->open_for_writing<GripperInterface>("Robotino");
117
118 if (cfg_odom_mode_ == ODOM_CALC) {
119 imu_if_ = blackboard->open_for_reading<IMUInterface>(imu_if_id.c_str());
120 imu_if_writer_warning_printed_ = false;
121 imu_if_changed_warning_printed_ = false;
122 imu_if_invquat_warning_printed_ = false;
123 imu_if_nochange_loops_ = 0;
124 }
125
126 //state_->emergencyStop.isEnabled = cfg_bumper_estop_enabled_;
127
128 motor_if_->set_motor_state(MotorInterface::MOTOR_ENABLED);
129 motor_if_->write();
130}
131
132void
134{
135 blackboard->close(imu_if_);
136 blackboard->close(motor_if_);
137 blackboard->close(gripper_if_);
138 com_->set_speed_points(0., 0., 0.);
139 com_ = NULL;
140 delete odom_time_;
141}
142
143void
145{
146 try {
147 com_->set_bumper_estop_enabled(cfg_bumper_estop_enabled_);
148 } catch (Exception &e) {
149 }
150}
151
152void
154{
155 if (!com_->is_connected()) {
156 if (!motor_if_->msgq_empty()) {
157 logger->log_warn(name(), "Motor commands received while not connected");
158 motor_if_->msgq_flush();
159 }
160 if (!gripper_if_->msgq_empty()) {
161 logger->log_warn(name(), "Gripper commands received while not connected");
162 gripper_if_->msgq_flush();
163 }
164 return;
165 }
166
167 bool reset_odometry = false;
168 bool set_des_vel = false;
169 while (!motor_if_->msgq_empty()) {
170 if (MotorInterface::SetMotorStateMessage *msg = motor_if_->msgq_first_safe(msg)) {
172 "%sabling motor on request",
173 msg->motor_state() == MotorInterface::MOTOR_ENABLED ? "En" : "Dis");
174 motor_if_->set_motor_state(msg->motor_state());
175 motor_if_->write();
176
177 des_vx_ = des_vy_ = des_omega_ = 0.;
178 set_des_vel = true;
179 }
180
181 else if (MotorInterface::TransRotMessage *msg = motor_if_->msgq_first_safe(msg)) {
182 des_vx_ = msg->vx();
183 des_vy_ = msg->vy();
184 des_omega_ = msg->omega();
185
186 last_msg_time_ = clock->now();
187 msg_received_ = true;
188
189 set_des_vel = true;
190 msg_zero_vel_ = (des_vx_ == 0.0 && des_vy_ == 0.0 && des_omega_ == 0.0);
191
192 if (msg->sender_thread_name() != last_transrot_sender_) {
193 last_transrot_sender_ = msg->sender_thread_name();
195 "Sender of TransRotMessage changed to %s",
196 last_transrot_sender_.c_str());
197 }
198 }
199
201 odom_x_ = odom_y_ = odom_phi_ = 0.;
202 if (imu_if_) {
203 imu_if_->read();
204 odom_gyro_origin_ = tf::get_yaw(imu_if_->orientation());
205 }
206
207 reset_odometry = true;
208 }
209
210 motor_if_->msgq_pop();
211 } // while motor msgq
212
213 if (cfg_gripper_enabled_) {
214 bool open_gripper = false, close_gripper = false;
215 while (!gripper_if_->msgq_empty()) {
217 close_gripper = false;
218 open_gripper = true;
219 } else if (gripper_if_->msgq_first_is<GripperInterface::CloseGripperMessage>()) {
220 close_gripper = true;
221 open_gripper = false;
222 }
223
224 gripper_if_->msgq_pop();
225 }
226
227 if (close_gripper || open_gripper) {
228 gripper_close_ = close_gripper;
229 com_->set_gripper(open_gripper);
230 }
231 } else {
232 if (!gripper_if_->msgq_empty())
233 gripper_if_->msgq_flush();
234 }
235
236 // deadman switch to set the velocities to zero if no new message arrives
237 double diff = (clock->now() - (&last_msg_time_));
238 if (diff >= cfg_deadman_threshold_ && msg_received_ && !msg_zero_vel_) {
240 "Time-Gap between TransRotMsgs too large "
241 "(%f sec.), motion planner alive?",
242 diff);
243 des_vx_ = des_vy_ = des_omega_ = 0.;
244 msg_zero_vel_ = true;
245 set_des_vel = true;
246 msg_received_ = false;
247 }
248
249 if (motor_if_->motor_state() == MotorInterface::MOTOR_DISABLED) {
250 if (set_des_vel && ((des_vx_ != 0.0) || (des_vy_ != 0.0) || (des_omega_ != 0.0))) {
251 logger->log_warn(name(), "Motor command received while disabled, ignoring");
252 }
253 des_vx_ = des_vy_ = des_omega_ = 0.;
254 set_des_vel = true;
255 }
256
257 if (reset_odometry)
258 com_->reset_odometry();
259 if (set_des_vel)
260 com_->set_desired_vel(des_vx_, des_vy_, des_omega_);
261
262 publish_odometry();
263
264 if (cfg_gripper_enabled_) {
265 publish_gripper();
266 }
267}
268
269void
270RobotinoActThread::publish_odometry()
271{
272 fawkes::Time sensor_time;
273 float a1 = 0., a2 = 0., a3 = 0.;
274 unsigned int seq = 0;
275 com_->get_act_velocity(a1, a2, a3, seq, sensor_time);
276
277 if (seq != last_seqnum_) {
278 last_seqnum_ = seq;
279
280 float vx = 0., vy = 0., omega = 0.;
281 com_->unproject(&vx, &vy, &omega, a1, a2, a3);
282
283 motor_if_->set_vx(vx);
284 motor_if_->set_vy(vy);
285 motor_if_->set_omega(omega);
286
287 motor_if_->set_des_vx(des_vx_);
288 motor_if_->set_des_vy(des_vy_);
289 motor_if_->set_des_omega(des_omega_);
290
291 if (cfg_odom_mode_ == ODOM_COPY) {
292 float diff_sec = sensor_time - odom_time_;
293 *odom_time_ = sensor_time;
294 odom_phi_ = normalize_mirror_rad(odom_phi_ + omega * diff_sec * cfg_odom_corr_phi_);
295 odom_x_ += cos(odom_phi_) * vx * diff_sec * cfg_odom_corr_trans_
296 - sin(odom_phi_) * vy * diff_sec * cfg_odom_corr_trans_;
297 odom_y_ += sin(odom_phi_) * vx * diff_sec * cfg_odom_corr_trans_
298 + cos(odom_phi_) * vy * diff_sec * cfg_odom_corr_trans_;
299 } else {
300 float diff_sec = sensor_time - odom_time_;
301 *odom_time_ = sensor_time;
302
303 // velocity-based method
304 if (imu_if_ && imu_if_->has_writer()) {
305 imu_if_->read();
306 if (imu_if_->refreshed()) {
307 //float imu_age = now - imu_if_->timestamp();
308 //logger->log_debug(name(), "IMU age: %f sec", imu_age);
309 float *ori_q = imu_if_->orientation();
310 try {
311 tf::Quaternion q(ori_q[0], ori_q[1], ori_q[2], ori_q[3]);
312 tf::assert_quaternion_valid(q);
313
314 // reset no change loop count
315 imu_if_nochange_loops_ = 0;
316
317 if (imu_if_writer_warning_printed_ || imu_if_invquat_warning_printed_
318 || imu_if_changed_warning_printed_) {
319 float old_odom_gyro_origin = odom_gyro_origin_;
320
321 // adjust gyro angle for continuity if we used
322 // wheel odometry for some time
323 // Note that we use the _updated_ odometry, i.e. we
324 // use wheel odometry for the last time frame because
325 // we do not have any point of reference for the gyro, yet
326 float wheel_odom_phi = normalize_mirror_rad(odom_phi_ + omega * diff_sec);
327 odom_gyro_origin_ = tf::get_yaw(q) - wheel_odom_phi;
328
329 if (imu_if_writer_warning_printed_) {
330 imu_if_writer_warning_printed_ = false;
332 "IMU writer is back again, "
333 "adjusted origin to %f (was %f)",
334 odom_gyro_origin_,
335 old_odom_gyro_origin);
336 }
337
338 if (imu_if_changed_warning_printed_) {
339 imu_if_changed_warning_printed_ = false;
341 "IMU interface changed again, "
342 "adjusted origin to %f (was %f)",
343 odom_gyro_origin_,
344 old_odom_gyro_origin);
345 }
346 if (imu_if_invquat_warning_printed_) {
347 imu_if_invquat_warning_printed_ = false;
348
350 "IMU quaternion valid again, "
351 "adjusted origin to %f (was %f)",
352 odom_gyro_origin_,
353 old_odom_gyro_origin);
354 }
355 }
356
357 // Yaw taken as asbolute value from IMU, the origin is used
358 // to smooth recovery of IMU data (see above) or for
359 // odometry reset requests (see message processing)
360 odom_phi_ = normalize_mirror_rad(tf::get_yaw(q) - odom_gyro_origin_);
361
362 } catch (Exception &e) {
363 if (!imu_if_invquat_warning_printed_) {
364 imu_if_invquat_warning_printed_ = true;
366 "Invalid gyro quaternion (%f,%f,%f,%f), "
367 "falling back to wheel odometry",
368 ori_q[0],
369 ori_q[1],
370 ori_q[2],
371 ori_q[3]);
372 }
373 odom_phi_ = normalize_mirror_rad(odom_phi_ + omega * diff_sec * cfg_odom_corr_phi_);
374 }
375 } else {
376 if (++imu_if_nochange_loops_ > cfg_imu_deadman_loops_) {
377 if (!imu_if_changed_warning_printed_) {
378 imu_if_changed_warning_printed_ = true;
380 "IMU interface not changed, "
381 "falling back to wheel odometry");
382 }
383 odom_phi_ = normalize_mirror_rad(odom_phi_ + omega * diff_sec * cfg_odom_corr_phi_);
384 } // else use previous odometry yaw value
385 }
386 } else {
387 if (!imu_if_writer_warning_printed_) {
389 "No writer for IMU interface, "
390 "using wheel odometry only");
391 imu_if_writer_warning_printed_ = true;
392 }
393
394 odom_phi_ = normalize_mirror_rad(odom_phi_ + omega * diff_sec * cfg_odom_corr_phi_);
395 }
396
397 odom_x_ += cos(odom_phi_) * vx * diff_sec * cfg_odom_corr_trans_
398 - sin(odom_phi_) * vy * diff_sec * cfg_odom_corr_trans_;
399 odom_y_ += sin(odom_phi_) * vx * diff_sec * cfg_odom_corr_trans_
400 + cos(odom_phi_) * vy * diff_sec * cfg_odom_corr_trans_;
401 }
402
403 motor_if_->set_odometry_position_x(odom_x_);
404 motor_if_->set_odometry_position_y(odom_y_);
405 motor_if_->set_odometry_orientation(odom_phi_);
406 motor_if_->write();
407
408#ifdef HAVE_TF
409 if (cfg_publish_transform_) {
410 tf::Transform t(tf::Quaternion(tf::Vector3(0, 0, 1), odom_phi_),
411 tf::Vector3(odom_x_, odom_y_, 0.));
412
413 try {
414 tf_publisher->send_transform(t,
415 sensor_time + cfg_odom_time_offset_,
416 cfg_odom_frame_,
417 cfg_base_frame_);
418 } catch (Exception &e) {
420 "Failed to publish odometry transform for "
421 "(%f,%f,%f), exception follows",
422 odom_x_,
423 odom_y_,
424 odom_phi_);
425 logger->log_warn(name(), e);
426 }
427 }
428#endif
429 }
430}
431
432void
433RobotinoActThread::publish_gripper()
434{
435 if (com_->is_gripper_open()) {
436 gripper_if_->set_gripper_state(GripperInterface::CLOSED);
437 gripper_if_->write();
438 } else {
439 gripper_if_->set_gripper_state(GripperInterface::OPEN);
440 gripper_if_->write();
441 }
442}
virtual void finalize()
Finalize the thread.
Definition: act_thread.cpp:133
RobotinoActThread(RobotinoComThread *com_thread)
Constructor.
Definition: act_thread.cpp:46
virtual void once()
Execute an action exactly once.
Definition: act_thread.cpp:144
virtual void loop()
Code to execute in the thread.
Definition: act_thread.cpp:153
virtual void init()
Initialize the thread.
Definition: act_thread.cpp:57
Virtual base class for thread that communicates with a Robotino.
Definition: com_thread.h:41
virtual void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t)=0
Get actual velocity.
virtual void reset_odometry()=0
Reset odometry to zero.
virtual void set_gripper(bool opened)=0
Open or close gripper.
void set_drive_limits(float trans_accel, float trans_decel, float rot_accel, float rot_decel)
Set the omni drive limits.
Definition: com_thread.cpp:197
virtual bool is_connected()=0
Check if we are connected to OpenRobotino.
virtual void set_bumper_estop_enabled(bool enabled)=0
Enable or disable emergency stop on bumper contact.
virtual void set_speed_points(float s1, float s2, float s3)=0
Set speed points for wheels.
void set_drive_layout(float rb, float rw, float gear)
Set omni drive layout parameters.
Definition: com_thread.cpp:183
virtual bool is_gripper_open()=0
Check if gripper is open.
virtual void set_desired_vel(float vx, float vy, float omega)
Set desired velocities.
Definition: com_thread.cpp:214
void unproject(float *vx, float *vy, float *omega, float m1, float m2, float m3) const
Project single motor speeds to velocity in cartesian coordinates.
Definition: com_thread.cpp:394
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Time now() const
Get the current time.
Definition: clock.cpp:242
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 bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
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
CloseGripperMessage Fawkes BlackBoard Interface Message.
OpenGripperMessage Fawkes BlackBoard Interface Message.
GripperInterface Fawkes BlackBoard Interface.
void set_gripper_state(const GripperState new_gripper_state)
Set gripper_state value.
IMUInterface Fawkes BlackBoard Interface.
Definition: IMUInterface.h:34
float * orientation() const
Get orientation value.
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
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
void msgq_flush()
Flush all messages.
Definition: interface.cpp:1079
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
MessageType * msgq_first_safe(MessageType *&msg) noexcept
Get first message casted to the desired type without exceptions.
Definition: interface.h:340
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:848
bool refreshed() const
Check if data has been refreshed.
Definition: interface.cpp:811
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
ResetOdometryMessage Fawkes BlackBoard Interface Message.
SetMotorStateMessage Fawkes BlackBoard Interface Message.
TransRotMessage Fawkes BlackBoard Interface Message.
MotorInterface Fawkes BlackBoard Interface.
void set_des_vx(const float new_des_vx)
Set des_vx value.
void set_des_vy(const float new_des_vy)
Set des_vy value.
void set_odometry_orientation(const float new_odometry_orientation)
Set odometry_orientation value.
uint32_t motor_state() const
Get motor_state value.
void set_vy(const float new_vy)
Set vy value.
void set_vx(const float new_vx)
Set vx value.
void set_omega(const float new_omega)
Set omega value.
void set_des_omega(const float new_des_omega)
Set des_omega value.
void set_odometry_position_x(const float new_odometry_position_x)
Set odometry_position_x value.
void set_odometry_position_y(const float new_odometry_position_y)
Set odometry_position_y value.
void set_motor_state(const uint32_t new_motor_state)
Set motor_state value.
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
A class for handling time.
Definition: time.h:93
Thread aspect to access the transform system.
Definition: tf.h:39
Fawkes library namespace.
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:72