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 gripper_has_puck_sub_;
81 gazebo::transport::SubscriberPtr pos_sub_;
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);
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_;
129 bool gyro_available_;
130 int gyro_buffer_size_;
131 int gyro_buffer_index_new_;
132 int gyro_buffer_index_delayed_;
134 float * gyro_angle_buffer_;
136 float infrared_puck_sensor_dist_;
137 float analog_in_left_;
138 float analog_in_right_;
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);