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>
41using 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");
97 pos_sub_ =
gazebonode->Subscribe(gps_topic, &RobotinoSimThread::on_pos_msg,
this);
103 gyro_sub_ =
gazebonode->Subscribe(gyro_topic, &RobotinoSimThread::on_gyro_msg,
this);
107 const std::string puck_sensor_topic =
config->
get_string(
"/gazsim/topics/infrared-puck-sensor");
108 logger->
log_info(
name(),
"Subscribing to puck sensor topic '%s'", puck_sensor_topic.c_str());
109 infrared_puck_sensor_sub_ =
111 &RobotinoSimThread::on_infrared_puck_sensor_msg,
115 if (have_gripper_sensors_) {
117 const std::string left_gripper_sensor_topic =
120 "Subscribing to left gripper topic '%s'",
121 left_gripper_sensor_topic.c_str());
122 gripper_laser_left_sensor_sub_ =
123 gazebonode->Subscribe(left_gripper_sensor_topic,
124 &RobotinoSimThread::on_gripper_laser_left_sensor_msg,
129 const std::string right_gripper_sensor_topic =
132 "Subscribing to right gripper topic '%s'",
133 right_gripper_sensor_topic.c_str());
134 gripper_laser_right_sensor_sub_ =
135 gazebonode->Subscribe(right_gripper_sensor_topic,
136 &RobotinoSimThread::on_gripper_laser_right_sensor_msg,
142 const std::string has_puck_topic =
config->
get_string(
"/gazsim/topics/gripper-has-puck");
143 logger->
log_info(
name(),
"Subscribing to has-puck topic '%s'", has_puck_topic.c_str());
144 gripper_has_puck_sub_ =
145 gazebonode->Subscribe(has_puck_topic, &RobotinoSimThread::on_gripper_has_puck_msg,
this);
149 const std::string motor_topic =
config->
get_string(
"/gazsim/topics/motor-move");
151 motor_move_pub_ =
gazebonode->Advertise<msgs::Vector3d>(motor_topic);
155 switch_if_->set_enabled(
true);
159 sens_if_->set_digital_in(0,
false);
160 sens_if_->set_digital_in(1,
false);
163 imu_if_->set_frame(cfg_frame_imu_.c_str());
164 imu_if_->set_linear_acceleration(0, -1.);
167 imu_if_->set_angular_velocity_covariance(0, -1.);
185 gyro_buffer_index_new_ = 0;
186 gyro_buffer_index_delayed_ = 0;
187 gyro_timestamp_buffer_ =
new fawkes::Time[gyro_buffer_size_];
188 gyro_angle_buffer_ =
new float[gyro_buffer_size_];
192 if (string_pub_->HasConnections()) {
193 msgs::Header helloMessage;
194 helloMessage.set_str_id(
"gazsim-robotino plugin connected");
195 string_pub_->Publish(helloMessage);
208 delete[] gyro_timestamp_buffer_;
209 delete[] gyro_angle_buffer_;
216 process_motor_messages();
226 if (gyro_available_) {
229 while ((now - gyro_timestamp_buffer_[(gyro_buffer_index_delayed_ + 1) % gyro_buffer_size_])
232 && gyro_buffer_index_delayed_ < gyro_buffer_index_new_) {
233 gyro_buffer_index_delayed_++;
237 tf::create_quaternion_from_yaw(gyro_angle_buffer_[gyro_buffer_index_delayed_]);
242 for (uint i = 0; i < 9u; i += 4) {
256 sens_if_->
set_distance(infrared_sensor_index_, infrared_puck_sensor_dist_);
258 if (have_gripper_sensors_) {
259 sens_if_->
set_analog_in(gripper_laser_left_pos_, analog_in_left_);
260 sens_if_->
set_analog_in(gripper_laser_right_pos_, analog_in_right_);
269RobotinoSimThread::send_transroot(
double vx,
double vy,
double omega)
271 msgs::Vector3d motorMove;
272 motorMove.set_x(vx_);
273 motorMove.set_y(vy_);
274 motorMove.set_z(vomega_);
275 motor_move_pub_->Publish(motorMove);
279RobotinoSimThread::process_motor_messages()
286 send_transroot(0, 0, 0);
290 send_transroot(vx_, vy_, vomega_);
302 while (motor_move_pub_->HasConnections() && !motor_if_->
msgq_empty()) {
305 if (vel_changed(msg->vx(), vx_, 0.01) || vel_changed(msg->vy(), vy_, 0.01)
306 || vel_changed(msg->omega(), vomega_, 0.01)) {
309 vomega_ = msg->omega();
312 des_vomega_ = vomega_;
315 send_transroot(vx_ * moving_speed_factor_,
316 vy_ * moving_speed_factor_,
317 vomega_ * rotation_speed_factor_);
343RobotinoSimThread::vel_changed(
float before,
float after,
float relativeThreashold)
345 return (before == 0.0 || after == 0.0 || fabs((before - after) / before) > relativeThreashold);
350RobotinoSimThread::on_pos_msg(ConstPosePtr &msg)
357 float new_x = msg->position().x() - x_offset_;
358 float new_y = msg->position().y() - y_offset_;
360 float new_ori = tf::get_yaw(tf::Quaternion(msg->orientation().x(),
361 msg->orientation().y(),
362 msg->orientation().z(),
363 msg->orientation().w()));
364 new_ori -= ori_offset_;
367 float length_driven = sqrt((new_x - x_) * (new_x - x_) + (new_y - y_) * (new_y - y_));
369 if (slippery_wheels_enabled_) {
372 double duration = new_time.
in_sec() - last_pos_time_.
in_sec();
374 double velocity_set_duration = new_time.
in_sec() - last_vel_set_time_.
in_sec();
376 last_pos_time_ = new_time;
378 double total_speed = sqrt(vx_ * vx_ + vy_ * vy_);
379 if (length_driven < total_speed * duration * slippery_wheels_threshold_
380 && velocity_set_duration > duration) {
381 double speed_abs_x = vx_ * cos(ori_) - vy_ * sin(ori_);
382 double speed_abs_y = vx_ * sin(ori_) + vy_ * cos(ori_);
383 double slipped_x = speed_abs_x * duration * slippery_wheels_threshold_;
384 double slipped_y = speed_abs_y * duration * slippery_wheels_threshold_;
386 new_x = x_ + slipped_x;
387 new_y = y_ + slipped_y;
389 x_offset_ -= slipped_x;
390 y_offset_ -= slipped_y;
392 length_driven = sqrt((new_x - x_) * (new_x - x_) + (new_y - y_) * (new_y - y_));
400 path_length_ += length_driven;
405 tf::Transform t(tf::Quaternion(tf::Vector3(0, 0, 1), ori_), tf::Vector3(x_, y_, 0.0));
410RobotinoSimThread::on_gyro_msg(ConstVector3dPtr &msg)
413 gyro_buffer_index_new_ = (gyro_buffer_index_new_ + 1) % gyro_buffer_size_;
414 gyro_angle_buffer_[gyro_buffer_index_new_] = msg->z();
415 gyro_timestamp_buffer_[gyro_buffer_index_new_] =
clock->
now();
416 gyro_available_ =
true;
420RobotinoSimThread::on_infrared_puck_sensor_msg(ConstLaserScanStampedPtr &msg)
424 infrared_puck_sensor_dist_ = msg->scan().ranges(0);
428RobotinoSimThread::on_gripper_laser_right_sensor_msg(ConstFloatPtr &msg)
432 if (msg->value() < gripper_laser_threshold_) {
433 analog_in_right_ = gripper_laser_value_near_;
435 analog_in_right_ = gripper_laser_value_far_;
440RobotinoSimThread::on_gripper_laser_left_sensor_msg(ConstFloatPtr &msg)
444 if (msg->value() < gripper_laser_threshold_) {
445 analog_in_left_ = gripper_laser_value_near_;
447 analog_in_left_ = gripper_laser_value_far_;
453RobotinoSimThread::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.
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.
MessageType * msgq_first_safe(MessageType *&msg) noexcept
Get first message casted to the desired type without exceptions.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational 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.