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,
121 switch_if_->set_enabled(
true);
124 imu_if_->set_frame(cfg_frame_imu_.c_str());
125 imu_if_->set_linear_acceleration(0, -1.);
128 imu_if_->set_angular_velocity_covariance(0, -1.);
146 gyro_buffer_index_new_ = 0;
147 gyro_buffer_index_delayed_ = 0;
148 gyro_timestamp_buffer_ =
new fawkes::Time[gyro_buffer_size_];
149 gyro_angle_buffer_ =
new float[gyro_buffer_size_];
153 if (string_pub_->HasConnections()) {
154 msgs::Header helloMessage;
155 helloMessage.set_str_id(
"gazsim-robotino plugin connected");
156 string_pub_->Publish(helloMessage);
169 delete[] gyro_timestamp_buffer_;
170 delete[] gyro_angle_buffer_;
177 process_motor_messages();
187 if (gyro_available_) {
190 while ((now - gyro_timestamp_buffer_[(gyro_buffer_index_delayed_ + 1) % gyro_buffer_size_])
193 && gyro_buffer_index_delayed_ < gyro_buffer_index_new_) {
194 gyro_buffer_index_delayed_++;
198 tf::create_quaternion_from_yaw(gyro_angle_buffer_[gyro_buffer_index_delayed_]);
203 for (uint i = 0; i < 9u; i += 4) {
217 sens_if_->
set_distance(infrared_sensor_index_, infrared_puck_sensor_dist_);
219 if (have_gripper_sensors_) {
220 sens_if_->
set_analog_in(gripper_laser_left_pos_, analog_in_left_);
221 sens_if_->
set_analog_in(gripper_laser_right_pos_, analog_in_right_);
230 RobotinoSimThread::send_transroot(
double vx,
double vy,
double omega)
232 msgs::Vector3d motorMove;
233 motorMove.set_x(vx_);
234 motorMove.set_y(vy_);
235 motorMove.set_z(vomega_);
236 motor_move_pub_->Publish(motorMove);
240 RobotinoSimThread::process_motor_messages()
247 send_transroot(0, 0, 0);
251 send_transroot(vx_, vy_, vomega_);
263 while (motor_move_pub_->HasConnections() && !motor_if_->
msgq_empty()) {
266 if (vel_changed(msg->vx(), vx_, 0.01) || vel_changed(msg->vy(), vy_, 0.01)
267 || vel_changed(msg->omega(), vomega_, 0.01)) {
270 vomega_ = msg->omega();
273 des_vomega_ = vomega_;
276 send_transroot(vx_ * moving_speed_factor_,
277 vy_ * moving_speed_factor_,
278 vomega_ * rotation_speed_factor_);
304 RobotinoSimThread::vel_changed(
float before,
float after,
float relativeThreashold)
306 return (before == 0.0 || after == 0.0 || fabs((before - after) / before) > relativeThreashold);
311 RobotinoSimThread::on_pos_msg(ConstPosePtr &msg)
318 float new_x = msg->position().x() - x_offset_;
319 float new_y = msg->position().y() - y_offset_;
321 float new_ori = tf::get_yaw(tf::Quaternion(msg->orientation().x(),
322 msg->orientation().y(),
323 msg->orientation().z(),
324 msg->orientation().w()));
325 new_ori -= ori_offset_;
328 float length_driven = sqrt((new_x - x_) * (new_x - x_) + (new_y - y_) * (new_y - y_));
330 if (slippery_wheels_enabled_) {
333 double duration = new_time.
in_sec() - last_pos_time_.
in_sec();
335 double velocity_set_duration = new_time.
in_sec() - last_vel_set_time_.
in_sec();
337 last_pos_time_ = new_time;
339 double total_speed = sqrt(vx_ * vx_ + vy_ * vy_);
340 if (length_driven < total_speed * duration * slippery_wheels_threshold_
341 && velocity_set_duration > duration) {
342 double speed_abs_x = vx_ * cos(ori_) - vy_ * sin(ori_);
343 double speed_abs_y = vx_ * sin(ori_) + vy_ * cos(ori_);
344 double slipped_x = speed_abs_x * duration * slippery_wheels_threshold_;
345 double slipped_y = speed_abs_y * duration * slippery_wheels_threshold_;
347 new_x = x_ + slipped_x;
348 new_y = y_ + slipped_y;
350 x_offset_ -= slipped_x;
351 y_offset_ -= slipped_y;
353 length_driven = sqrt((new_x - x_) * (new_x - x_) + (new_y - y_) * (new_y - y_));
361 path_length_ += length_driven;
366 tf::Transform t(tf::Quaternion(tf::Vector3(0, 0, 1), ori_), tf::Vector3(x_, y_, 0.0));
371 RobotinoSimThread::on_gyro_msg(ConstVector3dPtr &msg)
374 gyro_buffer_index_new_ = (gyro_buffer_index_new_ + 1) % gyro_buffer_size_;
375 gyro_angle_buffer_[gyro_buffer_index_new_] = msg->z();
376 gyro_timestamp_buffer_[gyro_buffer_index_new_] =
clock->
now();
377 gyro_available_ =
true;
381 RobotinoSimThread::on_infrared_puck_sensor_msg(ConstLaserScanStampedPtr &msg)
385 infrared_puck_sensor_dist_ = msg->scan().ranges(0);
389 RobotinoSimThread::on_gripper_laser_right_sensor_msg(ConstFloatPtr &msg)
393 if (msg->value() < gripper_laser_threshold_) {
394 analog_in_right_ = gripper_laser_value_near_;
396 analog_in_right_ = gripper_laser_value_far_;
401 RobotinoSimThread::on_gripper_laser_left_sensor_msg(ConstFloatPtr &msg)
405 if (msg->value() < gripper_laser_threshold_) {
406 analog_in_left_ = gripper_laser_value_near_;
408 analog_in_left_ = gripper_laser_value_far_;