tibi_dabo_pitch_estimator_alg_node.h
Go to the documentation of this file.
00001 // Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
00002 // Author 
00003 // All rights reserved.
00004 //
00005 // This file is part of iri-ros-pkg
00006 // iri-ros-pkg is free software: you can redistribute it and/or modify
00007 // it under the terms of the GNU Lesser General Public License as published by
00008 // the Free Software Foundation, either version 3 of the License, or
00009 // at your option) any later version.
00010 //
00011 // This program is distributed in the hope that it will be useful,
00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 // GNU Lesser General Public License for more details.
00015 //
00016 // You should have received a copy of the GNU Lesser General Public License
00017 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00018 // 
00019 // IMPORTANT NOTE: This code has been generated through a script from the 
00020 // iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
00021 // of the scripts. ROS topics can be easly add by using those scripts. Please
00022 // refer to the IRI wiki page for more information:
00023 // http://wikiri.upc.es/index.php/Robotics_Lab
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 // [publisher subscriber headers]
00035 #include <iri_segway_rmp_msgs/SegwayRMP200Status.h>
00036 #include <visualization_msgs/MarkerArray.h>
00037 
00038 #include <sensor_msgs/LaserScan.h>
00039 //#include <geometry_msgs/PointStamped.h>
00040 #include <string>
00041 
00042 #define PI 3.14159265358
00043 
00044 // [service client headers]
00045 
00046 // [action server client headers]
00047 
00048 
00049 //enum res_lines {One_line_state=0, Two_lines_state, Line_partial_state, Line_unknown_state };
00050 
00057 class TibiDaboPitchEstimatorAlgNode : public algorithm_base::IriBaseAlgorithm<TibiDaboPitchEstimatorAlgorithm>
00058 {
00059   private:
00060     // [publisher attributes]
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    /* ros::Publisher rear_laser_filtrado_publisher_;
00067     sensor_msgs::LaserScan LaserScan_msg_;*/
00068     ros::Publisher MarkerArray_lines_publisher_;
00069     visualization_msgs::MarkerArray MarkerArray_msg_;
00070     
00071                 /*MarkerArray_msg_.markers.clear();
00072                                         MarkerArray_msg_.markers.push_back(distance_front_laser_marker6_);*/
00073                                         
00074 //variables and functions to fill the visualization messages=markers.
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 //node functions.       
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     // [subscriber attributes]
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     // [service attributes]
00109 
00110     // [client attributes]
00111 
00112     // [action server attributes]
00113 
00114     // [action client attributes]
00115 
00116 
00117    //simulation variables
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_; //margen de distancia para filtrar los puntos del front laser en funcion de la interseccion con el suelo.
00123 
00124    //for the debugging mode
00125         
00126         
00127 
00128   public:
00135     TibiDaboPitchEstimatorAlgNode(void);
00136 
00143     ~TibiDaboPitchEstimatorAlgNode(void);
00144 
00145 
00146 
00147     //my stuff
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     // [diagnostic functions]
00191     
00192     // [test functions]
00193 };
00194 
00195 #endif


tibi_dabo_pitch_estimator
Author(s): erepiso
autogenerated on Fri Dec 6 2013 20:37:50