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 
40 using namespace fawkes;
41 using 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 
58 void
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");
90  sens_if_ = blackboard->open_for_writing<RobotinoSensorInterface>("Robotino");
91  imu_if_ = blackboard->open_for_writing<IMUInterface>("IMU Robotino");
92 
93  //Create suscribers
94  pos_sub_ = gazebonode->Subscribe(config->get_string("/gazsim/topics/gps"),
95  &RobotinoSimThread::on_pos_msg,
96  this);
97  gyro_sub_ = gazebonode->Subscribe(config->get_string("/gazsim/topics/gyro"),
98  &RobotinoSimThread::on_gyro_msg,
99  this);
100  infrared_puck_sensor_sub_ =
101  gazebonode->Subscribe(config->get_string("/gazsim/topics/infrared-puck-sensor"),
102  &RobotinoSimThread::on_infrared_puck_sensor_msg,
103  this);
104  if (have_gripper_sensors_) {
105  gripper_laser_left_sensor_sub_ =
106  gazebonode->Subscribe(config->get_string("/gazsim/topics/gripper-laser-left"),
107  &RobotinoSimThread::on_gripper_laser_left_sensor_msg,
108  this);
109  gripper_laser_right_sensor_sub_ =
110  gazebonode->Subscribe(config->get_string("/gazsim/topics/gripper-laser-right"),
111  &RobotinoSimThread::on_gripper_laser_right_sensor_msg,
112  this);
113  }
114 
115  gripper_has_puck_sub_ =
116  gazebonode->Subscribe(config->get_string("/gazsim/topics/gripper-has-puck"),
117  &RobotinoSimThread::on_gripper_has_puck_msg,
118  this);
119 
120  //Create publishers
121  motor_move_pub_ =
122  gazebonode->Advertise<msgs::Vector3d>(config->get_string("/gazsim/topics/motor-move"));
123  string_pub_ = gazebonode->Advertise<msgs::Header>(config->get_string("/gazsim/topics/message"));
124 
125  //enable motor by default
126  switch_if_->set_enabled(true);
127  switch_if_->write();
128 
129  // puck sensor connected to first 2 inputs of RobotinoInterface
130  sens_if_->set_digital_in(0, false);
131  sens_if_->set_digital_in(1, false);
132  sens_if_->write();
133 
134  imu_if_->set_frame(cfg_frame_imu_.c_str());
135  imu_if_->set_linear_acceleration(0, -1.);
136  //imu_if_->set_angular_velocity_covariance(8, deg2rad(0.1));
137  // set as not available as we do not currently provide angular velocities.
138  imu_if_->set_angular_velocity_covariance(0, -1.);
139  imu_if_->write();
140 
141  //init motor variables
142  x_ = 0.0;
143  y_ = 0.0;
144  ori_ = 0.0;
145  vx_ = 0.0;
146  vy_ = 0.0;
147  vomega_ = 0.0;
148  des_vx_ = 0.0;
149  des_vy_ = 0.0;
150  des_vomega_ = 0.0;
151  x_offset_ = 0.0;
152  y_offset_ = 0.0;
153  ori_offset_ = 0.0;
154  path_length_ = 0.0;
155 
156  gyro_buffer_index_new_ = 0;
157  gyro_buffer_index_delayed_ = 0;
158  gyro_timestamp_buffer_ = new fawkes::Time[gyro_buffer_size_];
159  gyro_angle_buffer_ = new float[gyro_buffer_size_];
160 
161  new_data_ = false;
162 
163  if (string_pub_->HasConnections()) {
164  msgs::Header helloMessage;
165  helloMessage.set_str_id("gazsim-robotino plugin connected");
166  string_pub_->Publish(helloMessage);
167  }
168 }
169 
170 void
172 {
173  //close interfaces
174  blackboard->close(imu_if_);
175  blackboard->close(sens_if_);
176  blackboard->close(motor_if_);
177  blackboard->close(switch_if_);
178 
179  delete[] gyro_timestamp_buffer_;
180  delete[] gyro_angle_buffer_;
181 }
182 
183 void
185 {
186  //work off all messages passed to the motor_interfaces
187  process_motor_messages();
188 
189  //update interfaces
190  if (new_data_) {
191  motor_if_->set_odometry_position_x(x_);
192  motor_if_->set_odometry_position_y(y_);
193  motor_if_->set_odometry_orientation(ori_);
194  motor_if_->set_odometry_path_length(path_length_);
195  motor_if_->write();
196 
197  if (gyro_available_) {
198  //update gyro (with delay)
199  fawkes::Time now(clock);
200  while ((now - gyro_timestamp_buffer_[(gyro_buffer_index_delayed_ + 1) % gyro_buffer_size_])
201  .in_sec()
202  >= gyro_delay_
203  && gyro_buffer_index_delayed_ < gyro_buffer_index_new_) {
204  gyro_buffer_index_delayed_++;
205  }
206 
207  tf::Quaternion q =
208  tf::create_quaternion_from_yaw(gyro_angle_buffer_[gyro_buffer_index_delayed_]);
209  imu_if_->set_orientation(0, q.x());
210  imu_if_->set_orientation(1, q.y());
211  imu_if_->set_orientation(2, q.z());
212  imu_if_->set_orientation(3, q.w());
213  for (uint i = 0; i < 9u; i += 4) {
214  imu_if_->set_orientation_covariance(i, 1e-3);
215  imu_if_->set_angular_velocity_covariance(i, 1e-3);
216  imu_if_->set_linear_acceleration_covariance(i, 1e-3);
217  }
218  } else {
219  imu_if_->set_angular_velocity(0, -1.);
220  imu_if_->set_orientation(0, -1.);
221  imu_if_->set_orientation(1, 0.);
222  imu_if_->set_orientation(2, 0.);
223  imu_if_->set_orientation(3, 0.);
224  }
225  imu_if_->write();
226 
227  sens_if_->set_distance(infrared_sensor_index_, infrared_puck_sensor_dist_);
228 
229  if (have_gripper_sensors_) {
230  sens_if_->set_analog_in(gripper_laser_left_pos_, analog_in_left_);
231  sens_if_->set_analog_in(gripper_laser_right_pos_, analog_in_right_);
232  }
233  sens_if_->write();
234 
235  new_data_ = false;
236  }
237 }
238 
239 void
240 RobotinoSimThread::send_transroot(double vx, double vy, double omega)
241 {
242  msgs::Vector3d motorMove;
243  motorMove.set_x(vx_);
244  motorMove.set_y(vy_);
245  motorMove.set_z(vomega_);
246  motor_move_pub_->Publish(motorMove);
247 }
248 
249 void
250 RobotinoSimThread::process_motor_messages()
251 {
252  //check messages of the switch interface
253  while (!switch_if_->msgq_empty()) {
254  if (SwitchInterface::DisableSwitchMessage *msg = switch_if_->msgq_first_safe(msg)) {
255  switch_if_->set_enabled(false);
256  //pause movement
257  send_transroot(0, 0, 0);
258  } else if (SwitchInterface::EnableSwitchMessage *msg = switch_if_->msgq_first_safe(msg)) {
259  switch_if_->set_enabled(true);
260  //unpause movement
261  send_transroot(vx_, vy_, vomega_);
262  }
263  switch_if_->msgq_pop();
264  switch_if_->write();
265  }
266 
267  //do not do anything if the motor is disabled
268  if (!switch_if_->is_enabled()) {
269  return;
270  }
271 
272  //check messages of the motor interface
273  while (motor_move_pub_->HasConnections() && !motor_if_->msgq_empty()) {
274  if (MotorInterface::TransRotMessage *msg = motor_if_->msgq_first_safe(msg)) {
275  //send command only if changed
276  if (vel_changed(msg->vx(), vx_, 0.01) || vel_changed(msg->vy(), vy_, 0.01)
277  || vel_changed(msg->omega(), vomega_, 0.01)) {
278  vx_ = msg->vx();
279  vy_ = msg->vy();
280  vomega_ = msg->omega();
281  des_vx_ = vx_;
282  des_vy_ = vy_;
283  des_vomega_ = vomega_;
284 
285  //send message to gazebo (apply movement_factor to compensate friction)
286  send_transroot(vx_ * moving_speed_factor_,
287  vy_ * moving_speed_factor_,
288  vomega_ * rotation_speed_factor_);
289 
290  //update interface
291  motor_if_->set_vx(vx_);
292  motor_if_->set_vy(vy_);
293  motor_if_->set_omega(vomega_);
294  motor_if_->set_des_vx(des_vx_);
295  motor_if_->set_des_vy(des_vy_);
296  motor_if_->set_des_omega(des_vomega_);
297  //update interface
298  motor_if_->write();
299  }
300  } else if (motor_if_->msgq_first_is<MotorInterface::ResetOdometryMessage>()) {
301  x_offset_ += x_;
302  y_offset_ += y_;
303  ori_offset_ += ori_;
304  x_ = 0.0;
305  y_ = 0.0;
306  ori_ = 0.0;
307  last_vel_set_time_ = clock->now();
308  }
309  motor_if_->msgq_pop();
310  }
311 }
312 
313 bool
314 RobotinoSimThread::vel_changed(float before, float after, float relativeThreashold)
315 {
316  return (before == 0.0 || after == 0.0 || fabs((before - after) / before) > relativeThreashold);
317 }
318 
319 //Handler Methods:
320 void
321 RobotinoSimThread::on_pos_msg(ConstPosePtr &msg)
322 {
323  //logger_->log_debug(name_, "Got Position MSG from gazebo with ori: %f", msg->z());
324 
325  MutexLocker lock(loop_mutex);
326 
327  //read out values + substract offset
328  float new_x = msg->position().x() - x_offset_;
329  float new_y = msg->position().y() - y_offset_;
330  //calculate ori from quaternion
331  float new_ori = tf::get_yaw(tf::Quaternion(msg->orientation().x(),
332  msg->orientation().y(),
333  msg->orientation().z(),
334  msg->orientation().w()));
335  new_ori -= ori_offset_;
336 
337  //estimate path-length
338  float length_driven = sqrt((new_x - x_) * (new_x - x_) + (new_y - y_) * (new_y - y_));
339 
340  if (slippery_wheels_enabled_) {
341  //simulate slipping wheels when driving against an obstacle
342  fawkes::Time new_time = clock->now();
343  double duration = new_time.in_sec() - last_pos_time_.in_sec();
344  //calculate duration since the velocity was last set to filter slipping while accelerating
345  double velocity_set_duration = new_time.in_sec() - last_vel_set_time_.in_sec();
346 
347  last_pos_time_ = new_time;
348 
349  double total_speed = sqrt(vx_ * vx_ + vy_ * vy_);
350  if (length_driven < total_speed * duration * slippery_wheels_threshold_
351  && velocity_set_duration > duration) {
352  double speed_abs_x = vx_ * cos(ori_) - vy_ * sin(ori_);
353  double speed_abs_y = vx_ * sin(ori_) + vy_ * cos(ori_);
354  double slipped_x = speed_abs_x * duration * slippery_wheels_threshold_;
355  double slipped_y = speed_abs_y * duration * slippery_wheels_threshold_;
356  //logger_->log_debug(name_, "Wheels are slipping (%f, %f)", slipped_x, slipped_y);
357  new_x = x_ + slipped_x;
358  new_y = y_ + slipped_y;
359  //update the offset (otherwise the slippery error would be corrected in the next iteration)
360  x_offset_ -= slipped_x;
361  y_offset_ -= slipped_y;
362 
363  length_driven = sqrt((new_x - x_) * (new_x - x_) + (new_y - y_) * (new_y - y_));
364  }
365  }
366 
367  //update stored values
368  x_ = new_x;
369  y_ = new_y;
370  ori_ = new_ori;
371  path_length_ += length_driven;
372  new_data_ = true;
373 
374  //publish transform (otherwise the transform can not convert /base_link to /odom)
375  fawkes::Time now(clock);
376  tf::Transform t(tf::Quaternion(tf::Vector3(0, 0, 1), ori_), tf::Vector3(x_, y_, 0.0));
377 
378  tf_publisher->send_transform(t, now, cfg_frame_odom_, cfg_frame_base_);
379 }
380 void
381 RobotinoSimThread::on_gyro_msg(ConstVector3dPtr &msg)
382 {
383  MutexLocker lock(loop_mutex);
384  gyro_buffer_index_new_ = (gyro_buffer_index_new_ + 1) % gyro_buffer_size_;
385  gyro_angle_buffer_[gyro_buffer_index_new_] = msg->z();
386  gyro_timestamp_buffer_[gyro_buffer_index_new_] = clock->now();
387  gyro_available_ = true;
388  new_data_ = true;
389 }
390 void
391 RobotinoSimThread::on_infrared_puck_sensor_msg(ConstLaserScanStampedPtr &msg)
392 {
393  MutexLocker lock(loop_mutex);
394  //make sure that the config values for fetch_puck are set right
395  infrared_puck_sensor_dist_ = msg->scan().ranges(0);
396  new_data_ = true;
397 }
398 void
399 RobotinoSimThread::on_gripper_laser_right_sensor_msg(ConstFloatPtr &msg)
400 {
401  MutexLocker lock(loop_mutex);
402 
403  if (msg->value() < gripper_laser_threshold_) {
404  analog_in_right_ = gripper_laser_value_near_;
405  } else {
406  analog_in_right_ = gripper_laser_value_far_;
407  }
408  new_data_ = true;
409 }
410 void
411 RobotinoSimThread::on_gripper_laser_left_sensor_msg(ConstFloatPtr &msg)
412 {
413  MutexLocker lock(loop_mutex);
414 
415  if (msg->value() < gripper_laser_threshold_) {
416  analog_in_left_ = gripper_laser_value_near_;
417  } else {
418  analog_in_left_ = gripper_laser_value_far_;
419  }
420  new_data_ = true;
421 }
422 
423 void
424 RobotinoSimThread::on_gripper_has_puck_msg(ConstIntPtr &msg)
425 {
426  // 1 means the gripper has a puck 0 not
427  sens_if_->set_digital_in(1, msg->data() > 0);
428  sens_if_->write();
429 }
fawkes::RobotinoSensorInterface::set_digital_in
void set_digital_in(unsigned int index, const bool new_digital_in)
Set digital_in value at given index.
Definition: RobotinoSensorInterface.cpp:373
fawkes::TransformAspect::tf_enable_publisher
void tf_enable_publisher(const char *frame_id=0)
Late enabling of publisher.
Definition: tf.cpp:145
fawkes::IMUInterface::set_orientation_covariance
void set_orientation_covariance(unsigned int index, const double new_orientation_covariance)
Set orientation_covariance value at given index.
Definition: IMUInterface.cpp:245
fawkes::Interface::msgq_first_is
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
fawkes::Interface::msgq_pop
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1182
fawkes::Interface::msgq_empty
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1029
fawkes::IMUInterface::set_angular_velocity_covariance
void set_angular_velocity_covariance(unsigned int index, const double new_angular_velocity_covariance)
Set angular_velocity_covariance value at given index.
Definition: IMUInterface.cpp:371
fawkes::SwitchInterface
SwitchInterface Fawkes BlackBoard Interface.
Definition: SwitchInterface.h:34
fawkes::IMUInterface
IMUInterface Fawkes BlackBoard Interface.
Definition: IMUInterface.h:34
fawkes::MotorInterface::set_vx
void set_vx(const float new_vx)
Set vx value.
Definition: MotorInterface.cpp:442
fawkes::MotorInterface::TransRotMessage
TransRotMessage Fawkes BlackBoard Interface Message.
Definition: MotorInterface.h:355
fawkes::Interface::msgq_first_safe
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Definition: interface.h:303
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
RobotinoSimThread::init
virtual void init()
Initialize the thread.
Definition: gazsim_robotino_thread.cpp:59
fawkes::MutexLocker
Mutex locking helper.
Definition: mutex_locker.h:34
fawkes::RobotinoSensorInterface::set_analog_in
void set_analog_in(unsigned int index, const float new_analog_in)
Set analog_in value at given index.
Definition: RobotinoSensorInterface.cpp:483
fawkes::GazeboAspect::gazebonode
gazebo::transport::NodePtr gazebonode
Gazebo Node for communication with a robot.
Definition: gazebo.h:46
fawkes::BlockedTimingAspect
Thread aspect to use blocked timing.
Definition: blocked_timing.h:51
fawkes::SwitchInterface::DisableSwitchMessage
DisableSwitchMessage Fawkes BlackBoard Interface Message.
Definition: SwitchInterface.h:136
fawkes::MotorInterface
MotorInterface Fawkes BlackBoard Interface.
Definition: MotorInterface.h:34
fawkes::Thread::name
const char * name() const
Get name of thread.
Definition: thread.h:100
fawkes::ClockAspect::clock
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
fawkes::MotorInterface::set_des_omega
void set_des_omega(const float new_des_omega)
Set des_omega value.
Definition: MotorInterface.cpp:612
fawkes::Configuration::get_int
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
RobotinoSimThread::RobotinoSimThread
RobotinoSimThread()
Constructor.
Definition: gazsim_robotino_thread.cpp:51
fawkes::SwitchInterface::EnableSwitchMessage
EnableSwitchMessage Fawkes BlackBoard Interface Message.
Definition: SwitchInterface.h:116
fawkes::LoggingAspect::logger
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
fawkes::MotorInterface::set_vy
void set_vy(const float new_vy)
Set vy value.
Definition: MotorInterface.cpp:476
fawkes::MotorInterface::set_odometry_position_x
void set_odometry_position_x(const float new_odometry_position_x)
Set odometry_position_x value.
Definition: MotorInterface.cpp:340
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
Close interface.
fawkes::MotorInterface::ResetOdometryMessage
ResetOdometryMessage Fawkes BlackBoard Interface Message.
Definition: MotorInterface.h:178
fawkes::SwitchInterface::set_enabled
void set_enabled(const bool new_enabled)
Set enabled value.
Definition: SwitchInterface.cpp:105
fawkes::Clock::now
Time now() const
Get the current time.
Definition: clock.cpp:242
fawkes
Fawkes library namespace.
fawkes::MotorInterface::set_des_vy
void set_des_vy(const float new_des_vy)
Set des_vy value.
Definition: MotorInterface.cpp:578
fawkes::tf::TransformPublisher::send_transform
virtual void send_transform(const StampedTransform &transform, const bool is_static=false)
Publish transform.
Definition: transform_publisher.cpp:115
fawkes::RobotinoSensorInterface
RobotinoSensorInterface Fawkes BlackBoard Interface.
Definition: RobotinoSensorInterface.h:34
fawkes::TransformAspect
Thread aspect to access the transform system.
Definition: tf.h:39
fawkes::RobotinoSensorInterface::set_distance
void set_distance(unsigned int index, const float new_distance)
Set distance value at given index.
Definition: RobotinoSensorInterface.cpp:318
fawkes::IMUInterface::set_orientation
void set_orientation(unsigned int index, const float new_orientation)
Set orientation value at given index.
Definition: IMUInterface.cpp:182
fawkes::ConfigurableAspect::config
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
RobotinoSimThread::finalize
virtual void finalize()
Finalize the thread.
Definition: gazsim_robotino_thread.cpp:171
fawkes::MotorInterface::set_odometry_position_y
void set_odometry_position_y(const float new_odometry_position_y)
Set odometry_position_y value.
Definition: MotorInterface.cpp:374
fawkes::Time
A class for handling time.
Definition: time.h:93
fawkes::MotorInterface::set_des_vx
void set_des_vx(const float new_des_vx)
Set des_vx value.
Definition: MotorInterface.cpp:544
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
fawkes::MotorInterface::set_odometry_orientation
void set_odometry_orientation(const float new_odometry_orientation)
Set odometry_orientation value.
Definition: MotorInterface.cpp:408
fawkes::TransformAspect::tf_publisher
tf::TransformPublisher * tf_publisher
This is the transform publisher which can be used to publish transforms via the blackboard.
Definition: tf.h:68
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
fawkes::MotorInterface::set_omega
void set_omega(const float new_omega)
Set omega value.
Definition: MotorInterface.cpp:510
fawkes::SwitchInterface::is_enabled
bool is_enabled() const
Get enabled value.
Definition: SwitchInterface.cpp:83
fawkes::Thread::loop_mutex
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
Definition: thread.h:152
fawkes::IMUInterface::set_linear_acceleration_covariance
void set_linear_acceleration_covariance(unsigned int index, const double new_linear_acceleration_covariance)
Set linear_acceleration_covariance value at given index.
Definition: IMUInterface.cpp:497
fawkes::Configuration::exists
virtual bool exists(const char *path)=0
Check if a given value exists.
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
fawkes::Interface::write
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
fawkes::Time::in_sec
double in_sec() const
Convet time to seconds.
Definition: time.cpp:219
RobotinoSimThread::loop
virtual void loop()
Code to execute in the thread.
Definition: gazsim_robotino_thread.cpp:184
fawkes::IMUInterface::set_angular_velocity
void set_angular_velocity(unsigned int index, const float new_angular_velocity)
Set angular_velocity value at given index.
Definition: IMUInterface.cpp:308
fawkes::MotorInterface::set_odometry_path_length
void set_odometry_path_length(const float new_odometry_path_length)
Set odometry_path_length value.
Definition: MotorInterface.cpp:306