27 #ifndef _c_manage_trajectory_H_
28 #define _c_manage_trajectory_H_
43 #include <visualization_msgs/Marker.h>
44 #include <visualization_msgs/MarkerArray.h>
45 #include <tf/transform_listener.h>
46 #include <pcl_ros/transforms.h>
47 #include <pcl/conversions.h>
48 #include <pcl/point_cloud.h>
49 #include <pcl/point_types.h>
50 #include <pcl/segmentation/extract_polygonal_prism_data.h>
51 #include <pcl/filters/extract_indices.h>
52 #include <pcl/filters/project_inliers.h>
53 #include <pcl/filters/voxel_grid.h>
56 #include <mtt/TargetListPC.h>
57 #include <trajectory_planner/traj_info.h>
58 #include <pcl_conversions/pcl_conversions.h>
69 std::vector<double>
x;
70 std::vector<double>
y;
76 #define DONT_INTERSECT 0
77 #define DO_INTERSECT 1
79 #define _SPEED_REQUIRED_ 0.25
80 #define _SPEED_SAFFETY_ 0.05
81 #define _USE_MATH_DEFINES
82 #define _TRAJECTORY_LIB_DEBUG_ 1
93 AP.x=-1.0; AP.y=0.0; AP.theta=M_PI/8;
95 #if _TRAJECTORY_LIB_DEBUG_
96 printf(
"Constructor...\n");
102 #if _TRAJECTORY_LIB_DEBUG_
103 printf(
"Destructor...\n");
108 std::vector<c_trajectoryPtr> vt;
114 std::vector<t_obstacle>
vo;
121 t_func_output get_traj_info_msg_from_chosen(trajectory_planner::traj_info*
info);
122 t_func_output set_vehicle_description(
double w,
double lb,
double lf,
double ht,
double hb);
124 int lineSegmentIntersection(
double Ax,
double Ay,
double Bx,
double By,
double Cx,
double Cy,
double Dx,
double Dy,
double *X,
double *Y);
127 t_func_output compute_vis_marker_array(visualization_msgs::MarkerArray* marker_array);
129 t_func_output create_new_trajectory(vector<double> alpha_in, vector<double> arc_in, vector<double> speed_in);
132 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);
134 t_func_output set_attractor_point(
double x,
double y,
double theta);
137 int set_chosen_traj(
int n);
vector< double > set_speed_vector(boost::shared_ptr< c_trajectory > t)
Determine the speed vector.
boost::shared_ptr< c_trajectory > c_trajectoryPtr
c_manage_trajectory(void)
class which generates a few number of trajectories from an input file
trajectory_planner::traj_info info
boost::shared_ptr< c_manage_trajectory > c_manage_trajectoryPtr
std::vector< visualization_msgs::Marker > static_marker_vec
std::vector< t_obstacle > vo
double inter_axis_distance
t_vehicle_description vehicle_description