helper_functions.hpp

Go to the documentation of this file.

#pragma once
/*
 * Copyright (C) 2021-2022 LEIDOS.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */
#include <basic_autonomy/basic_autonomy.hpp>
 
namespace basic_autonomy
{
namespace waypoint_generation
{
    //Small value corresponding to 0.1 mm in meters for comparing distance to a near zero.
    const double epsilon_ = 0.0000001;
    int get_nearest_point_index(const std::vector<lanelet::BasicPoint2d>& points,
                                                 const carma_planning_msgs::msg::VehicleState& state);
 
    size_t get_nearest_point_index(
        const std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint>& trajectory,
        const lanelet::BasicPoint2d& position);
 
    int get_nearest_point_index(const std::vector<PointSpeedPair>& points,
                                                 const carma_planning_msgs::msg::VehicleState& state);
 
    int get_nearest_index_by_downtrack(const std::vector<lanelet::BasicPoint2d>& points, const carma_wm::WorldModelConstPtr& wm, double target_downtrack);
 
    void split_point_speed_pairs(const std::vector<PointSpeedPair>& points,
                                                std::vector<lanelet::BasicPoint2d>* basic_points,
                                                std::vector<double>* speeds);
 
    int get_nearest_index_by_downtrack(const std::vector<PointSpeedPair>& points, const carma_wm::WorldModelConstPtr& wm,
                                      const carma_planning_msgs::msg::VehicleState& state);
 
    int get_nearest_index_by_downtrack(const std::vector<lanelet::BasicPoint2d>& points, const carma_wm::WorldModelConstPtr& wm,
                                      const carma_planning_msgs::msg::VehicleState& state);
 
    std::vector<lanelet::BasicPoint2d> build_chain_centerline(const carma_wm::WorldModelConstPtr &wm,
                                                               lanelet::ConstLanelet pivot,
                                                               double backward_length,
                                                               double forward_length);
 
    void extrapolate_to_length(std::vector<lanelet::BasicPoint2d>& centerline, double target_length, const std::string& description);
 
}
}