Fawkes API  Fawkes Development Version
gazsim_robotino_thread.h
1 /***************************************************************************
2  * gazsim_robotino_thread.h - Thread simulate the Robotino in Gazebo
3  * by sending needed informations to the Robotino-plugin in Gazebo
4  * and recieving sensordata from Gazebo
5  *
6  * Created: Fr 3. Mai 21:20:08 CEST 2013
7  * Copyright 2013 Frederik Zwilling
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #ifndef __PLUGINS_GAZSIM_ROBOTINO_THREAD_H_
24 #define __PLUGINS_GAZSIM_ROBOTINO_THREAD_H_
25 
26 #include <list>
27 
28 #include <core/threading/thread.h>
29 #include <aspect/clock.h>
30 #include <aspect/configurable.h>
31 #include <aspect/logging.h>
32 #include <aspect/blackboard.h>
33 #include <aspect/blocked_timing.h>
34 #include <plugins/gazebo/aspect/gazebo.h>
35 #include <aspect/tf.h>
36 #include "../msgs/Float.pb.h"
37 
38 //from Gazebo
39 #include <gazebo/transport/TransportTypes.hh>
40 #include <gazebo/msgs/MessageTypes.hh>
41 #include <gazebo/transport/transport.hh>
42 
43 
44 typedef const boost::shared_ptr<gazsim_msgs::Float const> ConstFloatPtr;
45 
46 namespace fawkes {
47  class BatteryInterface;
48  class IMUInterface;
49  class MotorInterface;
50  class RobotinoSensorInterface;
51  class SwitchInterface;
52 }
53 
55 : public fawkes::Thread,
56  public fawkes::ClockAspect,
57  public fawkes::LoggingAspect,
63 {
64  public:
66 
67  virtual void init();
68  virtual void loop();
69  virtual void finalize();
70  private:
71  //Publisher to send messages to gazebo
72  gazebo::transport::PublisherPtr string_pub_;
73  gazebo::transport::PublisherPtr motor_move_pub_;
74 
75  //Suscribers to recieve messages from gazebo
76  gazebo::transport::SubscriberPtr gyro_sub_;
77  gazebo::transport::SubscriberPtr infrared_puck_sensor_sub_;
78  gazebo::transport::SubscriberPtr gripper_laser_left_sensor_sub_;
79  gazebo::transport::SubscriberPtr gripper_laser_right_sensor_sub_;
80  gazebo::transport::SubscriberPtr pos_sub_;
81 
82  //Handler functions for incoming messages
83  void on_gyro_msg(ConstVector3dPtr &msg);
84  void on_infrared_puck_sensor_msg(ConstLaserScanStampedPtr &msg);
85  void on_gripper_laser_left_sensor_msg(ConstFloatPtr &msg);
86  void on_gripper_laser_right_sensor_msg(ConstFloatPtr &msg);
87  void on_pos_msg(ConstPosePtr &msg);
88 
89  //provided interfaces
91  fawkes::MotorInterface *motor_if_;
92  fawkes::SwitchInterface *switch_if_;
93  fawkes::IMUInterface *imu_if_;
94 
95  //config values
96  std::string cfg_frame_odom_;
97  std::string cfg_frame_base_;
98  double gripper_laser_threshold_;
99  double gripper_laser_value_far_;
100  double gripper_laser_value_near_;
101  bool slippery_wheels_enabled_;
102  double slippery_wheels_threshold_;
103  double moving_speed_factor_;
104  double rotation_speed_factor_;
105  bool have_gripper_sensors_;
106  int gripper_laser_left_pos_;
107  int gripper_laser_right_pos_;
108  int infrared_sensor_index_;
109 
110  //Helper variables for motor:
111 
112  //current motorMovements
113  float vx_;
114  float vy_;
115  float vomega_;
116  float des_vx_;
117  float des_vy_;
118  float des_vomega_;
119  //last received odom position
120  float x_;
121  float y_;
122  float ori_;
123  float path_length_;
124 
125  //RobotinoSensorInterface values (stored here to write the interfaces only in the loop)
126  bool gyro_available_;
127  int gyro_buffer_size_;
128  int gyro_buffer_index_new_;
129  int gyro_buffer_index_delayed_;
130  fawkes::Time *gyro_timestamp_buffer_;
131  float *gyro_angle_buffer_;
132  float gyro_delay_;
133  float infrared_puck_sensor_dist_;
134  float analog_in_left_;
135  float analog_in_right_;
136 
137  //are there new values to write in the interfaces?
138  bool new_data_;
139 
140  fawkes::Time last_pos_time_;
141  fawkes::Time last_vel_set_time_;
142 
143  //Odometry offset
144  float x_offset_;
145  float y_offset_;
146  float ori_offset_;
147 
148  //Helper functions:
149  void process_motor_messages();
150  void send_transroot(double vx, double vy, double omega);
151  bool vel_changed(float before, float after, float relativeThreashold);
152 };
153 
154 #endif
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:36
Thread simulate the Robotino in Gazebo by sending needed informations to the Robotino-plugin in Gazeb...
Fawkes library namespace.
Thread aspect to get access to a Gazebo node handle.
Definition: gazebo.h:37
A class for handling time.
Definition: time.h:91
Thread class encapsulation of pthreads.
Definition: thread.h:42
Thread aspect to access the transform system.
Definition: tf.h:42
RobotinoSensorInterface Fawkes BlackBoard Interface.
Thread aspect to use blocked timing.
SwitchInterface Fawkes BlackBoard Interface.
Thread aspect to log output.
Definition: logging.h:35
Thread aspect to access configuration data.
Definition: configurable.h:35
IMUInterface Fawkes BlackBoard Interface.
Definition: IMUInterface.h:33
MotorInterface Fawkes BlackBoard Interface.