#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::TargetList &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 60 of file c_manage_trajectory.h.
c_manage_trajectory::c_manage_trajectory | ( | void | ) | [inline] |
Definition at line 63 of file c_manage_trajectory.h.
c_manage_trajectory::~c_manage_trajectory | ( | ) | [inline] |
Definition at line 73 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.
c_trajectoryPtr& | trajectory |
t_desired_coordinates& | AP |
int | i |
Definition at line 615 of file c_manage_trajectory.cpp.
t_func_output c_manage_trajectory::compute_chosen_traj | ( | void | ) |
Chooses the trajectory.
void |
Definition at line 312 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.
boost::shared_ptr<c_trajectory> | trajectory |
t_desired_coordinates& | AP |
Definition at line 580 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.
c_trajectoryPtr& | trajectory_planner |
std::vector<t_obstacle>& | vo |
Definition at line 12 of file c_manage_trajectory.cpp.
t_func_output c_manage_trajectory::compute_global_traj_score | ( | c_trajectoryPtr & | trajectory | ) |
Computes the trajectory global score.
c_trajectoryPtr& | trajectory |
Definition at line 268 of file c_manage_trajectory.cpp.
t_func_output c_manage_trajectory::compute_trajectories_scores | ( | void | ) |
Compute trajectories scores.
void |
Definition at line 539 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.
visualization_msgs::MarkerArray* | marker_array |
Definition at line 355 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.
vector<double> | alpha_in |
vector<double> | arc_in |
vector<double> | speed_in |
Definition at line 297 of file c_manage_trajectory.cpp.
t_func_output c_manage_trajectory::create_static_markers | ( | void | ) |
Create a static marker.
void |
Definition at line 338 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.
c_trajectoryPtr& | trajectory_planner |
std::vector<visualization_msgs::Marker>* | marker_vec |
int* | marker_count |
double | z_high |
double | value |
double | normalized_value |
string | string_init |
Definition at line 634 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.
trajectory_planner::traj_info* | info |
Definition at line 188 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.
double | Ax |
double | Ay |
double | Bx |
double | By |
double | Cx |
double | Cy |
double | Dx |
double | Dy |
double | *X |
double | *Y |
Definition at line 93 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.
double | x |
double | y |
double | theta |
Definition at line 257 of file c_manage_trajectory.cpp.
int c_manage_trajectory::set_chosen_traj | ( | int | n | ) |
Set the chosen trajectory.
int | n |
Definition at line 244 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.
double | val |
Definition at line 284 of file c_manage_trajectory.cpp.
t_func_output c_manage_trajectory::set_obstacles | ( | mtt::TargetList & | msg | ) |
Sets the obstacles.
mtt::TargetList& | msg |
Definition at line 143 of file c_manage_trajectory.cpp.
t_func_output c_manage_trajectory::set_speed_vector | ( | trajectory_planner::traj_info * | info | ) |
Determine the speed vector.
trajectory_planner::traj_info* | info |
Definition at line 215 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.
double | w |
double | lb |
double | lf |
double | ht |
double | hb |
Definition at line 173 of file c_manage_trajectory.cpp.
Definition at line 83 of file c_manage_trajectory.h.
struct { ... } c_manage_trajectory::chosen_traj |
Definition at line 85 of file c_manage_trajectory.h.
Definition at line 89 of file c_manage_trajectory.h.
Definition at line 84 of file c_manage_trajectory.h.
std::vector<visualization_msgs::Marker> c_manage_trajectory::static_marker_vec |
Definition at line 86 of file c_manage_trajectory.h.
Definition at line 82 of file c_manage_trajectory.h.
std::vector<t_obstacle> c_manage_trajectory::vo |
Definition at line 87 of file c_manage_trajectory.h.
std::vector<c_trajectoryPtr> c_manage_trajectory::vt |
Definition at line 78 of file c_manage_trajectory.h.