Skip to content

Commit

Permalink
Lazy Theta Star considering EDF properties
Browse files Browse the repository at this point in the history
  • Loading branch information
@jcobano
jcobano committed Jan 22, 2024
1 parent 1067e6b commit d683193
Show file tree
Hide file tree
Showing 42 changed files with 2,999 additions and 2,325 deletions.
202 changes: 101 additions & 101 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,8 @@ project(heuristic_planners)

add_compile_options(-std=c++17 -Wall -Werror -Wextra -pedantic )

option(BUILD_DOC "Build documentation " OFF)
option(BUILD_DOC "Build documentation " ON)
option(BUILD_ROS_SUPPORT "Build with ROS Support" ON)
option(OPTIMIZE_FLAG "Enable Compiler Optimization for Runtime Performance" ON)
option(BUILD_DEBUG "Build debug features " OFF)
option(BUILD_COMPUTE_STATS "Build Algorithms with statistics" ON)
Expand Down Expand Up @@ -45,33 +46,21 @@ unset(BUILD_DOC CACHE)

find_package(Boost)

add_compile_definitions(ROS)
# find dependencies
set(PROJECT_DEPENDENCIES
ament_cmake
ament_cmake_ros
rclcpp
std_msgs
std_srvs
geometry_msgs
nav_msgs
visualization_msgs
builtin_interfaces
pcl_conversions
pcl_ros
octomap_ros
nav2_costmap_2d
trajectory_msgs
tf2
tf2_ros
tf2_geometry_msgs
)


foreach(DEPENDENCY ${PROJECT_DEPENDENCIES})
find_package(${DEPENDENCY} REQUIRED)
endforeach()

if(BUILD_ROS_SUPPORT)
add_compile_definitions(ROS)
## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
std_msgs
geometry_msgs
nav_msgs
visualization_msgs
message_generation
roscpp
pcl_conversions
pcl_ros
octomap_ros
costmap_2d
)
#Eigen is used to calculate metrics parameters
find_package (Eigen3 REQUIRED NO_MODULE)
find_package(OpenSSL REQUIRED)
Expand All @@ -83,35 +72,48 @@ endforeach()
################################################
## Declare ROS messages, services and actions ##
################################################
# catkin_python_setup()
catkin_python_setup()

## Generate services in the 'srv' folder
find_package(rosidl_default_generators REQUIRED)

