21 #include "gazsim_robotino_thread.h" 24 #include <core/threading/mutex_locker.h> 26 #include <gazebo/transport/Node.hh> 27 #include <gazebo/msgs/msgs.hh> 28 #include <gazebo/transport/transport.hh> 29 #include <aspect/logging.h> 31 #include <interfaces/MotorInterface.h> 32 #include <interfaces/RobotinoSensorInterface.h> 33 #include <interfaces/SwitchInterface.h> 34 #include <interfaces/IMUInterface.h> 36 #include <tf/transform_publisher.h> 37 #include <utils/time/clock.h> 38 #include <utils/math/angle.h> 55 :
Thread(
"RobotinoSimThread",
Thread::OPMODE_WAITFORWAKEUP),
70 slippery_wheels_enabled_ =
config->
get_bool(
"gazsim/robotino/motor/slippery-wheels-enabled");
71 slippery_wheels_threshold_ =
config->
get_float(
"gazsim/robotino/motor/slippery-wheels-threshold");
72 moving_speed_factor_ =
config->
get_float(
"gazsim/robotino/motor/moving-speed-factor");
73 rotation_speed_factor_ =
config->
get_float(
"gazsim/robotino/motor/rotation-speed-factor");
74 gripper_laser_threshold_ =
config->
get_float(
"/gazsim/robotino/gripper-laser-threshold");
75 gripper_laser_value_far_ =
config->
get_float(
"/gazsim/robotino/gripper-laser-value-far");
76 gripper_laser_value_near_ =
config->
get_float(
"/gazsim/robotino/gripper-laser-value-near");
77 have_gripper_sensors_ =
config->
exists(
"/hardware/robotino/sensors/right_ir_id")
78 &&
config->
exists(
"/hardware/robotino/sensors/left_ir_id");
79 if(have_gripper_sensors_)
81 gripper_laser_right_pos_ =
config->
get_int(
"/hardware/robotino/sensors/right_ir_id");
82 gripper_laser_left_pos_ =
config->
get_int(
"/hardware/robotino/sensors/left_ir_id");
84 gyro_buffer_size_ =
config->
get_int(
"/gazsim/robotino/gyro-buffer-size");
86 infrared_sensor_index_ =
config->
get_int(
"/gazsim/robotino/infrared-sensor-index");
99 infrared_puck_sensor_sub_ =
gazebonode->Subscribe(
config->
get_string(
"/gazsim/topics/infrared-puck-sensor"), &RobotinoSimThread::on_infrared_puck_sensor_msg,
this);
100 if(have_gripper_sensors_)
102 gripper_laser_left_sensor_sub_ =
gazebonode->Subscribe(
config->
get_string(
"/gazsim/topics/gripper-laser-left"), &RobotinoSimThread::on_gripper_laser_left_sensor_msg,
this);
103 gripper_laser_right_sensor_sub_ =
gazebonode->Subscribe(
config->
get_string(
"/gazsim/topics/gripper-laser-right"), &RobotinoSimThread::on_gripper_laser_right_sensor_msg,
this);
111 switch_if_->set_enabled(
true);
114 imu_if_->set_linear_acceleration(0, -1.);
117 imu_if_->set_angular_velocity_covariance(0, -1.);
135 gyro_buffer_index_new_ = 0;
136 gyro_buffer_index_delayed_ = 0;
137 gyro_timestamp_buffer_ =
new fawkes::Time[gyro_buffer_size_];
138 gyro_angle_buffer_ =
new float[gyro_buffer_size_];
142 if(string_pub_->HasConnections())
144 msgs::Header helloMessage;
145 helloMessage.set_str_id(
"gazsim-robotino plugin connected");
146 string_pub_->Publish(helloMessage);
160 delete [] gyro_timestamp_buffer_;
161 delete [] gyro_angle_buffer_;
168 process_motor_messages();
183 while ((now - gyro_timestamp_buffer_[(gyro_buffer_index_delayed_ + 1) % gyro_buffer_size_]).in_sec() >= gyro_delay_
184 && gyro_buffer_index_delayed_ < gyro_buffer_index_new_)
186 gyro_buffer_index_delayed_++;
190 tf::create_quaternion_from_yaw(gyro_angle_buffer_[gyro_buffer_index_delayed_]);
204 sens_if_->
set_distance(infrared_sensor_index_, infrared_puck_sensor_dist_);
206 if(have_gripper_sensors_)
208 sens_if_->
set_analog_in(gripper_laser_left_pos_, analog_in_left_);
209 sens_if_->
set_analog_in(gripper_laser_right_pos_, analog_in_right_);
217 void RobotinoSimThread::send_transroot(
double vx,
double vy,
double omega)
219 msgs::Vector3d motorMove;
220 motorMove.set_x(vx_);
221 motorMove.set_y(vy_);
222 motorMove.set_z(vomega_);
223 motor_move_pub_->Publish(motorMove);
226 void RobotinoSimThread::process_motor_messages()
234 send_transroot(0, 0, 0);
239 send_transroot(vx_, vy_, vomega_);
252 while(motor_move_pub_->HasConnections() && !motor_if_->
msgq_empty())
258 if(vel_changed(msg->vx(), vx_, 0.01) || vel_changed(msg->vy(), vy_, 0.01) || vel_changed(msg->omega(), vomega_, 0.01))
262 vomega_ = msg->omega();
265 des_vomega_ = vomega_;
268 send_transroot(vx_ * moving_speed_factor_, vy_ * moving_speed_factor_, vomega_ * rotation_speed_factor_);
295 bool RobotinoSimThread::vel_changed(
float before,
float after,
float relativeThreashold)
297 return(before == 0.0 || after == 0.0 || fabs((before-after)/before) > relativeThreashold);
302 void RobotinoSimThread::on_pos_msg(ConstPosePtr &msg)
309 float new_x = msg->position().x() - x_offset_;
310 float new_y = msg->position().y() - y_offset_;
312 float new_ori = tf::get_yaw(tf::Quaternion(msg->orientation().x(), msg->orientation().y()
313 , msg->orientation().z(), msg->orientation().w()));
314 new_ori -= ori_offset_;
317 float length_driven = sqrt((new_x-x_) * (new_x-x_) + (new_y-y_) * (new_y-y_));
319 if(slippery_wheels_enabled_)
323 double duration = new_time.
in_sec() - last_pos_time_.
in_sec();
325 double velocity_set_duration = new_time.
in_sec() - last_vel_set_time_.
in_sec();
327 last_pos_time_ = new_time;
330 double total_speed = sqrt(vx_ * vx_ + vy_ * vy_);
331 if(length_driven < total_speed * duration * slippery_wheels_threshold_ && velocity_set_duration > duration)
333 double speed_abs_x = vx_ * cos(ori_) - vy_ * sin(ori_);
334 double speed_abs_y = vx_ * sin(ori_) + vy_ * cos(ori_);
335 double slipped_x = speed_abs_x * duration * slippery_wheels_threshold_;
336 double slipped_y = speed_abs_y * duration * slippery_wheels_threshold_;
338 new_x = x_ + slipped_x;
339 new_y = y_ + slipped_y;
341 x_offset_ -= slipped_x;
342 y_offset_ -= slipped_y;
344 length_driven = sqrt((new_x-x_) * (new_x-x_) + (new_y-y_) * (new_y-y_));
352 path_length_ += length_driven;
357 tf::Transform t(tf::Quaternion(tf::Vector3(0,0,1),ori_),
358 tf::Vector3(x_, y_, 0.0));
362 void RobotinoSimThread::on_gyro_msg(ConstVector3dPtr &msg)
365 gyro_buffer_index_new_ = (gyro_buffer_index_new_ + 1) % gyro_buffer_size_;
366 gyro_angle_buffer_[gyro_buffer_index_new_] = msg->z();
367 gyro_timestamp_buffer_[gyro_buffer_index_new_] =
clock->
now();
368 gyro_available_ =
true;
371 void RobotinoSimThread::on_infrared_puck_sensor_msg(ConstLaserScanStampedPtr &msg)
375 infrared_puck_sensor_dist_ = msg->scan().ranges(0);
378 void RobotinoSimThread::on_gripper_laser_right_sensor_msg(ConstFloatPtr &msg)
382 if(msg->value() < gripper_laser_threshold_)
384 analog_in_right_ = gripper_laser_value_near_;
388 analog_in_right_ = gripper_laser_value_far_;
392 void RobotinoSimThread::on_gripper_laser_left_sensor_msg(ConstFloatPtr &msg)
396 if(msg->value() < gripper_laser_threshold_)
398 analog_in_left_ = gripper_laser_value_near_;
402 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_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_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.
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.