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_;
void set_des_vx(const float new_des_vx)
Set des_vx value.
double in_sec() const
Convet time to seconds.
bool msgq_empty()
Check if queue is empty.
virtual void loop()
Code to execute in the thread.
TransRotMessage Fawkes BlackBoard Interface Message.
void set_odometry_position_y(const float new_odometry_position_y)
Set odometry_position_y value.
void set_orientation(unsigned int index, const float new_orientation)
Set orientation value at given index.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
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.
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
Logger * logger
This is the Logger member used to access the logger.
void set_linear_acceleration_covariance(unsigned int index, const double new_linear_acceleration_covariance)
Set linear_acceleration_covariance value at given index.
void set_des_omega(const float new_des_omega)
Set des_omega value.
void set_angular_velocity(unsigned int index, const float new_angular_velocity)
Set angular_velocity value at given index.
void set_analog_in(unsigned int index, const float new_analog_in)
Set analog_in value at given index.
ResetOdometryMessage Fawkes BlackBoard Interface Message.
Clock * clock
By means of this member access to the clock is given.
void set_odometry_path_length(const float new_odometry_path_length)
Set odometry_path_length value.
RobotinoSensorInterface Fawkes BlackBoard Interface.
Time now() const
Get the current time.
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
SwitchInterface Fawkes BlackBoard Interface.
virtual void init()
Initialize the thread.
void set_des_vy(const float new_des_vy)
Set des_vy value.
void set_orientation_covariance(unsigned int index, const double new_orientation_covariance)
Set orientation_covariance value at given index.
void set_vy(const float new_vy)
Set vy value.
void set_vx(const float new_vx)
Set vx value.
void set_enabled(const bool new_enabled)
Set enabled value.
void set_omega(const float new_omega)
Set omega value.
void set_angular_velocity_covariance(unsigned int index, const double new_angular_velocity_covariance)
Set angular_velocity_covariance value at given index.
DisableSwitchMessage Fawkes BlackBoard Interface Message.
const char * name() const
Get name of thread.
gazebo::transport::NodePtr gazebonode
Gazebo Node for communication with a robot.
bool msgq_first_is()
Check if first message has desired type.
RobotinoSimThread()
Constructor.
bool is_enabled() const
Get enabled value.
IMUInterface Fawkes BlackBoard Interface.
EnableSwitchMessage Fawkes BlackBoard Interface Message.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
void set_distance(unsigned int index, const float new_distance)
Set distance value at given index.
void set_odometry_position_x(const float new_odometry_position_x)
Set odometry_position_x value.
virtual bool exists(const char *path)=0
Check if a given value exists.
MotorInterface Fawkes BlackBoard Interface.
virtual void finalize()
Finalize the thread.
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.
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.
virtual void close(Interface *interface)=0
Close interface.