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
fawkes::GazeboAspect
Thread aspect to get access to a Gazebo node handle.
Definition: gazebo.h:37
fawkes::SwitchInterface
SwitchInterface Fawkes BlackBoard Interface.
Definition: SwitchInterface.h:34
fawkes::IMUInterface
IMUInterface Fawkes BlackBoard Interface.
Definition: IMUInterface.h:34
RobotinoSimThread
Thread simulate the Robotino in Gazebo by sending needed informations to the Robotino-plugin in Gazeb...
Definition: gazsim_robotino_thread.h:62
RobotinoSimThread::init
virtual void init()
Initialize the thread.
Definition: gazsim_robotino_thread.cpp:59
fawkes::BlockedTimingAspect
Thread aspect to use blocked timing.
Definition: blocked_timing.h:51
fawkes::MotorInterface
MotorInterface Fawkes BlackBoard Interface.
Definition: MotorInterface.h:34
RobotinoSimThread::RobotinoSimThread
RobotinoSimThread()
Constructor.
Definition: gazsim_robotino_thread.cpp:51
fawkes::BlackBoardAspect
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
fawkes
Fawkes library namespace.
fawkes::LoggingAspect
Thread aspect to log output.
Definition: logging.h:33
fawkes::RobotinoSensorInterface
RobotinoSensorInterface Fawkes BlackBoard Interface.
Definition: RobotinoSensorInterface.h:34
fawkes::TransformAspect
Thread aspect to access the transform system.
Definition: tf.h:39
RobotinoSimThread::finalize
virtual void finalize()
Finalize the thread.
Definition: gazsim_robotino_thread.cpp:171
fawkes::Time
A class for handling time.
Definition: time.h:93
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
fawkes::ConfigurableAspect
Thread aspect to access configuration data.
Definition: configurable.h:33
fawkes::ClockAspect
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:34
RobotinoSimThread::loop
virtual void loop()
Code to execute in the thread.
Definition: gazsim_robotino_thread.cpp:184