tensor_field_nav_core: TfCore.h Source File

Go to the documentation of this file.
00001 /***
00002  The core code for robot control including path generation, topology branch selection, tensor field update, obastacle avoidance
00003  ***/
00004 #ifndef _TfCore_H
00005 #define _TfCore_H
00006 
00007 #include <GL/glut.h> 
00008 #include "Parameters.h"
00009 #include "dataStructure.h"
00010 #include "icVector.h"
00011 #include "QuadMesh.h"
00012 #include "Util.h"
00013 #include <std_msgs/String.h>
00014 #include "EvenStreamlinePlace.h"
00015 #include <ros/ros.h>
00016 #include <tf/transform_listener.h>
00017 #include <std_msgs/Empty.h>
00018 #include <nav_msgs/Path.h>
00019 #include <geometry_msgs/PoseStamped.h>
00020 #include <opencv2/highgui/highgui.hpp>
00021 #include <opencv2/imgproc/imgproc.hpp>
00022 #include <image_transport/image_transport.h>
00023 #include <cv_bridge/cv_bridge.h>
00024 #include <std_msgs/Int32.h>
00025 #include "KeyFrameList.h"
00026 #include "ParaThread.h"
00027 #include "VertTimeList.h"
00028 #include "Numerical.h"
00029 #include "mst.h"
00030 #include <nav_msgs/OccupancyGrid.h>
00031 #include "pure_pursuit_controller/executePath.h"
00032 #include "pure_pursuit_controller/cancelPath.h"
00033 #include "pure_pursuit_controller/cutPath.h"
00034 #include "pure_pursuit_controller/recoverPath.h"
00035 #include "std_msgs/Float64MultiArray.h"
00036 #include "tensor_field_nav_core/CalPathInfoGain.h"
00037 
00038 // CTfCore
00039 class TfCore
00040 {
00041 public:
00042     TfCore(ros::NodeHandle private_nh_ = ros::NodeHandle("~"));
00043     ~TfCore();
00044 
00045         // Attributes
00046 public:
00047         //
00048         double  ten_tmax ;
00049         double  ten_dmax ;
00050         int    iframe ; 
00051         int    Npat  ;
00052         int    alpha  ;
00053         float  tmax ;
00054         float  dmax ;
00055         double trans_x,trans_y,zoom_factor;
00056         float inverse_tran[16];
00057         int curMaxNumTenRegElems;
00058         int MaxNumDegeneratePts;
00059         int major_iframe;
00060         int ntenelems;
00061         int nten_regelems;
00062         int ndegpts;
00063         DegeneratePt *degpts;
00064         TenRegularElem *ten_regularelems;
00065         GLuint tentextnames[16];
00066         GLubyte major_tex1[NPIX][NPIX][3], major_tex2[NPIX][NPIX][3], major_tex[NPIX][NPIX][3],
00067                 minor_tex1[NPIX][NPIX][3], minor_tex2[NPIX][NPIX][3], minor_tex[NPIX][NPIX][3],
00068                 major_alpha_map[NPIX][NPIX][3], minor_alpha_map[NPIX][NPIX][3];
00069 
00070         GLubyte major_temp[NPIX][NPIX][4], minor_temp[NPIX][NPIX][4];
00071 
00072         /*the quad mesh object for the tensor field design*/
00073         QuadMesh *quadmesh;
00075         Degenerate_Design *ten_designelems;
00076         int curMaxNumTenDesignElems;
00077 
00078         int ndegenerate_tris;
00079         int *degenerate_tris;
00080 
00081         EvenStreamlinePlace *major_path;
00082     //2016_12_14
00083     int NUM_Slices;
00084     KeyFrameList *keyframes;
00085     bool enRelaxKeyframeOn;
00086     int InterpScheme;
00087     int curSliceID;
00088 
00089     std::vector<ParaThread *> m_myThreads;
00090     //
00091     VertTimeList *vertTimeList;
00092     Vec_INT *ija_p;
00093     Vec_DP *sa_p;
00094     icVector2 *vecsTime;
00096     bool showIBFVOn;
00097     bool showRegularElemOn;
00098     bool showGridOn;
00099     bool showSingularitiesOn;
00100     bool showMajorTenLine;
00101     bool isReceiveNewMap;
00102     double realWorld_to_field_scale;
00103     double realWorld_to_field_offset_x;
00104     double realWorld_to_field_offset_y;
00106 
00107     std::vector<std::vector<cv::Point> > contours;
00108     std::vector<std::vector<cv::Point2f> > dirMap2Field;
00109     std::vector<std::vector<cv::Point2f> > dirMapInWorld;
00110     std::vector<std::vector<cv::Point2f> > contours2Field;
00111     std::vector<std::vector<cv::Point2f> > contoursInWorld;
00112     std::vector<std::vector<icVector2> > constraints_vecs;
00113 
00114     icVector2 m_robotDirect;
00115     icVector2 m_globalDirect;
00116     Seed *robot_loc;
00117     float robot_world_pos[2];
00118     bool recoverMode;
00119     bool ifReachTrisector;
00120     bool ifCancelPath;
00121     float leftBottom[2];
00122     float rightTop[2];
00123     std::vector<Location> bresenham_line_points;
00124     int selected_target_tri;
00125     bool reGenPath;
00126     icVector2 outside_push;
00128     //ros
00129     ros::NodeHandle nh_;
00130     tf::TransformListener listener;
00131     ros::Subscriber gridmap_sub;
00132     ros::Subscriber target_sub;
00133     ros::Subscriber first_point_sub;
00134     ros::Publisher safetyWayPointNum_pub;
00135     ros::Subscriber frontierPoints_sub;
00136     ros::Subscriber finishGoTri;
00137     image_transport::Publisher rgb_pub;
00138     image_transport::ImageTransport m_it;
00139     nav_msgs::OccupancyGrid dirMap;
00140     ros::ServiceClient goTriExecuteclient;
00141     ros::ServiceClient pathExecuteClient;
00142     ros::ServiceClient pathCancelClient;
00143     ros::ServiceClient pathCutClient;
00144     ros::ServiceClient reqCalDegptPathsInfoClient;
00145 
00146     unsigned char  *rgb_im;
00147     std_msgs::Float64MultiArray frontierPoints;
00148 
00149     bool ifFinish_goTri;
00150 
00151 
00152     //
00153     //topo conection
00154     std::vector<DegeneratePt> validDegpts;
00155     std::vector<DegeneratePt> validTriDegpts;
00156     EvenStreamlinePlace *separatrices;
00157     std::vector<vector<int> > degptsPathsInfoGain;
00158     int targetTrisectorIndex;
00159     std::set<pair<int,int> > degptsLinks;
00160 
00161     //
00162     //key paramters
00163     float robotInitialPos_x;
00164     float robotInitialPos_y;
00165     std::string rgbTopicName;
00166     std::string gridMapTopicName;
00167     std::string worldFrameId;
00168     std::string baseFrameId;
00169     float safeDistance;
00170     int interFrameNum;
00171     float interFrameTime;
00172     //
00174         // Operations for OpenGl window setting and displaying
00175 public:
00176     //opengl
00177     void TfCoreInit();
00179         int InitGL();              //initialize the OpenGl envrionment
00180         void tensor_init_tex();
00181         void init_degpts();
00182         void init_regular_ten_designelems();
00184         int DrawGLScene(GLenum mode);       //The same function as display routine to show the visual effect in the opengl window
00185         void cal_inverse_transform();
00186         void transform_fun();
00187         int without_anti_aliasing(GLenum mode);
00188         void make_tens_Patterns(void);
00189     void makePatterns(void);
00191         void render_majorfield_quad();
00192         void major_vis_quad();
00193     void render_ibfv_tens_quad( bool x_y);
00194     void render_alpha_map_quad();
00195         void render_tensor_blend();
00196     void render_tensor_final();
00197         void drawSolidRect_size(double cx, double cy, double R);
00198         void drawSolidCircle_size(double cx, double cy, double R);
00199         void display_tenRegElem(GLenum mode);
00200         void display_design_grid();
00201         void display_degenerate_pts(GLenum mode);
00202     void display_valid_degenerate_pts(GLenum mode);
00203     void display_valid_trisectors(GLenum mode);
00204         void draw_hollow_circle_size(double cx, double cy, double R);
00205     void display_separatrices(GLenum mode);
00207     //tensor field
00208     void set_ten_regBasis(double x, double y, int type);
00209         unsigned char get_region_id(double x, double y);
00210     void set_ten_regDir(double x, double y);
00211     void set_ten_regDir(double x, double y,icVector2 &vec);
00212     void cal_tensorvals_quad();
00213     void get_tensor(double x, double y, double t[4]);
00214         void init_ten_designelems();
00215         void cal_all_eigenvecs_quad();
00216         void cal_eigenvecs_onevert_quad(int ver);
00217         void cal_eigen_vector_sym(icMatrix2x2 m, icVector2 ev[2]);
00218         void normalized_tensorfield_quad();
00219         void locate_degpts_cells_tranvec_quad(void);
00220         void compute_degpts_pos_tranvec_quad();
00221         void compute_onedegpt_pos_tranvec_quad(int cellid, double &x, double &y);
00222         void compute_a_alongx_degptlocate(double &a, icVector2 v0, icVector2 v1, icVector2 v2, icVector2 v3,
00223                 icVector2 &v0v1, icVector2 &v2v3);
00224     void compute_separatrixVec_degpt_quad(int degpt_index);
00225         void display_major_tenlines(GLenum mode);
00226     void setRegularElems();
00227 
00228     //time-vary tensorfield
00229   void update_tensor_field();
00230   void init_keyframeList(int size = 2);
00231   void set_cur_as_keyframe(int frameID);
00232   void update_cur_TF_keyframe();
00233   void cal_TF_at_slice_keyframe_interp(int which_slice);
00234   void copy_from_keyframe(int keyframelistPos);
00235   void cal_TF_at_inbetween_slice_keyframe_interp(double ratio, int start, int end);
00236   void Laplace_relax_timeDep();
00237   void init_vertTimeList();
00238   void add_verts_to_vertTimeList();
00239   void set_constraints_keyframes();
00240   void count_nonconstrained_verts();
00241   void con_linear_Sys_spatioTemp(Vec_DP &tsa, Vec_INT &tija, Mat_DP &bound_v);
00242   void get_laplace_TVTF(int which_slice);
00243 
00244 
00246 
00248     //ros
00249     void gridMap_callback(const nav_msgs::OccupancyGrid &msg);
00250     void goTri_callback(const std_msgs::String &msg);
00251     void frontier_point_callback(const std_msgs::Float64MultiArray &msg);
00252     void listen_to_robot_loc();
00253     void add_robot_wayPoints();
00254     void add_separatrix_points_toRobot(Trajectory * target_traj);
00255     void publish_image();
00256     void ensure_robot_safety();
00257     void cancelPath();
00258     void cut_robot_path();
00259 
00260     //topo connection
00261     void filterDegpts();
00262     void gen_separatrix();
00263     void cal_sep_infoGain();
00264     bool check_bypass_trisector();
00265     bool check_reach_trisector();
00266     void display_trisectorVec(GLenum mode);
00267     void cal_separatrix_vec();
00268     void init_separatrices();
00269     void reset_separatrices();
00270     void  display_bresenham_line();
00271     bool check_line_insect_obstacle();
00272     bool insertLinks(int i, int j);
00273     std::map<pair<int ,int>,float> search_degpts_mst(std::set<int> connect_degpts, int target_tri_index);
00274     std::set<int> find_connect_degpts(int target_tri_index);
00275     Trajectory * select_a_branch(std::map<pair<int,int>,float> cur_mst, int target_tri_index);
00276     void select_target_trisector_branch_move();
00277 
00278     //parallel
00279     void create_paraThread();
00280     void parallel_update_tensorField();
00281     void split_tensorField( int i, int threadNum, int &x1, int &x2);
00282     void parallel_cal_tensorvals_quad(int &x1, int &x2);
00283       //
00284 
00286     void set_obstacles();
00287     void play_all_frames();
00288     void draw_map_contour();
00289     void get_obstacles_contour();
00290     void reset_regular_and_degenrateElem();
00291     void gen_major_path();
00292     void reset_major_path();
00293     void set_robot_loc(double x,double y);
00294     void init_majorPath();
00295     void locat_point_inMap(double pos[2],cv::Point2i &mapLoc);
00296     bool check_reach_wedge();
00298 
00299 };
00300 
00301 
00302 
00303 #endif