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

Go to the documentation of this file.

#ifndef _c_manage_trajectory_H_
#define _c_manage_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>
#include <numeric>
#include <trajectory_planner/c_trajectory.h>
#include <mtt/TargetList.h>
#include <trajectory_planner/traj_info.h>

typedef struct{
        // Positions
        double x;
        double y;
        // Orientation
        double theta;
}t_desired_coordinates;

typedef struct{
        std::vector<double> x;
        std::vector<double> y;
        int id;
}t_obstacle;


//Defines
#define DONT_INTERSECT    0
#define DO_INTERSECT      1
#define COLLINEAR         2
#define _SPEED_REQUIRED_ 0.25
#define _SPEED_SAFFETY_ 0.05
#define _USE_MATH_DEFINES
#define _TRAJECTORY_LIB_DEBUG_ 1

// Namespaces
using namespace std;

class c_manage_trajectory
{
        public:
                c_manage_trajectory(void)
                {
                        chosen_traj.index=-1;
                        AP.x=-1.0; AP.y=0.0; AP.theta=M_PI/8;
                        current_node=0;
#if _TRAJECTORY_LIB_DEBUG_ 
                        printf("Constructor...\n");
#endif
                };

                ~c_manage_trajectory()
                {
#if _TRAJECTORY_LIB_DEBUG_ 
                        printf("Destructor...\n");
#endif
                };

                // -----  Variables  ------
                std::vector<c_trajectoryPtr> vt; //the traj vector
                t_vehicle_description vehicle_description;
                t_desired_coordinates AP;
                double inter_axis_distance;
                int current_node;
                std::vector<visualization_msgs::Marker> static_marker_vec;
                std::vector<t_obstacle> vo;
                struct{
                        int index;
                }chosen_traj;
                
                // ------  Functions  ------
                t_func_output set_speed_vector(trajectory_planner::traj_info* info);
                t_func_output get_traj_info_msg_from_chosen(trajectory_planner::traj_info* info);
                t_func_output set_vehicle_description(double w, double lb, double lf, double ht, double hb);
                t_func_output set_obstacles(mtt::TargetList& msg);
                int lineSegmentIntersection(double Ax, double Ay,double Bx, double By,double Cx, double Cy,double Dx, double Dy,double *X, double *Y);
                t_func_output compute_DLO(c_trajectoryPtr& trajectory, std::vector<t_obstacle>& vo);
                t_func_output compute_trajectories_scores();
                t_func_output compute_vis_marker_array(visualization_msgs::MarkerArray* marker_array);
                t_func_output create_static_markers();
                t_func_output create_new_trajectory(vector<double> alpha_in, vector<double> arc_in, vector<double> speed_in);
                t_func_output set_inter_axis_distance(double val);
                t_func_output compute_DAP(c_trajectoryPtr& trajectory,t_desired_coordinates& AP);
                void draw_on_node(c_trajectoryPtr& trajectory,std::vector<visualization_msgs::Marker>* marker_vec, int* marker_count,double z_high, double value, double normalized_value, string string_init);
                double compute_ADAP(c_trajectoryPtr& trajectory,t_desired_coordinates& AP, int i);
                t_func_output set_attractor_point(double x, double y, double theta);
                t_func_output compute_global_traj_score(c_trajectoryPtr& trajectory);
                t_func_output compute_chosen_traj();
                int set_chosen_traj(int n);

        private:

};
typedef boost::shared_ptr<c_manage_trajectory> c_manage_trajectoryPtr;
#endif