set(SRVS_FILES
"srv/GetPath.srv"
"srv/SetAlgorithm.srv"
add_service_files(
FILES
GetPath.srv
SetAlgorithm.srv
)

## Generate added messages and services with any dependencies listed here

rosidl_generate_interfaces(${PROJECT_NAME}
${SRVS_FILES}
DEPENDENCIES geometry_msgs std_msgs
generate_messages(
DEPENDENCIES
geometry_msgs std_msgs
)

# list(APPEND PROJECT_DEPENDENCIES ${PROJECT_NAME}_msgs)

##########
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
LIBRARIES AlgorithmBase AStar AStar_Gradient AStar_EDF AStarM1 AStarM2 ThetaStar ThetaStarM1 ThetaStarM2 LazyThetaStar LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2 LazyThetaStar_Gradient LazyThetaStar_EDF
#
CATKIN_DEPENDS std_msgs visualization_msgs geometry_msgs nav_msgs roscpp message_runtime costmap_2d
# DEPENDS system_lib
)
endif()
###########
## Build ##
###########

include_directories(
include
include/Planners
${catkin_INCLUDE_DIRS}
${CMAKE_CURRENT_SOURCE_DIR}/include/voro++-0.4.6
)

if( BUILD_VOROCPP )
list(APPEND VORO_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/include/voro++-0.4.6/src/voro++.cc
${CMAKE_CURRENT_SOURCE_DIR}/include/voro++-0.4.6/src/cell.cc
Expand All @@ -131,8 +133,6 @@ if( BUILD_VOROCPP )
add_executable(voro++ include/voro++-0.4.6/src/cmd_line.cc)
target_link_libraries(voro++ Voro++)
endif()


##This is True by default but it's good to have it here as a reminder
set(BUILD_SHARED_LIBS TRUE)
list(APPEND ${PROJECT_NAME}_UTILS_SOURCES src/utils/geometry_utils.cpp
Expand All @@ -141,7 +141,9 @@ list(APPEND ${PROJECT_NAME}_UTILS_SOURCES src/utils/geometry_utils.cpp
src/utils/utils.cpp
src/utils/metrics.cpp
)
if(BUILD_ROS_SUPPORT)
list(APPEND ${PROJECT_NAME}_UTILS_SOURCES src/utils/ros/ROSInterfaces.cpp)
endif()

add_library(AlgorithmBase src/Planners/AlgorithmBase.cpp
${${PROJECT_NAME}_UTILS_SOURCES})
Expand All @@ -151,6 +153,8 @@ add_library(AStar src/Planners/AStar.cpp
${${PROJECT_NAME}_UTILS_SOURCES}
)

add_library(AStar_Gradient src/Planners/AStar_Gradient.cpp )
add_library(AStar_EDF src/Planners/AStar_EDF.cpp )
add_library(AStarM1 src/Planners/AStarM1.cpp )
add_library(AStarM2 src/Planners/AStarM2.cpp )
add_library(ThetaStar src/Planners/ThetaStar.cpp )
Expand All @@ -160,73 +164,69 @@ add_library(LazyThetaStar src/Planners/LazyThetaStar.cpp )
add_library(LazyThetaStarM1 src/Planners/LazyThetaStarM1.cpp )
add_library(LazyThetaStarM1Mod src/Planners/LazyThetaStarM1Mod.cpp )
add_library(LazyThetaStarM2 src/Planners/LazyThetaStarM2.cpp )
add_library(LazyThetaStar_Gradient src/Planners/LazyThetaStar_Gradient.cpp )
add_library(LazyThetaStar_EDF src/Planners/LazyThetaStar_EDF.cpp )


list(APPEND ${PROJECT_NAME}_LIBRARIES AlgorithmBase AStar AStarM1 AStarM2 ThetaStar ThetaStarM1 ThetaStarM2 LazyThetaStar LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2)

foreach(LIB ${${PROJECT_NAME}_LIBRARIES})
message("Building ${LIB}")
target_include_directories(${LIB}
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
ament_target_dependencies(${LIB} ${PROJECT_DEPENDENCIES})
ament_export_targets(export_${LIB} HAS_LIBRARY_TARGET)
ament_export_dependencies(${LIB} ${PROJECT_DEPENDENCIES})
target_link_libraries(${LIB} ${Boost_LIBRARIES})
endforeach()


add_executable(planner_ros_node src/ROS/planner_ros_node.cpp )
ament_target_dependencies(planner_ros_node ${PROJECT_DEPENDENCIES})
# rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}" "rosidl_typesupport_cpp")
# target_link_libraries(planner_ros_node "${cpp_typesupport_target}")
rosidl_target_interfaces(planner_ros_node
${PROJECT_NAME} "rosidl_typesupport_cpp")
target_link_libraries(planner_ros_node ${${PROJECT_NAME}_LIBRARIES} -lstdc++fs Eigen3::Eigen -lcrypto -lssl)

#add_executable(local_planner_node src/ROS/local_planner_node.cpp src/Planners/LocalPlanner.cpp include/Planners/LocalPlanner.hpp)
#ament_target_dependencies(local_planner_node ${PROJECT_DEPENDENCIES})
#rosidl_target_interfaces(local_planner_node
# ${PROJECT_NAME} "rosidl_typesupport_cpp")
#target_link_libraries(local_planner_node ${${PROJECT_NAME}_LIBRARIES} -lstdc++fs Eigen3::Eigen -lcrypto -lssl)

#rosidl_target_interfaces(local_planner_node
# ${PROJECT_NAME} "rosidl_typesupport_cpp")
#target_link_libraries(local_planner_node ${${PROJECT_NAME}_LIBRARIES} -lstdc++fs Eigen3::Eigen -lcrypto -lssl)

install(TARGETS
planner_ros_node
#local_planner_node
DESTINATION lib/${PROJECT_NAME})

list(APPEND ${PROJECT_NAME}_LIBRARIES AlgorithmBase AStar AStar_Gradient AStar_EDF AStarM1 AStarM2 ThetaStar ThetaStarM1 ThetaStarM2 LazyThetaStar LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2 LazyThetaStar_Gradient LazyThetaStar_EDF)
target_link_libraries(${${PROJECT_NAME}_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
add_dependencies( ${${PROJECT_NAME}_LIBRARIES} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
list(APPEND ${PROJECT_NAME}_TARGETS ${${PROJECT_NAME}_LIBRARIES})

if(BUILD_ROS_SUPPORT)
add_executable(planner_ros_node src/ROS/planner_ros_node.cpp )
add_dependencies(planner_ros_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_LIBRARIES})
target_link_libraries(planner_ros_node ${catkin_LIBRARIES} ${${PROJECT_NAME}_LIBRARIES} -lstdc++fs Eigen3::Eigen -lcrypto -lssl)
list(APPEND ${PROJECT_NAME}_TARGETS planner_ros_node)
#############
## Install ##
#############

ament_export_dependencies(${PROJECT_DEPENDENCIES})

install(
DIRECTORY include/
DESTINATION include
)
## Mark executables and/or libraries for installation
install(TARGETS ${${PROJECT_NAME}_TARGETS}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

catkin_install_python(PROGRAMS scripts/test_algorithms.py
scripts/test_algorithms_compare.py
scripts/test_algorithms_performance.py
scripts/test_algorithms_pseudo_random_paths.py
scripts/compare_trajectories_rviz.py
scripts/plan_caller.py
scripts/generate2d_random_map.py
setup.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

foreach(LIB ${${PROJECT_NAME}_LIBRARIES})
install(
TARGETS ${LIB}
EXPORT export_${LIB}
ARCHIVE DESTINATION libs/${LIB}
LIBRARY DESTINATION libs/${LIB}
RUNTIME DESTINATION bin
## Mark cpp header files for installation
install(DIRECTORY include/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.hpp"
)
endforeach()

install(DIRECTORY
launch
DESTINATION share/${PROJECT_NAME})
## Mark other files for installation (e.g. launch and bag files, etc.)

install(DIRECTORY
resources
DESTINATION share/${PROJECT_NAME})
install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch)
install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config)
install(DIRECTORY rviz/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz)
install(DIRECTORY resources/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/resources
PATTERN ".gridm" EXCLUDE)

ament_package()
else()

install(DIRECTORY include/
DESTINATION ${CMAKE_INSTALL_PREFIX}/include
FILES_MATCHING PATTERN "*.hpp"
)

install(TARGETS ${${PROJECT_NAME}_TARGETS}
DESTINATION ${CMAKE_INSTALL_PREFIX}/bin
)

endif()
unset(BUILD_ROS_SUPPORT CACHE)
2 changes: 1 addition & 1 deletion config/costmap_for_rand_maps.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ costmap:
cost_scaling_factor: 2
enabled: true
inflate_unknown: false
inflation_radius: 3.0
_radius: 3.0

static_layer:
unknown_cost_value: -1
Expand Down
Loading

0 comments on commit d683193

Please sign in to comment.