Fawkes API  Fawkes Development Version
gazsim_robotino_thread.cpp
1 /***************************************************************************
2  * gazsim_robotino_thread.cpp - Thread simulate the Robotino in Gazebo by sending needed informations to the Robotino-plugin in Gazebo and recieving sensordata from Gazebo
3  *
4  * Created: Fr 3. Mai 21:27:06 CEST 2013
5  * Copyright 2013 Frederik Zwilling
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 #include "gazsim_robotino_thread.h"
22 
23 #include <tf/types.h>
24 #include <core/threading/mutex_locker.h>
25 
26 #include <gazebo/transport/Node.hh>
27 #include <gazebo/msgs/msgs.hh>
28 #include <gazebo/transport/transport.hh>
29 #include <aspect/logging.h>
30 
31 #include <interfaces/MotorInterface.h>
32 #include <interfaces/RobotinoSensorInterface.h>
33 #include <interfaces/SwitchInterface.h>
34 #include <interfaces/IMUInterface.h>
35 
36 #include <tf/transform_publisher.h>
37 #include <utils/time/clock.h>
38 #include <utils/math/angle.h>
39 
40 #include <cstdio>
41 #include <list>
42 
43 using namespace fawkes;
44 using namespace gazebo;
45 
46 /** @class RobotinoSimThread "gazsim_robotino_thread.h"
47  * Thread simulate the Robotino in Gazebo
48  * by sending needed informations to the Robotino-plugin in Gazebo
49  * and recieving sensordata from Gazebo
50  * @author Frederik Zwilling
51  */
52 
53 /** Constructor. */
55  : Thread("RobotinoSimThread", Thread::OPMODE_WAITFORWAKEUP),
56  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE), //sonsor and act here
57  TransformAspect(TransformAspect::DEFER_PUBLISHER)
58 {
59 }
60 
61 void
63 {
64  //get a connection to gazebo (copied from gazeboscene)
65  logger->log_debug(name(), "Creating Gazebo publishers");
66 
67  //read config values
68  cfg_frame_odom_ = config->get_string("/frames/odom");
69  cfg_frame_base_ = config->get_string("/frames/base");
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_)
80  {
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");
83  }
84  gyro_buffer_size_ = config->get_int("/gazsim/robotino/gyro-buffer-size");
85  gyro_delay_ = config->get_float("/gazsim/robotino/gyro-delay");
86  infrared_sensor_index_ = config->get_int("/gazsim/robotino/infrared-sensor-index");
87 
88  tf_enable_publisher(cfg_frame_base_.c_str());
89 
90  //Open interfaces
91  motor_if_ = blackboard->open_for_writing<MotorInterface>("Robotino");
92  switch_if_ = blackboard->open_for_writing<fawkes::SwitchInterface>("Robotino Motor");
93  sens_if_ = blackboard->open_for_writing<RobotinoSensorInterface>("Robotino");
94  imu_if_ = blackboard->open_for_writing<IMUInterface>("IMU Robotino");
95 
96  //Create suscribers
97  pos_sub_ = gazebonode->Subscribe(config->get_string("/gazsim/topics/gps"), &RobotinoSimThread::on_pos_msg, this);
98  gyro_sub_ = gazebonode->Subscribe(config->get_string("/gazsim/topics/gyro"), &RobotinoSimThread::on_gyro_msg, this);
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_)
101  {
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);
104  }
105 
106  //Create publishers
107  motor_move_pub_ = gazebonode->Advertise<msgs::Vector3d>(config->get_string("/gazsim/topics/motor-move"));
108  string_pub_ = gazebonode->Advertise<msgs::Header>(config->get_string("/gazsim/topics/message"));
109 
110  //enable motor by default
111  switch_if_->set_enabled(true);
112  switch_if_->write();
113 
114  imu_if_->set_linear_acceleration(0, -1.);
115  //imu_if_->set_angular_velocity_covariance(8, deg2rad(0.1));
116  // set as not available as we do not currently provide angular velocities.
117  imu_if_->set_angular_velocity_covariance(0, -1.);
118  imu_if_->write();
119 
120  //init motor variables
121  x_ = 0.0;
122  y_ = 0.0;
123  ori_ = 0.0;
124  vx_ = 0.0;
125  vy_ = 0.0;
126  vomega_ = 0.0;
127  des_vx_ = 0.0;
128  des_vy_ = 0.0;
129  des_vomega_ = 0.0;
130  x_offset_ = 0.0;
131  y_offset_ = 0.0;
132  ori_offset_ = 0.0;
133  path_length_ = 0.0;
134 
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_];
139 
140  new_data_ = false;
141 
142  if(string_pub_->HasConnections())
143  {
144  msgs::Header helloMessage;
145  helloMessage.set_str_id("gazsim-robotino plugin connected");
146  string_pub_->Publish(helloMessage);
147 
148  }
149 }
150 
151 void
153 {
154  //close interfaces
155  blackboard->close(imu_if_);
156  blackboard->close(sens_if_);
157  blackboard->close(motor_if_);
158  blackboard->close(switch_if_);
159 
160  delete [] gyro_timestamp_buffer_;
161  delete [] gyro_angle_buffer_;
162 }
163 
164 void
166 {
167  //work off all messages passed to the motor_interfaces
168  process_motor_messages();
169 
170  //update interfaces
171  if(new_data_)
172  {
173  motor_if_->set_odometry_position_x(x_);
174  motor_if_->set_odometry_position_y(y_);
175  motor_if_->set_odometry_orientation(ori_);
176  motor_if_->set_odometry_path_length(path_length_);
177  motor_if_->write();
178 
179  if(gyro_available_)
180  {
181  //update gyro (with delay)
182  fawkes::Time now(clock);
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_)
185  {
186  gyro_buffer_index_delayed_++;
187  }
188 
189  tf::Quaternion q =
190  tf::create_quaternion_from_yaw(gyro_angle_buffer_[gyro_buffer_index_delayed_]);
191  imu_if_->set_orientation(0, q.x());
192  imu_if_->set_orientation(1, q.y());
193  imu_if_->set_orientation(2, q.z());
194  imu_if_->set_orientation(3, q.w());
195  } else {
196  imu_if_->set_angular_velocity(0, -1.);
197  imu_if_->set_orientation(0, -1.);
198  imu_if_->set_orientation(1, 0.);
199  imu_if_->set_orientation(2, 0.);
200  imu_if_->set_orientation(3, 0.);
201  }
202  imu_if_->write();
203 
204  sens_if_->set_distance(infrared_sensor_index_, infrared_puck_sensor_dist_);
205 
206  if(have_gripper_sensors_)
207  {
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_);
210  }
211  sens_if_->write();
212 
213  new_data_ = false;
214  }
215 }
216 
217 void RobotinoSimThread::send_transroot(double vx, double vy, double omega)
218 {
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);
224 }
225 
226 void RobotinoSimThread::process_motor_messages()
227 {
228  //check messages of the switch interface
229  while (!switch_if_->msgq_empty()) {
231  switch_if_->msgq_first_safe(msg)) {
232  switch_if_->set_enabled(false);
233  //pause movement
234  send_transroot(0, 0, 0);
235  } else if (SwitchInterface::EnableSwitchMessage *msg =
236  switch_if_->msgq_first_safe(msg)) {
237  switch_if_->set_enabled(true);
238  //unpause movement
239  send_transroot(vx_, vy_, vomega_);
240  }
241  switch_if_->msgq_pop();
242  switch_if_->write();
243  }
244 
245  //do not do anything if the motor is disabled
246  if(!switch_if_->is_enabled())
247  {
248  return;
249  }
250 
251  //check messages of the motor interface
252  while(motor_move_pub_->HasConnections() && !motor_if_->msgq_empty())
253  {
255  motor_if_->msgq_first_safe(msg))
256  {
257  //send command only if changed
258  if(vel_changed(msg->vx(), vx_, 0.01) || vel_changed(msg->vy(), vy_, 0.01) || vel_changed(msg->omega(), vomega_, 0.01))
259  {
260  vx_ = msg->vx();
261  vy_ = msg->vy();
262  vomega_ = msg->omega();
263  des_vx_ = vx_;
264  des_vy_ = vy_;
265  des_vomega_ = vomega_;
266 
267  //send message to gazebo (apply movement_factor to compensate friction)
268  send_transroot(vx_ * moving_speed_factor_, vy_ * moving_speed_factor_, vomega_ * rotation_speed_factor_);
269 
270  //update interface
271  motor_if_->set_vx(vx_);
272  motor_if_->set_vy(vy_);
273  motor_if_->set_omega(vomega_);
274  motor_if_->set_des_vx(des_vx_);
275  motor_if_->set_des_vy(des_vy_);
276  motor_if_->set_des_omega(des_vomega_);
277  //update interface
278  motor_if_->write();
279  }
280  }
282  {
283  x_offset_ += x_;
284  y_offset_ += y_;
285  ori_offset_ += ori_;
286  x_ = 0.0;
287  y_ = 0.0;
288  ori_ = 0.0;
289  last_vel_set_time_ = clock->now();
290  }
291  motor_if_->msgq_pop();
292  }
293 }
294 
295 bool RobotinoSimThread::vel_changed(float before, float after, float relativeThreashold)
296 {
297  return(before == 0.0 || after == 0.0 || fabs((before-after)/before) > relativeThreashold);
298 }
299 
300 
301 //Handler Methods:
302 void RobotinoSimThread::on_pos_msg(ConstPosePtr &msg)
303 {
304  //logger_->log_debug(name_, "Got Position MSG from gazebo with ori: %f", msg->z());
305 
306  MutexLocker lock(loop_mutex);
307 
308  //read out values + substract offset
309  float new_x = msg->position().x() - x_offset_;
310  float new_y = msg->position().y() - y_offset_;
311  //calculate ori from quaternion
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_;
315 
316  //estimate path-length
317  float length_driven = sqrt((new_x-x_) * (new_x-x_) + (new_y-y_) * (new_y-y_));
318 
319  if(slippery_wheels_enabled_)
320  {
321  //simulate slipping wheels when driving against an obstacle
322  fawkes::Time new_time = clock->now();
323  double duration = new_time.in_sec() - last_pos_time_.in_sec();
324  //calculate duration since the velocity was last set to filter slipping while accelerating
325  double velocity_set_duration = new_time.in_sec() - last_vel_set_time_.in_sec();
326 
327  last_pos_time_ = new_time;
328 
329 
330  double total_speed = sqrt(vx_ * vx_ + vy_ * vy_);
331  if(length_driven < total_speed * duration * slippery_wheels_threshold_ && velocity_set_duration > duration)
332  {
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_;
337  //logger_->log_debug(name_, "Wheels are slipping (%f, %f)", slipped_x, slipped_y);
338  new_x = x_ + slipped_x;
339  new_y = y_ + slipped_y;
340  //update the offset (otherwise the slippery error would be corrected in the next iteration)
341  x_offset_ -= slipped_x;
342  y_offset_ -= slipped_y;
343 
344  length_driven = sqrt((new_x-x_) * (new_x-x_) + (new_y-y_) * (new_y-y_));
345  }
346  }
347 
348  //update stored values
349  x_ = new_x;
350  y_ = new_y;
351  ori_ = new_ori;
352  path_length_ += length_driven;
353  new_data_ = true;
354 
355  //publish transform (otherwise the transform can not convert /base_link to /odom)
356  fawkes::Time now(clock);
357  tf::Transform t(tf::Quaternion(tf::Vector3(0,0,1),ori_),
358  tf::Vector3(x_, y_, 0.0));
359 
360  tf_publisher->send_transform(t, now, cfg_frame_odom_, cfg_frame_base_);
361 }
362 void RobotinoSimThread::on_gyro_msg(ConstVector3dPtr &msg)
363 {
364  MutexLocker lock(loop_mutex);
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;
369  new_data_ = true;
370 }
371 void RobotinoSimThread::on_infrared_puck_sensor_msg(ConstLaserScanStampedPtr &msg)
372 {
373  MutexLocker lock(loop_mutex);
374  //make sure that the config values for fetch_puck are set right
375  infrared_puck_sensor_dist_ = msg->scan().ranges(0);
376  new_data_ = true;
377 }
378 void RobotinoSimThread::on_gripper_laser_right_sensor_msg(ConstFloatPtr &msg)
379 {
380  MutexLocker lock(loop_mutex);
381 
382  if(msg->value() < gripper_laser_threshold_)
383  {
384  analog_in_right_ = gripper_laser_value_near_;
385  }
386  else
387  {
388  analog_in_right_ = gripper_laser_value_far_;
389  }
390  new_data_ = true;
391 }
392 void RobotinoSimThread::on_gripper_laser_left_sensor_msg(ConstFloatPtr &msg)
393 {
394  MutexLocker lock(loop_mutex);
395 
396  if(msg->value() < gripper_laser_threshold_)
397  {
398  analog_in_left_ = gripper_laser_value_near_;
399  }
400  else
401  {
402  analog_in_left_ = gripper_laser_value_far_;
403  }
404  new_data_ = true;
405 }
void set_des_vx(const float new_des_vx)
Set des_vx value.
tf::TransformPublisher * tf_publisher
This is the transform publisher which can be used to publish transforms via the blackboard.
Definition: tf.h:71
double in_sec() const
Convet time to seconds.
Definition: time.cpp:232
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1048
Definition: gps.h:30
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.
Mutex locking helper.
Definition: mutex_locker.h:33
void set_odometry_orientation(const float new_odometry_orientation)
Set odometry_orientation value.
A class for handling time.
Definition: time.h:91
Thread class encapsulation of pthreads.
Definition: thread.h:42
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
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().
Definition: thread.h:139
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
Thread aspect to access the transform system.
Definition: tf.h:42
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.
Definition: clock.h:45
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.
Definition: clock.cpp:269
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1193
SwitchInterface Fawkes BlackBoard Interface.
virtual void init()
Initialize the thread.
void set_des_vy(const float new_des_vy)
Set des_vy value.
virtual void send_transform(const StampedTransform &transform, const bool is_static=false)
Publish transform.
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.
Definition: thread.h:95
gazebo::transport::NodePtr gazebonode
Gazebo Node for communication with a robot.
Definition: gazebo.h:47
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
RobotinoSimThread()
Constructor.
bool is_enabled() const
Get enabled value.
IMUInterface Fawkes BlackBoard Interface.
Definition: IMUInterface.h:33
EnableSwitchMessage Fawkes BlackBoard Interface Message.
void tf_enable_publisher(const char *frame_id=0)
Late enabling of publisher.
Definition: tf.cpp:156
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.
Definition: interface.h:302
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.
Definition: configurable.h:44
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.
Definition: blackboard.h:44
virtual void close(Interface *interface)=0
Close interface.