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
44typedef const boost::shared_ptr<gazsim_msgs::Float const> ConstFloatPtr;
45
46namespace fawkes {
47class BatteryInterface;
48class IMUInterface;
49class MotorInterface;
50class RobotinoSensorInterface;
51class SwitchInterface;
52} // namespace fawkes
53
62{
63public:
65
66 virtual void init();
67 virtual void loop();
68 virtual void finalize();
69
70private:
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.