/home/joel/ros_workspace/lar3/planning/trajectory_planner/include/trajectory_planner/c_trajectory.h

Go to the documentation of this file.

#ifndef _c_trajectory_H_
#define _c_trajectory_H_

//INCLUDES
#include <ros/ros.h>
#include <stdio.h>
#include <vector>
#include <math.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <tf/transform_listener.h>
#include <pcl_ros/transforms.h>
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/segmentation/extract_polygonal_prism_data.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/filters/voxel_grid.h>

typedef enum {SUCCESS, FAILURE}t_func_output;

//Defines
#define PFLN printf("FILE %s LINE %d\n",__FILE__, __LINE__);
#define _USE_MATH_DEFINES
#define _TRAJECTORY_LIB_DEBUG_ 1


// Namespaces
using namespace std;

typedef struct{
double width;
double lenght_back;
double lenght_front;
double height_top;
double height_bottom;
}t_vehicle_description;


typedef struct{
        double x[2];
        double y[2];
}t_lines;


typedef struct{
        double x;
        double y;
}t_point;

class c_trajectory
{
        public:
                c_trajectory(double D_in)
                {
                        D = D_in;
                        #if _TRAJECTORY_LIB_DEBUG_ 
                                printf("Constructor - Defining D...\n");
                        #endif
                };

                ~c_trajectory()
                {
                        #if _TRAJECTORY_LIB_DEBUG_ 
                                printf("Destructor...\n");
                        #endif
                };
        
                // -----  Variables  ------
                // Local coordinates
                vector<vector<t_lines> > v_lines;
                vector<t_point> collision_pts;
                vector<double> lx;
                vector<double> ly;
                vector<double> ltheta;
                vector<tf::Transform> ltrans;

                // Global coordinates
                vector<double> x;
                vector<double> y;
                vector<double> theta;
                
                struct{
                        double DAP;   //distance to application point
                        double ADAP;  //angular difference related to the application point
                        double DLO;
                        double DAPnorm;  // normalized
                        double ADAPnorm;  // normalized
                        double DLOnorm;
                        double FS;
                        double overall_norm; 
                }score;
                
                int closest_node;
                
                // Input parameters
                vector<double> alpha;
                vector<double> arc;
                vector<double> speed;
                double D;
                
                // Other paramenters
                vector<double> total_arc;
                
                // ------  Functions  ------
                t_func_output generate(vector<double> alpha_in, vector<double> arc_in, vector<double> speed_in, t_vehicle_description& vd);
                t_func_output compute_transformation(); 
                void create_markers(std::vector<visualization_msgs::Marker>* marker_vec, int* marker_count, int num_traj);


        private:
                
                
};
typedef boost::shared_ptr<c_trajectory> c_trajectoryPtr;

#endif