#include <c_manage_trajectory.h>
Public Member Functions | |
c_manage_trajectory (void) | |
double | compute_ADAP (c_trajectoryPtr &trajectory, t_desired_coordinates &AP, int i) |
Compute the angular difference. | |
t_func_output | compute_chosen_traj () |
Chooses the trajectory. | |
t_func_output | compute_DAP (c_trajectoryPtr &trajectory, t_desired_coordinates &AP) |
Compute the distance to application point. | |
t_func_output | compute_DLO (c_trajectoryPtr &trajectory, std::vector< t_obstacle > &vo) |
Compute the free space. | |
t_func_output | compute_global_traj_score (c_trajectoryPtr &trajectory) |
Computes the trajectory global score. | |
t_func_output | compute_trajectories_scores () |
Compute trajectories scores. | |
t_func_output | compute_vis_marker_array (visualization_msgs::MarkerArray *marker_array) |
Compute the markers array. | |
t_func_output | create_new_trajectory (vector< double > alpha_in, vector< double > arc_in, vector< double > speed_in) |
Create a trajectory. | |
t_func_output | create_static_markers () |
Create a static marker. | |
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) |
Draw some informations about trajectories. | |
t_func_output | get_traj_info_msg_from_chosen (trajectory_planner::traj_info *info) |
Gets the info of chosen nodes. | |
int | lineSegmentIntersection (double Ax, double Ay, double Bx, double By, double Cx, double Cy, double Dx, double Dy, double *X, double *Y) |
Verifies intersections between lines. | |
t_func_output | set_attractor_point (double x, double y, double theta) |
Set the attractor point. | |
int | set_chosen_traj (int n) |
Set the chosen trajectory. | |
t_func_output | set_inter_axis_distance (double val) |
Set the inter-axis distance of the vehicle. | |
t_func_output | set_obstacles (mtt::TargetListPC &msg) |
Sets the obstacles. | |
t_func_output | set_speed_vector (trajectory_planner::traj_info *info) |
Determine the speed vector. | |
t_func_output | set_vehicle_description (double w, double lb, double lf, double ht, double hb) |
Set the vehicle description. | |
~c_manage_trajectory () | |
Public Attributes | |
t_desired_coordinates | AP |
struct { | |
int index | |
} | chosen_traj |
int | current_node |
double | inter_axis_distance |
std::vector < visualization_msgs::Marker > | static_marker_vec |
t_vehicle_description | vehicle_description |
std::vector< t_obstacle > | vo |
std::vector< c_trajectoryPtr > | vt |
Definition at line 86 of file c_manage_trajectory.h.
c_manage_trajectory::c_manage_trajectory | ( | void | ) | [inline] |
Definition at line 89 of file c_manage_trajectory.h.
c_manage_trajectory::~c_manage_trajectory | ( | ) | [inline] |
Definition at line 99 of file c_manage_trajectory.h.
double c_manage_trajectory::compute_ADAP | ( | c_trajectoryPtr & | trajectory, | |
t_desired_coordinates & | AP, | |||
int | i | |||
) |
Compute the angular difference.
trajectory | ||
AP | ||
i |
Definition at line 652 of file c_manage_trajectory.cpp.
t_func_output c_manage_trajectory::compute_chosen_traj | ( | void | ) |
Chooses the trajectory.
Definition at line 346 of file c_manage_trajectory.cpp.
t_func_output c_manage_trajectory::compute_DAP | ( | c_trajectoryPtr & | trajectory, | |
t_desired_coordinates & | AP | |||
) |
Compute the distance to application point.
trajectory | ||
AP |
Definition at line 616 of file c_manage_trajectory.cpp.
t_func_output c_manage_trajectory::compute_DLO | ( | c_trajectoryPtr & | trajectory, | |
std::vector< t_obstacle > & | vo | |||
) |
Compute the free space.
Determines the intersection point of the line segment defined by points A and B with the line segment defined by points C and D. Returns YES if the intersection point was found, and stores that point in X,Y. Returns NO if there is no determinable intersection point, in which case X,Y will be unmodified.
trajectory_planner | ||
vo |
Definition at line 39 of file c_manage_trajectory.cpp.
t_func_output c_manage_trajectory::compute_global_traj_score | ( | c_trajectoryPtr & | trajectory | ) |
Computes the trajectory global score.
trajectory |
Definition at line 301 of file c_manage_trajectory.cpp.
t_func_output c_manage_trajectory::compute_trajectories_scores | ( | void | ) |
Compute trajectories scores.
Definition at line 574 of file c_manage_trajectory.cpp.
t_func_output c_manage_trajectory::compute_vis_marker_array | ( | visualization_msgs::MarkerArray * | marker_array | ) |
Compute the markers array.
marker_array |
Definition at line 390 of file c_manage_trajectory.cpp.
t_func_output c_manage_trajectory::create_new_trajectory | ( | vector< double > | alpha_in, | |
vector< double > | arc_in, | |||
vector< double > | speed_in | |||
) |
Create a trajectory.
alpha_in | ||
arc_in | ||
speed_in |
Definition at line 331 of file c_manage_trajectory.cpp.
t_func_output c_manage_trajectory::create_static_markers | ( | void | ) |
Create a static marker.
Definition at line 372 of file c_manage_trajectory.cpp.
void c_manage_trajectory::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 | |||
) |
Draw some informations about trajectories.
trajectory_planner | ||
marker_vec | ||
marker_count | ||
z_high | ||
value | ||
normalized_value | ||
string_init |
Definition at line 671 of file c_manage_trajectory.cpp.
t_func_output c_manage_trajectory::get_traj_info_msg_from_chosen | ( | trajectory_planner::traj_info * | info | ) |
Gets the info of chosen nodes.
info |
Definition at line 218 of file c_manage_trajectory.cpp.
int c_manage_trajectory::lineSegmentIntersection | ( | double | Ax, | |
double | Ay, | |||
double | Bx, | |||
double | By, | |||
double | Cx, | |||
double | Cy, | |||
double | Dx, | |||
double | Dy, | |||
double * | X, | |||
double * | Y | |||
) |
Verifies intersections between lines.
Ax | ||
Ay | ||
Bx | ||
By | ||
Cx | ||
Cy | ||
Dx | ||
Dy | ||
X | ||
Y |
Definition at line 121 of file c_manage_trajectory.cpp.
t_func_output c_manage_trajectory::set_attractor_point | ( | double | x, | |
double | y, | |||
double | theta | |||
) |
Set the attractor point.
x | ||
y | ||
theta |
Definition at line 289 of file c_manage_trajectory.cpp.
int c_manage_trajectory::set_chosen_traj | ( | int | n | ) |
Set the chosen trajectory.
int | n |
Definition at line 275 of file c_manage_trajectory.cpp.
t_func_output c_manage_trajectory::set_inter_axis_distance | ( | double | val | ) |
Set the inter-axis distance of the vehicle.
val |
Definition at line 318 of file c_manage_trajectory.cpp.
t_func_output c_manage_trajectory::set_obstacles | ( | mtt::TargetListPC & | msg | ) |
Sets the obstacles.
mtt::TargetListPC& | msg |
Definition at line 171 of file c_manage_trajectory.cpp.
t_func_output c_manage_trajectory::set_speed_vector | ( | trajectory_planner::traj_info * | info | ) |
Determine the speed vector.
info |
Definition at line 246 of file c_manage_trajectory.cpp.
t_func_output c_manage_trajectory::set_vehicle_description | ( | double | w, | |
double | lb, | |||
double | lf, | |||
double | ht, | |||
double | hb | |||
) |
Set the vehicle description.
w | ||
lb | ||
lf | ||
ht | ||
hb |
Definition at line 202 of file c_manage_trajectory.cpp.
Definition at line 109 of file c_manage_trajectory.h.
struct { ... } c_manage_trajectory::chosen_traj |
Definition at line 111 of file c_manage_trajectory.h.
Definition at line 115 of file c_manage_trajectory.h.
Definition at line 110 of file c_manage_trajectory.h.
std::vector<visualization_msgs::Marker> c_manage_trajectory::static_marker_vec |
Definition at line 112 of file c_manage_trajectory.h.
Definition at line 108 of file c_manage_trajectory.h.
std::vector<t_obstacle> c_manage_trajectory::vo |
Definition at line 113 of file c_manage_trajectory.h.
std::vector<c_trajectoryPtr> c_manage_trajectory::vt |
Definition at line 104 of file c_manage_trajectory.h.