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 "../msgs/Float.pb.h"
27 
28 #include <aspect/blackboard.h>
29 #include <aspect/blocked_timing.h>
30 #include <aspect/clock.h>
31 #include <aspect/configurable.h>
32 #include <aspect/logging.h>
33 #include <aspect/tf.h>
34 #include <core/threading/thread.h>
35 #include <plugins/gazebo/aspect/gazebo.h>
36 
37 #include <list>
38 
39 //from Gazebo
40 #include <gazebo/msgs/MessageTypes.hh>
41 #include <gazebo/transport/TransportTypes.hh>
42 #include <gazebo/transport/transport.hh>
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 } // namespace fawkes
53 
55  public fawkes::ClockAspect,
56  public fawkes::LoggingAspect,
62 {
63 public:
65 
66  virtual void init();
67  virtual void loop();
68  virtual void finalize();
69 
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  std::string cfg_frame_imu_;
99  double gripper_laser_threshold_;
100  double gripper_laser_value_far_;
101  double gripper_laser_value_near_;
102  bool slippery_wheels_enabled_;
103  double slippery_wheels_threshold_;
104  double moving_speed_factor_;
105  double rotation_speed_factor_;
106  bool have_gripper_sensors_;
107  int gripper_laser_left_pos_;
108  int gripper_laser_right_pos_;
109  int infrared_sensor_index_;
110 
111  //Helper variables for motor:
112 
113  //current motorMovements
114  float vx_;
115  float vy_;
116  float vomega_;
117  float des_vx_;
118  float des_vy_;
119  float des_vomega_;
120  //last received odom position
121  float x_;
122  float y_;
123  float ori_;
124  float path_length_;
125 
126  //RobotinoSensorInterface values (stored here to write the interfaces only in the loop)
127  bool gyro_available_;
128  int gyro_buffer_size_;
129  int gyro_buffer_index_new_;
130  int gyro_buffer_index_delayed_;
131  fawkes::Time *gyro_timestamp_buffer_;
132  float * gyro_angle_buffer_;
133  float gyro_delay_;
134  float infrared_puck_sensor_dist_;
135  float analog_in_left_;
136  float analog_in_right_;
137 
138  //are there new values to write in the interfaces?
139  bool new_data_;
140 
141  fawkes::Time last_pos_time_;
142  fawkes::Time last_vel_set_time_;
143 
144  //Odometry offset
145  float x_offset_;
146  float y_offset_;
147  float ori_offset_;
148 
149  //Helper functions:
150  void process_motor_messages();
151  void send_transroot(double vx, double vy, double omega);
152  bool vel_changed(float before, float after, float relativeThreashold);
153 };
154 
155 #endif
Thread aspect to access to BlackBoard.
Definition: blackboard.h:33
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:33
virtual void loop()
Code to execute in the thread.
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:36
A class for handling time.
Definition: time.h:92
Thread class encapsulation of pthreads.
Definition: thread.h:45
Thread aspect to access the transform system.
Definition: tf.h:38
RobotinoSensorInterface Fawkes BlackBoard Interface.
Thread aspect to use blocked timing.
SwitchInterface Fawkes BlackBoard Interface.
virtual void init()
Initialize the thread.
Thread aspect to log output.
Definition: logging.h:32
Thread aspect to access configuration data.
Definition: configurable.h:32
IMUInterface Fawkes BlackBoard Interface.
Definition: IMUInterface.h:33
MotorInterface Fawkes BlackBoard Interface.
virtual void finalize()
Finalize the thread.