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();