Concatenate the best next path to the current path
| 748 | |
| 749 | // Concatenate the best next path to the current path |
| 750 | void FissPlannerNode::concatPath(const FrenetPath& next_traj, const int traj_max_size, const int traj_min_size, const double wp_max_seperation, const double wp_min_seperation) |
| 751 | { |
| 752 | size_t diff = 0; |
| 753 | if (curr_trajectory_.x.size() <= traj_min_size) |
| 754 | { |
| 755 | diff = std::min(traj_max_size - curr_trajectory_.x.size(), next_traj.x.size()); |
| 756 | } |
| 757 | |
| 758 | // Concatenate the best path to the output path |
| 759 | for (size_t i = 0; i < diff; i++) |
| 760 | { |
| 761 | // double wp_seperation; |
| 762 | |
| 763 | // // Check if the separation between adjacent waypoint are permitted |
| 764 | // if (!curr_trajectory_.x.empty() && !curr_trajectory_.y.empty()) |
| 765 | // { |
| 766 | // wp_seperation = distance(curr_trajectory_.x.back(), curr_trajectory_.y.back(), next_traj.x[i], next_traj.y[i]); |
| 767 | // } |
| 768 | // else |
| 769 | // { |
| 770 | // wp_seperation = distance(next_traj.x[i], next_traj.y[i], next_traj.x[i+1], next_traj.y[i+1]); |
| 771 | // } |
| 772 | |
| 773 | // // If the separation is too big/small, reject point onward |
| 774 | // if (wp_seperation >= wp_max_seperation || wp_seperation <= wp_min_seperation) |
| 775 | // { |
| 776 | // ROS_WARN("Local Planner: waypoint out of bound, rejected"); |
| 777 | // regenerate_flag_ = true; |
| 778 | // break; |
| 779 | // } |
| 780 | if (std::isnormal(next_traj.x[i]) && std::isnormal(next_traj.y[i]) && std::isnormal(next_traj.yaw[i]) && |
| 781 | std::isnormal(next_traj.s_d[i]) && std::isnormal(next_traj.d_d[i])) |
| 782 | { |
| 783 | curr_trajectory_.x.push_back(next_traj.x[i]); |
| 784 | curr_trajectory_.y.push_back(next_traj.y[i]); |
| 785 | curr_trajectory_.yaw.push_back(next_traj.yaw[i]); |
| 786 | curr_trajectory_.v.push_back(std::hypot(next_traj.s_d[i], next_traj.d_d[i])); |
| 787 | } |
| 788 | } |
| 789 | |
| 790 | // Calculate control outputs and Erase the point that have been executed |
| 791 | if (!curr_trajectory_.x.empty() && !curr_trajectory_.y.empty()) |
| 792 | { |
| 793 | // Calculate steering angle |
| 794 | updateVehicleFrontAxleState(); |
| 795 | const int next_frontlink_wp_id = nextWaypoint(frontaxle_state_, curr_trajectory_); |
| 796 | // Calculate Control Outputs |
| 797 | if (calculateControlOutput(next_frontlink_wp_id, frontaxle_state_)) |
| 798 | { |
| 799 | // Publish steering angle |
| 800 | publishVehicleCmd(acceleration_, steering_angle_); |
| 801 | } |
| 802 | else |
| 803 | { |
| 804 | ROS_ERROR("Local Planner: No output steering angle"); |
| 805 | publishVehicleCmd(-1.0, 0.0); // Publish empty control output |
| 806 | } |
| 807 |
nothing calls this directly
no test coverage detected