21 #include "gazsim_robotino_thread.h"
23 #include <aspect/logging.h>
24 #include <core/threading/mutex_locker.h>
25 #include <interfaces/IMUInterface.h>
26 #include <interfaces/MotorInterface.h>
27 #include <interfaces/RobotinoSensorInterface.h>
28 #include <interfaces/SwitchInterface.h>
29 #include <tf/transform_publisher.h>
31 #include <utils/math/angle.h>
32 #include <utils/time/clock.h>
35 #include <gazebo/msgs/msgs.hh>
36 #include <gazebo/transport/Node.hh>
37 #include <gazebo/transport/transport.hh>
41 using namespace gazebo;
52 :
Thread(
"RobotinoSimThread",
Thread::OPMODE_WAITFORWAKEUP),
68 slippery_wheels_enabled_ =
config->
get_bool(
"gazsim/robotino/motor/slippery-wheels-enabled");
69 slippery_wheels_threshold_ =
config->
get_float(
"gazsim/robotino/motor/slippery-wheels-threshold");
70 moving_speed_factor_ =
config->
get_float(
"gazsim/robotino/motor/moving-speed-factor");
71 rotation_speed_factor_ =
config->
get_float(
"gazsim/robotino/motor/rotation-speed-factor");
72 gripper_laser_threshold_ =
config->
get_float(
"/gazsim/robotino/gripper-laser-threshold");
73 gripper_laser_value_far_ =
config->
get_float(
"/gazsim/robotino/gripper-laser-value-far");
74 gripper_laser_value_near_ =
config->
get_float(
"/gazsim/robotino/gripper-laser-value-near");
75 have_gripper_sensors_ =
config->
exists(
"/hardware/robotino/sensors/right_ir_id")
76 &&
config->
exists(
"/hardware/robotino/sensors/left_ir_id");
77 if (have_gripper_sensors_) {
78 gripper_laser_right_pos_ =
config->
get_int(
"/hardware/robotino/sensors/right_ir_id");
79 gripper_laser_left_pos_ =
config->
get_int(
"/hardware/robotino/sensors/left_ir_id");
81 gyro_buffer_size_ =
config->
get_int(
"/gazsim/robotino/gyro-buffer-size");
83 infrared_sensor_index_ =
config->
get_int(
"/gazsim/robotino/infrared-sensor-index");
95 &RobotinoSimThread::on_pos_msg,
98 &RobotinoSimThread::on_gyro_msg,
100 infrared_puck_sensor_sub_ =
102 &RobotinoSimThread::on_infrared_puck_sensor_msg,
104 if (have_gripper_sensors_) {
105 gripper_laser_left_sensor_sub_ =
107 &RobotinoSimThread::on_gripper_laser_left_sensor_msg,
109 gripper_laser_right_sensor_sub_ =
111 &RobotinoSimThread::on_gripper_laser_right_sensor_msg,
115 gripper_has_puck_sub_ =
117 &RobotinoSimThread::on_gripper_has_puck_msg,
126 switch_if_->set_enabled(
true);
130 sens_if_->set_digital_in(0,
false);
131 sens_if_->set_digital_in(1,
false);
134 imu_if_->set_frame(cfg_frame_imu_.c_str());
135 imu_if_->set_linear_acceleration(0, -1.);
138 imu_if_->set_angular_velocity_covariance(0, -1.);
156 gyro_buffer_index_new_ = 0;
157 gyro_buffer_index_delayed_ = 0;
158 gyro_timestamp_buffer_ =
new fawkes::Time[gyro_buffer_size_];
159 gyro_angle_buffer_ =
new float[gyro_buffer_size_];
163 if (string_pub_->HasConnections()) {
164 msgs::Header helloMessage;
165 helloMessage.set_str_id(
"gazsim-robotino plugin connected");
166 string_pub_->Publish(helloMessage);
179 delete[] gyro_timestamp_buffer_;
180 delete[] gyro_angle_buffer_;
187 process_motor_messages();
197 if (gyro_available_) {
200 while ((now - gyro_timestamp_buffer_[(gyro_buffer_index_delayed_ + 1) % gyro_buffer_size_])
203 && gyro_buffer_index_delayed_ < gyro_buffer_index_new_) {
204 gyro_buffer_index_delayed_++;
208 tf::create_quaternion_from_yaw(gyro_angle_buffer_[gyro_buffer_index_delayed_]);
213 for (uint i = 0; i < 9u; i += 4) {
227 sens_if_->
set_distance(infrared_sensor_index_, infrared_puck_sensor_dist_);
229 if (have_gripper_sensors_) {
230 sens_if_->
set_analog_in(gripper_laser_left_pos_, analog_in_left_);
231 sens_if_->
set_analog_in(gripper_laser_right_pos_, analog_in_right_);
240 RobotinoSimThread::send_transroot(
double vx,
double vy,
double omega)
242 msgs::Vector3d motorMove;
243 motorMove.set_x(vx_);
244 motorMove.set_y(vy_);
245 motorMove.set_z(vomega_);
246 motor_move_pub_->Publish(motorMove);
250 RobotinoSimThread::process_motor_messages()
257 send_transroot(0, 0, 0);
261 send_transroot(vx_, vy_, vomega_);
273 while (motor_move_pub_->HasConnections() && !motor_if_->
msgq_empty()) {
276 if (vel_changed(msg->vx(), vx_, 0.01) || vel_changed(msg->vy(), vy_, 0.01)
277 || vel_changed(msg->omega(), vomega_, 0.01)) {
280 vomega_ = msg->omega();
283 des_vomega_ = vomega_;
286 send_transroot(vx_ * moving_speed_factor_,
287 vy_ * moving_speed_factor_,
288 vomega_ * rotation_speed_factor_);
314 RobotinoSimThread::vel_changed(
float before,
float after,
float relativeThreashold)
316 return (before == 0.0 || after == 0.0 || fabs((before - after) / before) > relativeThreashold);
321 RobotinoSimThread::on_pos_msg(ConstPosePtr &msg)
328 float new_x = msg->position().x() - x_offset_;
329 float new_y = msg->position().y() - y_offset_;
331 float new_ori = tf::get_yaw(tf::Quaternion(msg->orientation().x(),
332 msg->orientation().y(),
333 msg->orientation().z(),
334 msg->orientation().w()));
335 new_ori -= ori_offset_;
338 float length_driven = sqrt((new_x - x_) * (new_x - x_) + (new_y - y_) * (new_y - y_));
340 if (slippery_wheels_enabled_) {
343 double duration = new_time.
in_sec() - last_pos_time_.
in_sec();
345 double velocity_set_duration = new_time.
in_sec() - last_vel_set_time_.
in_sec();
347 last_pos_time_ = new_time;
349 double total_speed = sqrt(vx_ * vx_ + vy_ * vy_);
350 if (length_driven < total_speed * duration * slippery_wheels_threshold_
351 && velocity_set_duration > duration) {
352 double speed_abs_x = vx_ * cos(ori_) - vy_ * sin(ori_);
353 double speed_abs_y = vx_ * sin(ori_) + vy_ * cos(ori_);
354 double slipped_x = speed_abs_x * duration * slippery_wheels_threshold_;
355 double slipped_y = speed_abs_y * duration * slippery_wheels_threshold_;
357 new_x = x_ + slipped_x;
358 new_y = y_ + slipped_y;
360 x_offset_ -= slipped_x;
361 y_offset_ -= slipped_y;
363 length_driven = sqrt((new_x - x_) * (new_x - x_) + (new_y - y_) * (new_y - y_));
371 path_length_ += length_driven;
376 tf::Transform t(tf::Quaternion(tf::Vector3(0, 0, 1), ori_), tf::Vector3(x_, y_, 0.0));
381 RobotinoSimThread::on_gyro_msg(ConstVector3dPtr &msg)
384 gyro_buffer_index_new_ = (gyro_buffer_index_new_ + 1) % gyro_buffer_size_;
385 gyro_angle_buffer_[gyro_buffer_index_new_] = msg->z();
386 gyro_timestamp_buffer_[gyro_buffer_index_new_] =
clock->
now();
387 gyro_available_ =
true;
391 RobotinoSimThread::on_infrared_puck_sensor_msg(ConstLaserScanStampedPtr &msg)
395 infrared_puck_sensor_dist_ = msg->scan().ranges(0);
399 RobotinoSimThread::on_gripper_laser_right_sensor_msg(ConstFloatPtr &msg)
403 if (msg->value() < gripper_laser_threshold_) {
404 analog_in_right_ = gripper_laser_value_near_;
406 analog_in_right_ = gripper_laser_value_far_;
411 RobotinoSimThread::on_gripper_laser_left_sensor_msg(ConstFloatPtr &msg)
415 if (msg->value() < gripper_laser_threshold_) {
416 analog_in_left_ = gripper_laser_value_near_;
418 analog_in_left_ = gripper_laser_value_far_;
424 RobotinoSimThread::on_gripper_has_puck_msg(ConstIntPtr &msg)
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
RobotinoSimThread()
Constructor.
virtual void finalize()
Finalize the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
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 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 bool exists(const char *path)=0
Check if a given value exists.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
gazebo::transport::NodePtr gazebonode
Gazebo Node for communication with a robot.
IMUInterface Fawkes BlackBoard Interface.
void set_angular_velocity_covariance(unsigned int index, const double new_angular_velocity_covariance)
Set angular_velocity_covariance value at given index.
void set_orientation_covariance(unsigned int index, const double new_orientation_covariance)
Set orientation_covariance value at given index.
void set_orientation(unsigned int index, const float new_orientation)
Set orientation value at given index.
void set_angular_velocity(unsigned int index, const float new_angular_velocity)
Set angular_velocity value at given index.
void set_linear_acceleration_covariance(unsigned int index, const double new_linear_acceleration_covariance)
Set linear_acceleration_covariance value at given index.
bool msgq_first_is()
Check if first message has desired type.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
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.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
Logger * logger
This is the Logger member used to access the logger.
ResetOdometryMessage 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_path_length(const float new_odometry_path_length)
Set odometry_path_length value.
void set_odometry_orientation(const float new_odometry_orientation)
Set odometry_orientation 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.
RobotinoSensorInterface Fawkes BlackBoard Interface.
void set_digital_in(unsigned int index, const bool new_digital_in)
Set digital_in value at given index.
void set_analog_in(unsigned int index, const float new_analog_in)
Set analog_in value at given index.
void set_distance(unsigned int index, const float new_distance)
Set distance value at given index.
DisableSwitchMessage Fawkes BlackBoard Interface Message.
EnableSwitchMessage Fawkes BlackBoard Interface Message.
SwitchInterface Fawkes BlackBoard Interface.
void set_enabled(const bool new_enabled)
Set enabled value.
bool is_enabled() const
Get enabled value.
Thread class encapsulation of pthreads.
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
const char * name() const
Get name of thread.
A class for handling time.
double in_sec() const
Convet time to seconds.
Fawkes library namespace.