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);