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 gripper_has_puck_sub_;
81  gazebo::transport::SubscriberPtr pos_sub_;
82 
83  //Handler functions for incoming messages
84  void on_gyro_msg(ConstVector3dPtr &msg);
85  void on_infrared_puck_sensor_msg(ConstLaserScanStampedPtr &msg);
86  void on_gripper_laser_left_sensor_msg(ConstFloatPtr &msg);
87  void on_gripper_laser_right_sensor_msg(ConstFloatPtr &msg);
88  void on_gripper_has_puck_msg(ConstIntPtr &msg);
89  void on_pos_msg(ConstPosePtr &msg);
90 
91  //provided interfaces
93  fawkes::MotorInterface * motor_if_;
94  fawkes::SwitchInterface * switch_if_;
95  fawkes::IMUInterface * imu_if_;
96 
97  //config values
98  std::string cfg_frame_odom_;
99  std::string cfg_frame_base_;
100  std::string cfg_frame_imu_;
101  double gripper_laser_threshold_;
102  double gripper_laser_value_far_;
103  double gripper_laser_value_near_;
104  bool slippery_wheels_enabled_;
105  double slippery_wheels_threshold_;
106  double moving_speed_factor_;
107  double rotation_speed_factor_;
108  bool have_gripper_sensors_;
109  int gripper_laser_left_pos_;
110  int gripper_laser_right_pos_;
111  int infrared_sensor_index_;
112 
113  //Helper variables for motor:
114 
115  //current motorMovements
116  float vx_;
117  float vy_;
118  float vomega_;
119  float des_vx_;
120  float des_vy_;
121  float des_vomega_;
122  //last received odom position
123  float x_;
124  float y_;
125  float ori_;
126  float path_length_;
127 
128  //RobotinoSensorInterface values (stored here to write the interfaces only in the loop)
129  bool gyro_available_;
130  int gyro_buffer_size_;
131  int gyro_buffer_index_new_;
132  int gyro_buffer_index_delayed_;
133  fawkes::Time *gyro_timestamp_buffer_;
134  float * gyro_angle_buffer_;
135  float gyro_delay_;
136  float infrared_puck_sensor_dist_;
137  float analog_in_left_;
138  float analog_in_right_;
139 
140  //are there new values to write in the interfaces?
141  bool new_data_;
142 
143  fawkes::Time last_pos_time_;
144  fawkes::Time last_vel_set_time_;
145 
146  //Odometry offset
147  float x_offset_;
148  float y_offset_;
149  float ori_offset_;
150 
151  //Helper functions:
152  void process_motor_messages();
153  void send_transroot(double vx, double vy, double omega);
154  bool vel_changed(float before, float after, float relativeThreashold);
155 };
156 
157 #endif
Thread simulate the Robotino in Gazebo by sending needed informations to the Robotino-plugin in Gazeb...
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
Thread aspect to use blocked timing.
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:34
Thread aspect to access configuration data.
Definition: configurable.h:33
Thread aspect to get access to a Gazebo node handle.
Definition: gazebo.h:37
IMUInterface Fawkes BlackBoard Interface.
Definition: IMUInterface.h:34
Thread aspect to log output.
Definition: logging.h:33
MotorInterface Fawkes BlackBoard Interface.
RobotinoSensorInterface Fawkes BlackBoard Interface.
SwitchInterface Fawkes BlackBoard Interface.
Thread class encapsulation of pthreads.
Definition: thread.h:46
A class for handling time.
Definition: time.h:93
Thread aspect to access the transform system.
Definition: tf.h:39
Fawkes library namespace.