23 #ifndef _PLUGINS_GAZSIM_ROBOTINO_THREAD_H_ 24 #define _PLUGINS_GAZSIM_ROBOTINO_THREAD_H_ 26 #include "../msgs/Float.pb.h" 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> 40 #include <gazebo/msgs/MessageTypes.hh> 41 #include <gazebo/transport/TransportTypes.hh> 42 #include <gazebo/transport/transport.hh> 44 typedef const boost::shared_ptr<gazsim_msgs::Float const> ConstFloatPtr;
47 class BatteryInterface;
50 class RobotinoSensorInterface;
51 class SwitchInterface;
72 gazebo::transport::PublisherPtr string_pub_;
73 gazebo::transport::PublisherPtr motor_move_pub_;
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_;
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);
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_;
127 bool gyro_available_;
128 int gyro_buffer_size_;
129 int gyro_buffer_index_new_;
130 int gyro_buffer_index_delayed_;
132 float * gyro_angle_buffer_;
134 float infrared_puck_sensor_dist_;
135 float analog_in_left_;
136 float analog_in_right_;
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);
Thread aspect to access to BlackBoard.
Thread aspect that allows to obtain the current time from the clock.
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.
A class for handling time.
Thread class encapsulation of pthreads.
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.
RobotinoSimThread()
Constructor.
Thread aspect to access configuration data.
IMUInterface Fawkes BlackBoard Interface.
MotorInterface Fawkes BlackBoard Interface.
virtual void finalize()
Finalize the thread.