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 <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>
30#include <tf/types.h>
31#include <utils/math/angle.h>
32#include <utils/time/clock.h>
33
34#include <cstdio>
35#include <gazebo/msgs/msgs.hh>
36#include <gazebo/transport/Node.hh>
37#include <gazebo/transport/transport.hh>
38#include <list>
39
40using namespace fawkes;
41using namespace gazebo;
42
43/** @class RobotinoSimThread "gazsim_robotino_thread.h"
44 * Thread simulate the Robotino in Gazebo
45 * by sending needed informations to the Robotino-plugin in Gazebo
46 * and recieving sensordata from Gazebo
47 * @author Frederik Zwilling
48 */
49
50/** Constructor. */
52: Thread("RobotinoSimThread", Thread::OPMODE_WAITFORWAKEUP),
53 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE), //sonsor and act here
54 TransformAspect(TransformAspect::DEFER_PUBLISHER)
55{
56}
57
58void
60{
61 //get a connection to gazebo (copied from gazeboscene)
62 logger->log_debug(name(), "Creating Gazebo publishers");
63
64 //read config values
65 cfg_frame_odom_ = config->get_string("/frames/odom");
66 cfg_frame_base_ = config->get_string("/frames/base");
67 cfg_frame_imu_ = config->get_string("/gazsim/robotino/imu/frame");
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");
80 }
81 gyro_buffer_size_ = config->get_int("/gazsim/robotino/gyro-buffer-size");
82 gyro_delay_ = config->get_float("/gazsim/robotino/gyro-delay");
83 infrared_sensor_index_ = config->get_int("/gazsim/robotino/infrared-sensor-index");
84
85 tf_enable_publisher(cfg_frame_base_.c_str());
86
87 //Open interfaces
88 motor_if_ = blackboard->open_for_writing<MotorInterface>("Robotino");
89 switch_if_ = blackboard->open_for_writing<fawkes::SwitchInterface>("Robotino Motor");
91 imu_if_ = blackboard->open_for_writing<IMUInterface>("IMU Robotino");
92
93 //Create suscribers
94 {
95 const std::string gps_topic = config->get_string("/gazsim/topics/gps");
96 logger->log_info(name(), "Subscribing to GPS topic '%s'", gps_topic.c_str());
97 pos_sub_ = gazebonode->Subscribe(gps_topic, &RobotinoSimThread::on_pos_msg, this);
98 }
99
100 {
101 const std::string gyro_topic = config->get_string("/gazsim/topics/gyro");
102 logger->log_info(name(), "Subscribing to Gyro topic '%s'", gyro_topic.c_str());
103 gyro_sub_ = gazebonode->Subscribe(gyro_topic, &RobotinoSimThread::on_gyro_msg, this);
104 }
105
106 {
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_ =
110 gazebonode->Subscribe(puck_sensor_topic,
111 &RobotinoSimThread::on_infrared_puck_sensor_msg,
112 this);
113 }
114
115 if (have_gripper_sensors_) {
116 {
117 const std::string left_gripper_sensor_topic =
118 config->get_string("/gazsim/topics/gripper-laser-left");
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,
125 this);
126 }
127
128 {
129 const std::string right_gripper_sensor_topic =
130 config->get_string("/gazsim/topics/gripper-laser-right");
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,
137 this);
138 }
139 }
140
141 {
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);
146 }
147
148 //Create publishers
149 const std::string motor_topic = config->get_string("/gazsim/topics/motor-move");
150 logger->log_info(name(), "Publishing to motor topic '%s'", motor_topic.c_str());
151 motor_move_pub_ = gazebonode->Advertise<msgs::Vector3d>(motor_topic);
152 string_pub_ = gazebonode->Advertise<msgs::Header>(config->get_string("/gazsim/topics/message"));
153
154 //enable motor by default
155 switch_if_->set_enabled(true);
156 switch_if_->write();
157
158 // puck sensor connected to first 2 inputs of RobotinoInterface
159 sens_if_->set_digital_in(0, false);
160 sens_if_->set_digital_in(1, false);
161 sens_if_->write();
162
163 imu_if_->set_frame(cfg_frame_imu_.c_str());
164 imu_if_->set_linear_acceleration(0, -1.);
165 //imu_if_->set_angular_velocity_covariance(8, deg2rad(0.1));
166 // set as not available as we do not currently provide angular velocities.
167 imu_if_->set_angular_velocity_covariance(0, -1.);
168 imu_if_->write();
169
170 //init motor variables
171 x_ = 0.0;
172 y_ = 0.0;
173 ori_ = 0.0;
174 vx_ = 0.0;
175 vy_ = 0.0;
176 vomega_ = 0.0;
177 des_vx_ = 0.0;
178 des_vy_ = 0.0;
179 des_vomega_ = 0.0;
180 x_offset_ = 0.0;
181 y_offset_ = 0.0;
182 ori_offset_ = 0.0;
183 path_length_ = 0.0;
184
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_];
189
190 new_data_ = false;
191
192 if (string_pub_->HasConnections()) {
193 msgs::Header helloMessage;
194 helloMessage.set_str_id("gazsim-robotino plugin connected");
195 string_pub_->Publish(helloMessage);
196 }
197}
198
199void
201{
202 //close interfaces
203 blackboard->close(imu_if_);
204 blackboard->close(sens_if_);
205 blackboard->close(motor_if_);
206 blackboard->close(switch_if_);
207
208 delete[] gyro_timestamp_buffer_;
209 delete[] gyro_angle_buffer_;
210}
211
212void
214{
215 //work off all messages passed to the motor_interfaces
216 process_motor_messages();
217
218 //update interfaces
219 if (new_data_) {
220 motor_if_->set_odometry_position_x(x_);
221 motor_if_->set_odometry_position_y(y_);
222 motor_if_->set_odometry_orientation(ori_);
223 motor_if_->set_odometry_path_length(path_length_);
224 motor_if_->write();
225
226 if (gyro_available_) {
227 //update gyro (with delay)
228 fawkes::Time now(clock);
229 while ((now - gyro_timestamp_buffer_[(gyro_buffer_index_delayed_ + 1) % gyro_buffer_size_])
230 .in_sec()
231 >= gyro_delay_
232 && gyro_buffer_index_delayed_ < gyro_buffer_index_new_) {
233 gyro_buffer_index_delayed_++;
234 }
235
236 tf::Quaternion q =
237 tf::create_quaternion_from_yaw(gyro_angle_buffer_[gyro_buffer_index_delayed_]);
238 imu_if_->set_orientation(0, q.x());
239 imu_if_->set_orientation(1, q.y());
240 imu_if_->set_orientation(2, q.z());
241 imu_if_->set_orientation(3, q.w());
242 for (uint i = 0; i < 9u; i += 4) {
243 imu_if_->set_orientation_covariance(i, 1e-3);
244 imu_if_->set_angular_velocity_covariance(i, 1e-3);
245 imu_if_->set_linear_acceleration_covariance(i, 1e-3);
246 }
247 } else {
248 imu_if_->set_angular_velocity(0, -1.);
249 imu_if_->set_orientation(0, -1.);
250 imu_if_->set_orientation(1, 0.);
251 imu_if_->set_orientation(2, 0.);
252 imu_if_->set_orientation(3, 0.);
253 }
254 imu_if_->write();
255
256 sens_if_->set_distance(infrared_sensor_index_, infrared_puck_sensor_dist_);
257
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_);
261 }
262 sens_if_->write();
263
264 new_data_ = false;
265 }
266}
267
268void
269RobotinoSimThread::send_transroot(double vx, double vy, double omega)
270{
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);
276}
277
278void
279RobotinoSimThread::process_motor_messages()
280{
281 //check messages of the switch interface
282 while (!switch_if_->msgq_empty()) {
283 if (SwitchInterface::DisableSwitchMessage *msg = switch_if_->msgq_first_safe(msg)) {
284 switch_if_->set_enabled(false);
285 //pause movement
286 send_transroot(0, 0, 0);
287 } else if (SwitchInterface::EnableSwitchMessage *msg = switch_if_->msgq_first_safe(msg)) {
288 switch_if_->set_enabled(true);
289 //unpause movement
290 send_transroot(vx_, vy_, vomega_);
291 }
292 switch_if_->msgq_pop();
293 switch_if_->write();
294 }
295
296 //do not do anything if the motor is disabled
297 if (!switch_if_->is_enabled()) {
298 return;
299 }
300
301 //check messages of the motor interface
302 while (motor_move_pub_->HasConnections() && !motor_if_->msgq_empty()) {
303 if (MotorInterface::TransRotMessage *msg = motor_if_->msgq_first_safe(msg)) {
304 //send command only if changed
305 if (vel_changed(msg->vx(), vx_, 0.01) || vel_changed(msg->vy(), vy_, 0.01)
306 || vel_changed(msg->omega(), vomega_, 0.01)) {
307 vx_ = msg->vx();
308 vy_ = msg->vy();
309 vomega_ = msg->omega();
310 des_vx_ = vx_;
311 des_vy_ = vy_;
312 des_vomega_ = vomega_;
313
314 //send message to gazebo (apply movement_factor to compensate friction)
315 send_transroot(vx_ * moving_speed_factor_,
316 vy_ * moving_speed_factor_,
317 vomega_ * rotation_speed_factor_);
318
319 //update interface
320 motor_if_->set_vx(vx_);
321 motor_if_->set_vy(vy_);
322 motor_if_->set_omega(vomega_);
323 motor_if_->set_des_vx(des_vx_);
324 motor_if_->set_des_vy(des_vy_);
325 motor_if_->set_des_omega(des_vomega_);
326 //update interface
327 motor_if_->write();
328 }
329 } else if (motor_if_->msgq_first_is<MotorInterface::ResetOdometryMessage>()) {
330 x_offset_ += x_;
331 y_offset_ += y_;
332 ori_offset_ += ori_;
333 x_ = 0.0;
334 y_ = 0.0;
335 ori_ = 0.0;
336 last_vel_set_time_ = clock->now();
337 }
338 motor_if_->msgq_pop();
339 }
340}
341
342bool
343RobotinoSimThread::vel_changed(float before, float after, float relativeThreashold)
344{
345 return (before == 0.0 || after == 0.0 || fabs((before - after) / before) > relativeThreashold);
346}
347
348//Handler Methods:
349void
350RobotinoSimThread::on_pos_msg(ConstPosePtr &msg)
351{
352 //logger_->log_debug(name_, "Got Position MSG from gazebo with ori: %f", msg->z());
353
355
356 //read out values + substract offset
357 float new_x = msg->position().x() - x_offset_;
358 float new_y = msg->position().y() - y_offset_;
359 //calculate ori from quaternion
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_;
365
366 //estimate path-length
367 float length_driven = sqrt((new_x - x_) * (new_x - x_) + (new_y - y_) * (new_y - y_));
368
369 if (slippery_wheels_enabled_) {
370 //simulate slipping wheels when driving against an obstacle
371 fawkes::Time new_time = clock->now();
372 double duration = new_time.in_sec() - last_pos_time_.in_sec();
373 //calculate duration since the velocity was last set to filter slipping while accelerating
374 double velocity_set_duration = new_time.in_sec() - last_vel_set_time_.in_sec();
375
376 last_pos_time_ = new_time;
377
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_;
385 //logger_->log_debug(name_, "Wheels are slipping (%f, %f)", slipped_x, slipped_y);
386 new_x = x_ + slipped_x;
387 new_y = y_ + slipped_y;
388 //update the offset (otherwise the slippery error would be corrected in the next iteration)
389 x_offset_ -= slipped_x;
390 y_offset_ -= slipped_y;
391
392 length_driven = sqrt((new_x - x_) * (new_x - x_) + (new_y - y_) * (new_y - y_));
393 }
394 }
395
396 //update stored values
397 x_ = new_x;
398 y_ = new_y;
399 ori_ = new_ori;
400 path_length_ += length_driven;
401 new_data_ = true;
402
403 //publish transform (otherwise the transform can not convert /base_link to /odom)
404 fawkes::Time now(clock);
405 tf::Transform t(tf::Quaternion(tf::Vector3(0, 0, 1), ori_), tf::Vector3(x_, y_, 0.0));
406
407 tf_publisher->send_transform(t, now, cfg_frame_odom_, cfg_frame_base_);
408}
409void
410RobotinoSimThread::on_gyro_msg(ConstVector3dPtr &msg)
411{
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;
417 new_data_ = true;
418}
419void
420RobotinoSimThread::on_infrared_puck_sensor_msg(ConstLaserScanStampedPtr &msg)
421{
423 //make sure that the config values for fetch_puck are set right
424 infrared_puck_sensor_dist_ = msg->scan().ranges(0);
425 new_data_ = true;
426}
427void
428RobotinoSimThread::on_gripper_laser_right_sensor_msg(ConstFloatPtr &msg)
429{
431
432 if (msg->value() < gripper_laser_threshold_) {
433 analog_in_right_ = gripper_laser_value_near_;
434 } else {
435 analog_in_right_ = gripper_laser_value_far_;
436 }
437 new_data_ = true;
438}
439void
440RobotinoSimThread::on_gripper_laser_left_sensor_msg(ConstFloatPtr &msg)
441{
443
444 if (msg->value() < gripper_laser_threshold_) {
445 analog_in_left_ = gripper_laser_value_near_;
446 } else {
447 analog_in_left_ = gripper_laser_value_far_;
448 }
449 new_data_ = true;
450}
451
452void
453RobotinoSimThread::on_gripper_has_puck_msg(ConstIntPtr &msg)
454{
455 // 1 means the gripper has a puck 0 not
456 sens_if_->set_digital_in(1, msg->data() > 0);
457 sens_if_->write();
458}
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
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.
Definition: clock.h:42
Time now() const
Get the current time.
Definition: clock.cpp:242
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
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.
Definition: gazebo.h:46
IMUInterface Fawkes BlackBoard Interface.
Definition: IMUInterface.h:34
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.
Definition: interface.h:351
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
MessageType * msgq_first_safe(MessageType *&msg) noexcept
Get first message casted to the desired type without exceptions.
Definition: interface.h:340
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.
Definition: logging.h:41
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.
Mutex locking helper.
Definition: mutex_locker.h:34
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.
Definition: thread.h:46
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
Definition: thread.h:152
const char * name() const
Get name of thread.
Definition: thread.h:100
A class for handling time.
Definition: time.h:93
double in_sec() const
Convet time to seconds.
Definition: time.cpp:219
Thread aspect to access the transform system.
Definition: tf.h:39
void tf_enable_publisher(const char *frame_id=0)
Late enabling of publisher.
Definition: tf.cpp:145
tf::TransformPublisher * tf_publisher
This is the transform publisher which can be used to publish transforms via the blackboard.
Definition: tf.h:68
virtual void send_transform(const StampedTransform &transform, const bool is_static=false)
Publish transform.
Fawkes library namespace.