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();
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.
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.
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.
Thread class encapsulation of pthreads.
void write()
Write from local copy into BlackBoard memory.
Logger * logger
This is the Logger member used to access the logger.
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.
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.
Time now() const
Get the current time.
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
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).
void set_des_vy(const float new_des_vy)
Set des_vy value.
Base class for exceptions in Fawkes.
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.
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.
virtual void once()
Execute an action exactly once.
Virtual base class for thread that communicates with a Robotino.
bool has_writer() const
Check if there is a writer for the interface.
const char * name() const
Get name of thread.
bool msgq_first_is()
Check if first message has desired type.
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.
bool changed() const
Check if data has been changed.
IMUInterface Fawkes BlackBoard Interface.
virtual bool is_connected()=0
Check if we are connected to OpenRobotino.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
void set_drive_limits(float trans_accel, float trans_decel, float rot_accel, float rot_decel)
Set the omni drive limits.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
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.
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.
RobotinoActThread(RobotinoComThread *com_thread)
Constructor.
Configuration * config
This is the Configuration member used to access the configuration.
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.
GripperInterface Fawkes BlackBoard Interface.
virtual void close(Interface *interface)=0
Close interface.