25 #include "act_thread.h"
27 #include "com_thread.h"
29 #include <interfaces/GripperInterface.h>
30 #include <interfaces/IMUInterface.h>
31 #include <interfaces/MotorInterface.h>
32 #include <utils/math/angle.h>
47 :
Thread(
"RobotinoActThread",
Thread::OPMODE_WAITFORWAKEUP),
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");
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");
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");
83 cfg_publish_transform_ =
true;
85 cfg_publish_transform_ =
config->
get_bool(
"/hardware/robotino/odometry/publish_transform");
92 com_->
set_drive_limits(cfg_trans_accel_, cfg_trans_decel_, cfg_rot_accel_, cfg_rot_decel_);
94 std::string imu_if_id;
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");
104 throw Exception(
"Invalid odometry mode '%s', must be calc or copy", odom_mode.c_str());
107 gripper_close_ =
false;
109 msg_received_ =
false;
110 msg_zero_vel_ =
false;
112 odom_x_ = odom_y_ = odom_phi_ = 0.;
118 if (cfg_odom_mode_ == ODOM_CALC) {
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;
167 bool reset_odometry =
false;
168 bool set_des_vel =
false;
172 "%sabling motor on request",
173 msg->motor_state() == MotorInterface::MOTOR_ENABLED ?
"En" :
"Dis");
177 des_vx_ = des_vy_ = des_omega_ = 0.;
184 des_omega_ = msg->omega();
187 msg_received_ =
true;
190 msg_zero_vel_ = (des_vx_ == 0.0 && des_vy_ == 0.0 && des_omega_ == 0.0);
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());
201 odom_x_ = odom_y_ = odom_phi_ = 0.;
204 odom_gyro_origin_ = tf::get_yaw(imu_if_->
orientation());
207 reset_odometry =
true;
213 if (cfg_gripper_enabled_) {
214 bool open_gripper =
false, close_gripper =
false;
217 close_gripper =
false;
220 close_gripper =
true;
221 open_gripper =
false;
227 if (close_gripper || open_gripper) {
228 gripper_close_ = close_gripper;
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?",
243 des_vx_ = des_vy_ = des_omega_ = 0.;
244 msg_zero_vel_ =
true;
246 msg_received_ =
false;
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))) {
253 des_vx_ = des_vy_ = des_omega_ = 0.;
264 if (cfg_gripper_enabled_) {
270 RobotinoActThread::publish_odometry()
273 float a1 = 0., a2 = 0., a3 = 0.;
274 unsigned int seq = 0;
277 if (seq != last_seqnum_) {
280 float vx = 0., vy = 0., omega = 0.;
281 com_->
unproject(&vx, &vy, &omega, a1, a2, a3);
291 if (cfg_odom_mode_ == ODOM_COPY) {
292 float diff_sec = sensor_time - odom_time_;
293 *odom_time_ = sensor_time;
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_;
300 float diff_sec = sensor_time - odom_time_;
301 *odom_time_ = sensor_time;
311 tf::Quaternion q(ori_q[0], ori_q[1], ori_q[2], ori_q[3]);
312 tf::assert_quaternion_valid(q);
315 imu_if_nochange_loops_ = 0;
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_;
327 odom_gyro_origin_ = tf::get_yaw(q) - wheel_odom_phi;
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)",
335 old_odom_gyro_origin);
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)",
344 old_odom_gyro_origin);
346 if (imu_if_invquat_warning_printed_) {
347 imu_if_invquat_warning_printed_ =
false;
350 "IMU quaternion valid again, "
351 "adjusted origin to %f (was %f)",
353 old_odom_gyro_origin);
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",
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");
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;
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_;
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.));
414 tf_publisher->send_transform(t,
415 sensor_time + cfg_odom_time_offset_,
420 "Failed to publish odometry transform for "
421 "(%f,%f,%f), exception follows",
433 RobotinoActThread::publish_gripper()
437 gripper_if_->
write();
440 gripper_if_->
write();
virtual void finalize()
Finalize the thread.
RobotinoActThread(RobotinoComThread *com_thread)
Constructor.
virtual void once()
Execute an action exactly once.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
Virtual base class for thread that communicates with a Robotino.
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.
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.
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.
void unproject(float *vx, float *vy, float *omega, float m1, float m2, float m3) const
Project single motor speeds to velocity in cartesian coordinates.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
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.
Time now() const
Get the current time.
Configuration * config
This is the Configuration member used to access the configuration.
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.
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.
float * orientation() const
Get orientation value.
bool msgq_first_is()
Check if first message has desired type.
void msgq_pop()
Erase first message from queue.
void write()
Write from local copy into BlackBoard memory.
bool msgq_empty()
Check if queue is empty.
void msgq_flush()
Flush all messages.
void read()
Read from BlackBoard into local copy.
MessageType * msgq_first_safe(MessageType *&msg) noexcept
Get first message casted to the desired type without exceptions.
bool has_writer() const
Check if there is a writer for the interface.
bool refreshed() const
Check if data has been refreshed.
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.
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.
const char * name() const
Get name of thread.
A class for handling time.
Fawkes library namespace.
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).