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 
54 class RobotinoSimThread : public fawkes::Thread,
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
fawkes::GazeboAspect
Definition: gazebo.h:40
fawkes::SwitchInterface
Definition: SwitchInterface.h:37
fawkes::IMUInterface
Definition: IMUInterface.h:37
RobotinoSimThread
Definition: gazsim_robotino_thread.h:53
RobotinoSimThread::init
virtual void init()
Initialize the thread.
Definition: gazsim_robotino_thread.cpp:58
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:54
fawkes::MotorInterface
Definition: MotorInterface.h:37
RobotinoSimThread::RobotinoSimThread
RobotinoSimThread()
Constructor.
Definition: gazsim_robotino_thread.cpp:50
fawkes::BlackBoardAspect
Definition: blackboard.h:36
fawkes
fawkes::LoggingAspect
Definition: logging.h:36
fawkes::RobotinoSensorInterface
Definition: RobotinoSensorInterface.h:37
fawkes::TransformAspect
Definition: tf.h:41
RobotinoSimThread::finalize
virtual void finalize()
Finalize the thread.
Definition: gazsim_robotino_thread.cpp:160
fawkes::Time
Definition: time.h:96
fawkes::Thread
Definition: thread.h:44
fawkes::ConfigurableAspect
Definition: configurable.h:36
fawkes::ClockAspect
Definition: clock.h:38
RobotinoSimThread::loop
virtual void loop()
Code to execute in the thread.
Definition: gazsim_robotino_thread.cpp:173