24 #include "act_thread.h" 25 #include "com_thread.h" 27 #include <interfaces/MotorInterface.h> 28 #include <interfaces/GripperInterface.h> 29 #include <interfaces/IMUInterface.h> 30 #include <utils/math/angle.h> 45 :
Thread(
"RobotinoActThread",
Thread::OPMODE_WAITFORWAKEUP),
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");
68 std::string odom_mode =
config->
get_string(
"/hardware/robotino/odometry/mode");
71 cfg_odom_corr_trans_ =
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");
85 cfg_rot_accel_, cfg_rot_decel_);
87 std::string imu_if_id;
90 if (odom_mode ==
"copy") {
91 cfg_odom_mode_ = ODOM_COPY;
92 }
else if (odom_mode ==
"calc") {
93 cfg_odom_mode_ = ODOM_CALC;
96 cfg_imu_deadman_loops_ =
97 config->
get_uint(
"/hardware/robotino/odometry/calc/imu_deadman_loops");
99 throw Exception(
"Invalid odometry mode '%s', must be calc or copy", odom_mode.c_str());
102 gripper_close_ =
false;
104 msg_received_ =
false;
105 msg_zero_vel_ =
false;
107 odom_x_ = odom_y_ = odom_phi_ = 0.;
113 if (cfg_odom_mode_ == ODOM_CALC) {
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;
163 bool reset_odometry =
false;
164 bool set_des_vel =
false;
169 msg->motor_state() == MotorInterface::MOTOR_ENABLED
174 des_vx_ = des_vy_ = des_omega_ = 0.;
182 des_omega_ = msg->omega();
185 msg_received_ =
true;
188 msg_zero_vel_ = (des_vx_ == 0.0 && des_vy_ == 0.0 && des_omega_ == 0.0);
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());
198 odom_x_ = odom_y_ = odom_phi_ = 0.;
201 odom_gyro_origin_ = tf::get_yaw(imu_if_->
orientation());
204 reset_odometry =
true;
210 if (cfg_gripper_enabled_) {
211 bool open_gripper =
false, close_gripper =
false;
214 close_gripper =
false;
217 close_gripper =
true;
218 open_gripper =
false;
224 if (close_gripper || open_gripper) {
225 gripper_close_ = close_gripper;
233 double diff = (
clock->
now() - (&last_msg_time_) );
234 if (diff >= cfg_deadman_threshold_ && msg_received_ && !msg_zero_vel_) {
236 "(%f sec.), motion planner alive?", diff);
237 des_vx_ = des_vy_ = des_omega_ = 0.;
238 msg_zero_vel_ =
true;
240 msg_received_ =
false;
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))) {
247 des_vx_ = des_vy_ = des_omega_ = 0.;
256 if (cfg_gripper_enabled_) {
264 RobotinoActThread::publish_odometry()
267 float a1 = 0., a2 = 0., a3 = 0.;
268 unsigned int seq = 0;
271 if (seq != last_seqnum_) {
274 float vx = 0., vy = 0., omega = 0.;
275 com_->
unproject(&vx, &vy, &omega, a1, a2, a3);
285 if (cfg_odom_mode_ == ODOM_COPY) {
292 float diff_sec = sensor_time - odom_time_;
293 *odom_time_ = sensor_time;
303 tf::Quaternion q(ori_q[0], ori_q[1], ori_q[2], ori_q[3]);
304 tf::assert_quaternion_valid(q);
307 imu_if_nochange_loops_ = 0;
309 if (imu_if_writer_warning_printed_ ||
310 imu_if_invquat_warning_printed_ ||
311 imu_if_changed_warning_printed_)
313 float old_odom_gyro_origin = odom_gyro_origin_;
321 odom_gyro_origin_ = tf::get_yaw(q) - wheel_odom_phi;
323 if (imu_if_writer_warning_printed_) {
324 imu_if_writer_warning_printed_ =
false;
326 "adjusted origin to %f (was %f)",
327 odom_gyro_origin_, old_odom_gyro_origin);
330 if (imu_if_changed_warning_printed_) {
331 imu_if_changed_warning_printed_ =
false;
333 "adjusted origin to %f (was %f)",
334 odom_gyro_origin_, old_odom_gyro_origin);
336 if (imu_if_invquat_warning_printed_) {
337 imu_if_invquat_warning_printed_ =
false;
340 "adjusted origin to %f (was %f)",
341 odom_gyro_origin_, old_odom_gyro_origin);
352 if (! imu_if_invquat_warning_printed_) {
353 imu_if_invquat_warning_printed_ =
true;
355 "falling back to wheel odometry",
356 ori_q[0], ori_q[1], ori_q[2], ori_q[3]);
362 if (++imu_if_nochange_loops_ > cfg_imu_deadman_loops_) {
363 if (! imu_if_changed_warning_printed_) {
364 imu_if_changed_warning_printed_ =
true;
366 "falling back to wheel odometry");
373 if (! imu_if_writer_warning_printed_) {
375 "using wheel odometry only");
376 imu_if_writer_warning_printed_ =
true;
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_;
394 tf::Transform t(tf::Quaternion(tf::Vector3(0,0,1), odom_phi_),
395 tf::Vector3(odom_x_, odom_y_, 0.));
398 tf_publisher->send_transform(t, sensor_time + cfg_odom_time_offset_,
399 cfg_odom_frame_, cfg_base_frame_);
402 "(%f,%f,%f), exception follows",
403 odom_x_, odom_y_, odom_phi_);
411 RobotinoActThread::publish_gripper()
415 gripper_if_->
write();
418 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.
virtual void get_odometry(double &x, double &y, double &phi)=0
Get latest odometry value.
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.