Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef _tibi_dabo_pitch_estimator_alg_node_h_
00026 #define _tibi_dabo_pitch_estimator_alg_node_h_
00027
00028 #include <iri_base_algorithm/iri_base_algorithm.h>
00029 #include "tibi_dabo_pitch_estimator_alg.h"
00030
00031
00032 #include "laser_linear_regression.h"
00033
00034
00035 #include <iri_segway_rmp_msgs/SegwayRMP200Status.h>
00036 #include <visualization_msgs/MarkerArray.h>
00037
00038 #include <sensor_msgs/LaserScan.h>
00039
00040 #include <string>
00041
00042 #define PI 3.14159265358
00043
00044
00045
00046
00047
00048
00049
00050
00057 class TibiDaboPitchEstimatorAlgNode : public algorithm_base::IriBaseAlgorithm<TibiDaboPitchEstimatorAlgorithm>
00058 {
00059 private:
00060
00061 ros::Publisher front_laser_filtrado_publisher_;
00062 sensor_msgs::LaserScan LaserScan_front_msg_;
00063
00064 ros::Publisher rear_laser_filtrado_publisher_;
00065 sensor_msgs::LaserScan LaserScan_rear_msg_;
00066
00067
00068 ros::Publisher MarkerArray_lines_publisher_;
00069 visualization_msgs::MarkerArray MarkerArray_msg_;
00070
00071
00072
00073
00074
00075
00076 visualization_msgs::Marker line_marker_;
00077 visualization_msgs::Marker line_marker2_;
00078 visualization_msgs::Marker arrow_marker3_;
00079 visualization_msgs::Marker arrow_marker4_;
00080 visualization_msgs::Marker intersection_marker5_;
00081 visualization_msgs::Marker distance_front_laser_marker6_;
00082 enum res_lines{One_line_state=0, Two_lines_state, Line_partial_state, Line_unknown_state };
00083 res_lines result_lines_;
00084
00085
00086
00087 void line_node(double& a1,double& b1,double& a2,double& b2,res_lines result_lines_);
00088 void marker_point_front_laser_node(double front_laser_point_x, double front_laser_point_y);
00089
00090
00091 ros::Subscriber rear_laser_subscriber_;
00092 void rear_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg);
00093 CMutex rear_laser_mutex_;
00094
00095 ros::Subscriber front_laser_subscriber_;
00096 void front_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg);
00097 CMutex front_laser_mutex_;
00098
00099 ros::Subscriber pitch_subscriber_;
00100 void pitch_callback(const iri_segway_rmp_msgs::SegwayRMP200Status::ConstPtr& msg);
00101 CMutex pitch_mutex_;
00102
00103 ros::Subscriber laser_subscriber_;
00104 void laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg);
00105 CMutex laser_mutex_;
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120 double a1,b1,a2,b2,error,my_pitch_angle_,pitch;
00121 double front_laser_point_x,front_laser_point_y, rear_laser_point_x,rear_laser_point_y;
00122 double security_distance_;
00123
00124
00125
00126
00127
00128 public:
00135 TibiDaboPitchEstimatorAlgNode(void);
00136
00143 ~TibiDaboPitchEstimatorAlgNode(void);
00144
00145
00146
00147
00148 Claser_linear_regression reg_line_;
00149 vector<Cpoint> laser_;
00150 Cpoint point, intersection_;
00151 Claser_linear_regression::linear_regression_result result_, result_horizontal;
00152
00153 protected:
00166 void mainNodeThread(void);
00167
00180 void node_config_update(Config &config, uint32_t level);
00181
00188 void addNodeDiagnostics(void);
00189
00190
00191
00192
00193 };
00194
00195 #endif