-
Notifications
You must be signed in to change notification settings - Fork 124
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #1328 from usdot-fhwa-stol/release/zephyr
Release/zephyr
- Loading branch information
Showing
222 changed files
with
70,382 additions
and
1,690 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,92 @@ | ||
| # | ||
| # Copyright (C) 2021 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. | ||
| # | ||
|
|
||
| cmake_minimum_required(VERSION 2.8.3) | ||
| project(basic_autonomy) | ||
|
|
||
| ## Compile as C++11, supported in ROS Kinetic and newer | ||
| add_compile_options(-std=c++14) | ||
| set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") | ||
| set( CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall") | ||
|
|
||
| set( CATKIN_DEPS | ||
| roscpp | ||
| std_msgs | ||
| cav_msgs | ||
| carma_wm | ||
| ) | ||
|
|
||
| ## Find catkin macros and libraries | ||
| find_package(catkin REQUIRED COMPONENTS | ||
| ${CATKIN_DEPS} | ||
| ) | ||
|
|
||
| ## System dependencies are found with CMake's conventions | ||
| find_package(Boost) | ||
| find_package(Eigen3 REQUIRED) | ||
|
|
||
| ################################### | ||
| ## catkin specific configuration ## | ||
| ################################### | ||
|
|
||
| catkin_package( | ||
| INCLUDE_DIRS include | ||
| CATKIN_DEPENDS ${CATKIN_DEPS} | ||
| LIBRARIES basic_autonomy | ||
| DEPENDS Boost EIGEN3 | ||
| ) | ||
|
|
||
| ########### | ||
| ## Build ## | ||
| ########### | ||
|
|
||
| include_directories( | ||
| include | ||
| ${catkin_INCLUDE_DIRS} | ||
| ${Boost_INCLUDE_DIRS} | ||
| ${EIGEN3_INCLUDE_DIRS} | ||
| ) | ||
|
|
||
| add_library(basic_autonomy | ||
| src/helper_functions.cpp | ||
| ) | ||
|
|
||
| add_dependencies(basic_autonomy ${catkin_EXPORTED_TARGETS}) | ||
| target_link_libraries(basic_autonomy ${catkin_LIBRARIES}) | ||
|
|
||
| #target_link_libraries(basic_autonomy ${catkin_LIBRARIES} ${Boost_LIBRARIES}) | ||
| #add_dependencies(basic_autonomy ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) | ||
|
|
||
|
|
||
| ############# | ||
| ## Install ## | ||
| ############# | ||
|
|
||
| install(DIRECTORY include/${PROJECT_NAME}/ | ||
| DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} | ||
| ) | ||
|
|
||
| ## Install C++ | ||
| install(TARGETS ${PROJECT_NAME} | ||
| ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} | ||
| LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} | ||
| RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} | ||
| ) | ||
|
|
||
| ############# | ||
| ## Testing ## | ||
| ############# | ||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,37 @@ | ||
| #pragma once | ||
| /* | ||
| * Copyright (C) 2021 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 <vector> | ||
| #include <boost/geometry.hpp> | ||
| #include <carma_wm/Geometry.h> | ||
| #include <ros/ros.h> | ||
| #include <cav_msgs/VehicleState.h> | ||
| #include <lanelet2_core/geometry/Point.h> | ||
| #include <carma_wm/WorldModel.h> | ||
|
|
||
|
|
||
| namespace basic_autonomy | ||
| { | ||
| namespace waypoint_generation | ||
| { | ||
| struct PointSpeedPair | ||
| { | ||
| lanelet::BasicPoint2d point; | ||
| double speed = 0; | ||
| }; | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,98 @@ | ||
| #pragma once | ||
| /* | ||
| * Copyright (C) 2021 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.h> | ||
|
|
||
| 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; | ||
| /** | ||
| * \brief Returns the nearest point (in terms of cartesian 2d distance) to the provided vehicle pose in the provided l | ||
| * | ||
| * \param points The points to evaluate | ||
| * \param state The current vehicle state | ||
| * | ||
| * \return index of nearest point in points | ||
| */ | ||
| int get_nearest_point_index(const std::vector<lanelet::BasicPoint2d>& points, | ||
| const cav_msgs::VehicleState& state); | ||
| /** | ||
| * \brief Returns the nearest point (in terms of cartesian 2d distance) to the provided vehicle pose in the provided list | ||
| * | ||
| * \param points The points to evaluate | ||
| * \param state The current vehicle state | ||
| * | ||
| * \return index of nearest point in points | ||
| */ | ||
| int get_nearest_point_index(const std::vector<PointSpeedPair>& points, | ||
| const cav_msgs::VehicleState& state); | ||
| /** | ||
| * \brief Returns the nearest "less than" point to the provided vehicle pose in the provided list by utilizing the downtrack measured along the route | ||
| * NOTE: This function compares the downtrack, provided by routeTrackPos, of each points in the list to get the closest one to the given point's downtrack. | ||
| * Therefore, it is rather costlier method than comparing cartesian distance between the points and getting the closest. This way, however, the function | ||
| * correctly returns the end point's index if the given state, despite being valid, is farther than the given points and can technically be near any of them. | ||
| * | ||
| * \param points BasicLineString2d points | ||
| * \param target_downtrack target downtrack along the route to get index near to | ||
| * | ||
| * \return index of nearest point in points | ||
| */ | ||
| int get_nearest_index_by_downtrack(const std::vector<lanelet::BasicPoint2d>& points, const carma_wm::WorldModelConstPtr& wm, double target_downtrack); | ||
|
|
||
| /** | ||
| * \brief Helper method to split a list of PointSpeedPair into separate point and speed lists | ||
| * \param points Point Speed pair to split | ||
| * \param basic_points points vector to be filled | ||
| * \param speeds speeds vector to be filled | ||
| */ | ||
| void split_point_speed_pairs(const std::vector<PointSpeedPair>& points, | ||
| std::vector<lanelet::BasicPoint2d>* basic_points, | ||
| std::vector<double>* speeds); | ||
| /** | ||
| * \brief Overload: Returns the nearest point to the provided vehicle pose in the provided list by utilizing the downtrack measured along the route | ||
| * NOTE: This function compares the downtrack, provided by routeTrackPos, of each points in the list to get the closest one to the given point's downtrack. | ||
| * Therefore, it is rather costlier method than comparing cartesian distance between the points and getting the closest. This way, however, the function | ||
| * correctly returns the end point's index if the given state, despite being valid, is farther than the given points and can technically be near any of them. | ||
| * | ||
| * \param points The points and speed pairs to evaluate | ||
| * \param state The current vehicle state | ||
| * \param wm The carma world model | ||
| * | ||
| * \return index of nearest point in points | ||
| */ | ||
| int get_nearest_index_by_downtrack(const std::vector<PointSpeedPair>& points, const carma_wm::WorldModelConstPtr& wm, | ||
| const cav_msgs::VehicleState& state); | ||
|
|
||
| /** | ||
| * \brief Overload: Returns the nearest point to the provided vehicle pose in the provided list by utilizing the downtrack measured along the route | ||
| * NOTE: This function compares the downtrack, provided by routeTrackPos, of each points in the list to get the closest one to the given point's downtrack. | ||
| * Therefore, it is rather costlier method than comparing cartesian distance between the points and getting the closest. This way, however, the function | ||
| * correctly returns the end point if the given state, despite being valid, is farther than the given points and can technically be near any of them. | ||
| * | ||
| * \param points The points to evaluate | ||
| * \param state The current vehicle state | ||
| * \param wm The carma world model | ||
| * | ||
| * \return index of nearest point in points | ||
| */ | ||
| int get_nearest_index_by_downtrack(const std::vector<lanelet::BasicPoint2d>& points, const carma_wm::WorldModelConstPtr& wm, | ||
| const cav_msgs::VehicleState& state); | ||
|
|
||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,31 @@ | ||
| <?xml version="1.0"?> | ||
|
|
||
| <!-- | ||
| Copyright (C) 2021 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. | ||
| --> | ||
|
|
||
| <package format="3"> | ||
| <name>basic_autonomy</name> | ||
| <version>3.2.0</version> | ||
| <description>The Basic Autonomy Library includes logic for waypoint generation in CARMA Platform.</description> | ||
| <maintainer email="[email protected]">carma</maintainer> | ||
| <license>Apache License 2.0</license> | ||
| <author email="[email protected]">carma</author> | ||
| <buildtool_depend>catkin</buildtool_depend> | ||
| <depend>roscpp</depend> | ||
| <depend>std_msgs</depend> | ||
| <depend>cav_msgs</depend> | ||
| <depend>carma_wm</depend> | ||
| </package> |
Oops, something went wrong.