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