diff --git a/.gitignore b/.gitignore index 344c5d33b..8394b012b 100644 --- a/.gitignore +++ b/.gitignore @@ -22,6 +22,8 @@ simulate solve solve-mdp stest +bin +oppt_py.egg-info # Logging outputs core @@ -35,4 +37,4 @@ core *~ # temporary python files -*.pyc +*.pyc \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 000000000..879a965d8 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,305 @@ +cmake_minimum_required(VERSION 3.15) +project(oppt) + +cmake_policy(SET CMP0012 NEW) + +# Default to RelWithDebInfo build if not specified +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE RelWithDebInfo) +endif() + +string(TOUPPER ${CMAKE_BUILD_TYPE} CMAKE_BUILD_TYPE) +if(NOT CMAKE_BUILD_TYPE MATCHES RELWITHDEBINFO + AND NOT CMAKE_BUILD_TYPE MATCHES DEBUG + AND NOT CMAKE_BUILD_TYPE MATCHES RELEASE) + message( + FATAL_ERROR + "${CMAKE_BUILD_TYPE} is not a valid build type. Use Debug, Release, or RelWithDebInfo." + ) +endif() + +set(CMAKE_CXX_FLAGS_RELWITHDEBINFO + "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} -DEIGEN_NO_DEBUG") +set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -DEIGEN_NO_DEBUG") + +# Build flags +option(BUILD_PLUGINS "Build the POMDP model plugins" ON) +option(BUILD_SOLVERS "Build the POMDP solvers" ON) +option(BUILD_VIEWER "Build the viewer" ON) + +add_definitions(-DUSE_DOUBLE_PRECISION=true) + +# Setup paths +set(ROOT_PATH "${CMAKE_CURRENT_SOURCE_DIR}/src") +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src) +add_definitions(-DROOT_PATH=${ROOT_PATH}) + +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${ROOT_PATH}/../bin") +set(OPPT_CMAKE_DIR ${ROOT_PATH}/../cmake) +include(${OPPT_CMAKE_DIR}/CPM.cmake) + +# Include macros and search +include(${OPPT_CMAKE_DIR}/SearchForStuff.cmake) + + +CPMAddPackage( + NAME spatialindex + GITHUB_REPOSITORY libspatialindex/libspatialindex + GIT_TAG 1.8.5 + OPTIONS + "BUILD_TESTING OFF" + "BUILD_TOOLS OFF" +) + +add_library(spatialindex_iface INTERFACE) +target_link_libraries(spatialindex_iface INTERFACE spatialindex) +target_include_directories(spatialindex_iface INTERFACE ${spatialindex_SOURCE_DIR}/include) + +set(SPATIALINDEX_INCLUDE_DIRS ${spatialindex_SOURCE_DIR}) +list(APPEND oppt_INCLUDE_TARGETS ${SPATIALINDEX_INCLUDE_DIRS}) + +CPMAddPackage( + NAME Eigen + VERSION 3.2.8 + URL https://gitlab.com/libeigen/eigen/-/archive/3.4.0/eigen-3.4.0.tar.gz + # Eigen's CMakelists are not intended for library use + DOWNLOAD_ONLY YES +) + +if(Eigen_ADDED) + add_library(Eigen INTERFACE IMPORTED) + target_include_directories(Eigen INTERFACE ${Eigen_SOURCE_DIR}) +endif() + + +CPMAddPackage( + NAME + boost_pfr + GITHUB_REPOSITORY + boostorg/pfr + GIT_TAG + 2.2.0 +) + +find_package(Boost REQUIRED COMPONENTS system thread filesystem serialization) + +CPMAddPackage( + NAME tinyxml + GITHUB_REPOSITORY jslee02/tinyxml + GIT_TAG main +) + +find_package(assimp QUIET) + +if (NOT assimp_FOUND) + CPMAddPackage( + NAME assimp + GITHUB_REPOSITORY assimp/assimp + GIT_TAG v5.0.0 + OPTIONS + "ASSIMP_BUILD_TESTS OFF" + "ASSIMP_NO_EXPORT ON" + "BUILD_SHARED_LIBS OFF" + ) +endif() + +CPMAddPackage( + NAME fcl + GITHUB_REPOSITORY flexible-collision-library/fcl + GIT_TAG fcl-0.5 # Specify the version you want to use +) + +if(GZ_GT_10) + set(REQUIRED_CXX_STANDARD 17) +else() + set(REQUIRED_CXX_STANDARD 11) +endif() + +# Include macro template +configure_package_config_file( + "${OPPT_CMAKE_DIR}/opptMacros.cmake.in" + "${CMAKE_CURRENT_BINARY_DIR}/opptMacros.cmake" + INSTALL_DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/oppt") + +include(${CMAKE_CURRENT_BINARY_DIR}/opptMacros.cmake) + +# Include OPPT modules +include(${ROOT_PATH}/oppt/options/CMakeLists.txt) +include(${ROOT_PATH}/oppt/opptCore/CMakeLists.txt) +include(${ROOT_PATH}/oppt/plugin/CMakeLists.txt) +include(${ROOT_PATH}/oppt/robotEnvironment/CMakeLists.txt) +include(${ROOT_PATH}/oppt/robots/CMakeLists.txt) +include(${ROOT_PATH}/oppt/gazeboInterface/CMakeLists.txt) +include(${ROOT_PATH}/oppt/utils/CMakeLists.txt) +include(${ROOT_PATH}/oppt/filter/CMakeLists.txt) +include(${ROOT_PATH}/oppt/problemEnvironment/CMakeLists.txt) +include(${ROOT_PATH}/oppt/robotHeaders/CMakeLists.txt) +include(${ROOT_PATH}/oppt/solver/CMakeLists.txt) +include(${ROOT_PATH}/oppt/CMakeLists.txt) + +if(USE_RVIZ AND BUILD_VIEWER) + include(${ROOT_PATH}/oppt/viewerPublisher/CMakeLists.txt) +endif() + +# Source and library setup +set(COMMON_SRCS ${THIRD_PARTY_SRC} ${CMAKE_CURRENT_LIST_DIR}/src/oppt/global.cpp) + +set(OPPT_SRC + ${COMMON_SRCS} + ${OPTIONS_SRC} + ${ROBOT_PROBLEM_SRC} + ${ROBOT_ENVIRONMENT_SRC} + ${GAZEBO_INTERFACE_SRC} + ${ROBOTS_SRC} + ${OPPT_UTILS_SRC} + ${FILTER_SRC} + ${VIEWER_PUBLISHER_SRC} + ${CHANGES_PARSER_SRC}) + +# OPPT shared library +add_library(oppt SHARED ${OPPT_SRC}) + +find_package(ignition-math4 REQUIRED) +get_target_property(IGN_MATH4_INCLUDE_DIR ignition-math4::ignition-math4 INTERFACE_INCLUDE_DIRECTORIES) + +target_include_directories(oppt + PUBLIC + $ + $ + ${GAZEBO_INCLUDE_DIRS} + ${IGN_MATH4_INCLUDE_DIR} +) +target_link_libraries(oppt + PUBLIC + Eigen + ignition-math4::ignition-math4 + ${Boost_LIBRARIES} + ${GAZEBO_LIBRARIES} + ${SDF_LIBRARIES} + ${catkin_LIBRARIES} + ${TCLAP_LIBRARIES} + ${URDFDOM_LIBRARIES} + fcl + ${kdl_parser_LIBRARIES} + ${trac_ik_lib_LIBRARIES} + ${TINYXML_LIBRARIES} + ${ASSIMP_LIBRARIES} + ${CMAKE_DL_LIBS} + ${roscpp_LIBRARIES} + PRIVATE + spatialindex_iface + +) + +set_target_properties(oppt PROPERTIES CXX_STANDARD ${REQUIRED_CXX_STANDARD} + CXX_STANDARD_REQUIRED YES) + +install( + TARGETS oppt + EXPORT "opptTargets" + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}) + +install(TARGETS oppt + LIBRARY DESTINATION oppt_py) # ← install next to oppt_py.so + +# Plugins and solvers +if(BUILD_PLUGINS) + message(STATUS "Compiling OPPT plugins") + add_subdirectory(${ROOT_PATH}/plugins/) +endif() + +if(BUILD_SOLVERS) + message(STATUS "Compiling solvers") + add_subdirectory(${ROOT_PATH}/solvers/) +endif() + +if(USE_RVIZ AND BUILD_VIEWER) + message(STATUS "Compiling OPPT viewer") + add_subdirectory(${ROOT_PATH}/viewer/) +endif() + +# Install OPPT configuration +configure_package_config_file( + "${OPPT_CMAKE_DIR}/opptConfig.cmake.in" + "${CMAKE_CURRENT_BINARY_DIR}/opptConfig.cmake" + INSTALL_DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/oppt" + PATH_VARS CMAKE_INSTALL_INCLUDEDIR) + +install( + EXPORT opptTargets + FILE "opptTargets.cmake" + DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/oppt") + +install(FILES "${CMAKE_CURRENT_BINARY_DIR}/opptConfig.cmake" + "${CMAKE_CURRENT_BINARY_DIR}/opptMacros.cmake" + DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/oppt") + +install(FILES ${ROOT_PATH}/../setup.sh + DESTINATION ${CMAKE_INSTALL_DATADIR}/oppt/) +install(DIRECTORY ${ROOT_PATH}/../models + DESTINATION ${CMAKE_INSTALL_DATADIR}/oppt/) + +# Optional: Doxygen documentation +find_package(Doxygen) +if(DOXYGEN_FOUND) + set(DOXYGEN_IN ${CMAKE_CURRENT_SOURCE_DIR}/docs/Doxyfile.in) + set(DOXYGEN_OUT ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile) + configure_file(${DOXYGEN_IN} ${DOXYGEN_OUT} @ONLY) + add_custom_target( + doc + COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYGEN_OUT} + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} + COMMENT "Generating API documentation with Doxygen" + VERBATIM) +endif() + +# Python bindings + +cpmaddpackage(NAME pybind11 GITHUB_REPOSITORY pybind/pybind11 VERSION 2.12.0) + +pybind11_add_module(_oppt_py src/bindings.cpp) + +target_include_directories(_oppt_py PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src) + +target_link_libraries(_oppt_py + PRIVATE + oppt + abt_solver + ${GAZEBO_LIBRARIES} + ${Boost_LIBRARIES} + fcl + Eigen + spatialindex_iface +) + +target_link_options(_oppt_py PRIVATE + "-Wl,--no-as-needed" + "-Wl,--export-dynamic" +) + +set_target_properties(_oppt_py PROPERTIES + ENABLE_EXPORTS ON + CXX_STANDARD ${REQUIRED_CXX_STANDARD} + INSTALL_RPATH "$ORIGIN" + BUILD_RPATH "$ORIGIN" +) + +install(TARGETS _oppt_py + LIBRARY DESTINATION oppt_py +) + +install(FILES + ${spatialindex_BINARY_DIR}/bin/libspatialindex.so.4 + DESTINATION oppt_py +) + +add_custom_target(generate_stubs ALL + COMMAND ${Python3_EXECUTABLE} ${CMAKE_SOURCE_DIR}/build_tools/gen_stubs.py ${CMAKE_BINARY_DIR} + DEPENDS _oppt_py + COMMENT "Generating stubs for _oppt_py" +) + +install(FILES + ${CMAKE_BINARY_DIR}/_oppt_py.pyi + DESTINATION oppt_py +) \ No newline at end of file diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 000000000..7cc9a395d --- /dev/null +++ b/Dockerfile @@ -0,0 +1,22 @@ + +FROM ros:noetic + +RUN echo "deb https://mirror.aarnet.edu.au/pub/ubuntu/archive/ focal main restricted universe multiverse\n\ + deb https://mirror.aarnet.edu.au/pub/ubuntu/archive/ focal-updates main restricted universe multiverse\n\ + deb https://mirror.aarnet.edu.au/pub/ubuntu/archive/ focal-security main restricted universe multiverse\n" > /etc/apt/sources.list + +WORKDIR /dependencies + +# Extra dependencies that are needed. +RUN apt-get update && apt-get install -y \ + git python3-pip \ + ros-noetic-nlopt libnlopt-cxx-dev \ + wget ros-noetic-rviz ros-noetic-kdl-parser \ + ros-noetic-trac-ik ros-noetic-trac-ik-lib \ + gazebo9 libgazebo9-dev && \ + apt-get remove -y ros-noetic-fcl && \ + rm -rf /var/lib/apt/lists/* + +# Copy OPPT +WORKDIR /app +COPY . . diff --git a/build_tools/gen_stubs.py b/build_tools/gen_stubs.py new file mode 100755 index 000000000..7c816aa5b --- /dev/null +++ b/build_tools/gen_stubs.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python3 +from pathlib import Path +import subprocess +import os +import sys + +def main(): + if len(sys.argv) < 2: + print("Usage: gen_stubs.py ") + sys.exit(1) + + build_dir = Path(sys.argv[1]).resolve() + module_name = "_oppt_py" + output_dir = build_dir # just drop it here + + env = os.environ.copy() + env["PYTHONPATH"] = str(build_dir) + + subprocess.run([sys.executable, "-c", f"import {module_name}"], check=True, env=env) + + subprocess.run( + ["pybind11-stubgen", module_name, "-o", str(output_dir)], + check=True, + cwd=build_dir, + env=env + ) + + stub_file = output_dir / f"{module_name}.pyi" + subprocess.run(["black", str(stub_file)], check=True) + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/cmake/CPM.cmake b/cmake/CPM.cmake new file mode 100644 index 000000000..bfef29855 --- /dev/null +++ b/cmake/CPM.cmake @@ -0,0 +1,30 @@ +# SPDX-License-Identifier: MIT +# +# SPDX-FileCopyrightText: Copyright (c) 2019-2023 Lars Melchior and contributors + +set(CPM_DOWNLOAD_VERSION 0.40.2) +set(CPM_HASH_SUM + "c8cdc32c03816538ce22781ed72964dc864b2a34a310d3b7104812a5ca2d835d") + +if(CPM_SOURCE_CACHE) + set(CPM_DOWNLOAD_LOCATION + "${CPM_SOURCE_CACHE}/cpm/CPM_${CPM_DOWNLOAD_VERSION}.cmake") +elseif(DEFINED ENV{CPM_SOURCE_CACHE}) + set(CPM_DOWNLOAD_LOCATION + "$ENV{CPM_SOURCE_CACHE}/cpm/CPM_${CPM_DOWNLOAD_VERSION}.cmake") +else() + set(CPM_DOWNLOAD_LOCATION + "${CMAKE_BINARY_DIR}/cmake/CPM_${CPM_DOWNLOAD_VERSION}.cmake") +endif() + +# Expand relative path. This is important if the provided path contains a tilde +# (~) +get_filename_component(CPM_DOWNLOAD_LOCATION ${CPM_DOWNLOAD_LOCATION} ABSOLUTE) + +file( + DOWNLOAD + https://github.com/cpm-cmake/CPM.cmake/releases/download/v${CPM_DOWNLOAD_VERSION}/CPM.cmake + ${CPM_DOWNLOAD_LOCATION} + EXPECTED_HASH SHA256=${CPM_HASH_SUM}) + +include(${CPM_DOWNLOAD_LOCATION}) \ No newline at end of file diff --git a/cmake/FindAssimp.cmake b/cmake/FindAssimp.cmake deleted file mode 100644 index d7eaf3035..000000000 --- a/cmake/FindAssimp.cmake +++ /dev/null @@ -1,47 +0,0 @@ -# -# Try to find ASSIMP library and include path. -# Once done this will define -# -# ASSIMP_FOUND -# ASSIMP_INCLUDE_PATH -# ASSIMP_LIBRARY -# - -IF (WIN32) - FIND_PATH( ASSIMP_INCLUDE_PATH assimp/scene.h - ${ASSIMP_ROOT_DIR}/include - DOC "The directory where assimp/scene.h resides") - - FIND_LIBRARY( ASSIMP_LIBRARY - NAMES assimp ASSIMP assimp - PATHS - ${ASSIMP_ROOT_DIR}/lib - DOC "The ASSIMP library") -ELSE (WIN32) - FIND_PATH( ASSIMP_INCLUDE_PATH assimp/scene.h - /usr/include - /usr/local/include - /sw/include - /opt/local/include - ${ASSIMP_ROOT_DIR}/include - DOC "The directory where assimp/scene.h resides") - - FIND_LIBRARY( ASSIMP_LIBRARY - NAMES ASSIMP assimp - PATHS - /usr/lib64 - /usr/lib - /usr/local/lib64 - /usr/local/lib - /sw/lib - /opt/local/lib - ${ASSIMP_ROOT_DIR}/lib - DOC "The ASSIMP library") -ENDIF (WIN32) - -SET(ASSIMP_FOUND "NO") -IF (ASSIMP_INCLUDE_PATH AND ASSIMP_LIBRARY) - SET(ASSIMP_LIBRARIES ${ASSIMP_LIBRARY}) - SET(ASSIMP_FOUND "YES") - MESSAGE(STATUS "Found ASSIMP: ${ASSIMP_LIBRARY}") -ENDIF (ASSIMP_INCLUDE_PATH AND ASSIMP_LIBRARY) diff --git a/cmake/FindSpatialIndex.cmake b/cmake/FindSpatialIndex.cmake deleted file mode 100644 index afdd4c249..000000000 --- a/cmake/FindSpatialIndex.cmake +++ /dev/null @@ -1,52 +0,0 @@ -# Find Spatialindex -# ~~~~~~~~ -# Redistribution and use is allowed according to the terms of the BSD license. -# For details see the accompanying COPYING-CMAKE-SCRIPTS file. -# -# Once run this will define: -# -# SPATIALINDEX_FOUND = system has Spatialindex lib -# SPATIALINDEX_LIBRARIES = full path to the Spatialindex library -# SPATIALINDEX_LIBRARY_DIRS = path to the Spatialindex library -# SPATIALINDEX_INCLUDE_DIRS = where to find headers -# - - -FIND_PATH(SPATIALINDEX_INCLUDE_DIRS NAMES SpatialIndex.h PATHS - /usr/include - /usr/local/include - "$ENV{LIB_DIR}/include" - "$ENV{INCLUDE}" - "$ENV{OSGEO4W_ROOT}/include" - PATH_SUFFIXES spatialindex - ) - -FIND_PATH(SPATIALINDEX_LIBRARY_DIRS NAMES libspatialindex.so PATHS - /usr/lib - /usr/local/lib - "$ENV{LIB_DIR}/lib" - "$ENV{LIB}/lib" - "$ENV{OSGEO4W_ROOT}/lib" -) - -FIND_LIBRARY(SPATIALINDEX_LIBRARIES NAMES spatialindex_i spatialindex PATHS - /usr/lib - /usr/local/lib - "$ENV{LIB_DIR}/lib" - "$ENV{LIB}/lib" - "$ENV{OSGEO4W_ROOT}/lib" - ) - -IF (SPATIALINDEX_INCLUDE_DIRS AND SPATIALINDEX_LIBRARIES) - SET(SPATIALINDEX_FOUND TRUE) -ENDIF (SPATIALINDEX_INCLUDE_DIRS AND SPATIALINDEX_LIBRARIES) - -IF (SPATIALINDEX_FOUND) - IF (NOT SPATIALINDEX_FIND_QUIETLY) - MESSAGE(STATUS "Found libSpatialIndex: ${SPATIALINDEX_LIBRARY}") - ENDIF (NOT SPATIALINDEX_FIND_QUIETLY) -ELSE (SPATIALINDEX_FOUND) - IF (SPATIALINDEX_FIND_REQUIRED) - MESSAGE(FATAL_ERROR "Could not find libSpatialIndex") - ENDIF (SPATIALINDEX_FIND_REQUIRED) -ENDIF (SPATIALINDEX_FOUND) diff --git a/cmake/FindTinyXML.cmake b/cmake/FindTinyXML.cmake deleted file mode 100644 index ba6aa6daa..000000000 --- a/cmake/FindTinyXML.cmake +++ /dev/null @@ -1,24 +0,0 @@ -# - Find TinyXML -# Find the native TinyXML includes and library -# -# TINYXML_FOUND - True if TinyXML found. -# TINYXML_INCLUDE_DIR - where to find tinyxml.h, etc. -# TINYXML_LIBRARIES - TinyXML library. -# - -IF( TINYXML_INCLUDE_DIR ) - # Already in cache, be silent - SET( TinyXML_FIND_QUIETLY TRUE ) -ENDIF( TINYXML_INCLUDE_DIR ) - -FIND_PATH( TINYXML_INCLUDE_DIR "tinyxml.h" - PATH_SUFFIXES "tinyxml" ) - -FIND_LIBRARY( TINYXML_LIBRARIES - NAMES "tinyxml" - PATH_SUFFIXES "tinyxml" ) - -SET(TINYXML_FOUND "NO") -IF(TINYXML_LIBRARIES AND TINYXML_INCLUDE_DIR) - SET(TINYXML_FOUND "YES") -ENDIF(TINYXML_LIBRARIES AND TINYXML_INCLUDE_DIR) diff --git a/cmake/SearchForStuff.cmake b/cmake/SearchForStuff.cmake index c40363342..e77c85b9e 100644 --- a/cmake/SearchForStuff.cmake +++ b/cmake/SearchForStuff.cmake @@ -1,5 +1,3 @@ -SET(BOOST_MIN_VERSION "1.55.0") - include(FindPkgConfig) include(GNUInstallDirs) @@ -11,46 +9,14 @@ include(${CMAKE_CURRENT_LIST_DIR}/FindGazebo.cmake) set(GZ_INCLUDE_DIRS ${GAZEBO_INCLUDE_DIRS}) list(APPEND oppt_INCLUDE_TARGETS "${GAZEBO_INCLUDE_DIRS}") - -############################# LOAD TINYXML ############################# -include(${CMAKE_CURRENT_LIST_DIR}/FindTinyXML.cmake) -if (NOT TINYXML_FOUND) - message(FATAL_ERROR "TinyXML could not be found") -endif() -list(APPEND oppt_INCLUDE_TARGETS "${TINYXML_INCLUDE_DIRS}") - -############################# LOAD LIBSPATIALINDEX ############################# -include(${CMAKE_CURRENT_LIST_DIR}/FindSpatialIndex.cmake) -if (NOT SPATIALINDEX_FOUND) - message(FATAL_ERROR "libspatialindex could not be found") -endif() -list(APPEND oppt_INCLUDE_TARGETS "${SPATIALINDEX_INCLUDE_DIRS}") - -############################# LOAD SDFORMAT ############################# -set (SDFormat_VERSION 4.1.0) +# ############################ LOAD SDFORMAT ############################# +set(SDFormat_VERSION 4.1.0) find_package(SDFormat ${SDFormat_MIN_VERSION} REQUIRED) if(NOT SDFormat_FOUND) message(FATAL_ERROR "SDF could not be found") endif() list(APPEND oppt_INCLUDE_TARGETS "${SDFormat_INCLUDE_DIRS}") -############################# LOAD EIGEN ############################# -if(PKG_CONFIG_FOUND) - pkg_check_modules(EIGEN eigen3) - if(NOT EIGEN_FOUND) - message(FATAL_ERROR "EIGEN could not be found") - endif() - include_directories(${EIGEN_INCLUDE_DIRS}) - if (EIGEN_VERSION LESS 3.3.4) - else() - add_definitions(-DEIGEN_GT_3_3_4) - endif() -endif() -list(APPEND oppt_INCLUDE_TARGETS "${EIGEN_INCLUDE_DIRS}") - -############################# LOAD ASSIMP ############################# -include(${CMAKE_CURRENT_LIST_DIR}/FindAssimp.cmake) -list(APPEND oppt_INCLUDE_TARGETS "${ASSIMP_INCLUDE_PATH}") ############################# LOAD FCL ############################# if (GAZEBO_VERSION LESS 11) @@ -67,7 +33,7 @@ list(APPEND oppt_INCLUDE_TARGETS "${FCL_INCLUDE_DIRS}") else() add_definitions(-DFCL_GT_0_4) endif() - + ############################# LOAD ROS ############################# set(USE_RVIZ False) @@ -81,7 +47,6 @@ if (catkin_FOUND) else() message(STATUS "ROS and Rviz couldn't be found. Compiling without viewer support") endif() -endif() ############################# LOAD KDL PARSER ######################### set(SUPPORTS_IK True) @@ -101,16 +66,4 @@ if (SUPPORTS_IK) add_definitions(-DSUPPORTS_IK) list(APPEND oppt_INCLUDE_TARGETS "${kdl_parser_INCLUDE_DIRS}") list(APPEND oppt_INCLUDE_TARGETS "${trac_ik_lib_INCLUDE_DIRS}") -endif() - - -############################# LOAD BOOST ############################# -find_package(Boost - REQUIRED - system - thread - filesystem - serialization) -if (Boost_FOUND) - list(APPEND oppt_INCLUDE_TARGETS "${Boost_INCLUDE_DIRS}") -endif () +endif() \ No newline at end of file diff --git a/cmake/opptMacros.cmake.in b/cmake/opptMacros.cmake.in index 0a2a093f0..258852685 100644 --- a/cmake/opptMacros.cmake.in +++ b/cmake/opptMacros.cmake.in @@ -12,7 +12,7 @@ ENDMACRO() macro(ADD_TRANSITION_PLUGIN name src) set(TARGET_LINK_LIBRARIES ${ARGN}) add_library(${name} SHARED ${src}) - target_link_libraries(${name} ${TARGET_LINK_LIBRARIES}) + target_link_libraries(${name} ${TARGET_LINK_LIBRARIES}) oppt target_include_directories(${name} PUBLIC $;${oppt_INCLUDE_TARGETS};${oppt_INCLUDE_DIRS}) set_target_properties(${name} PROPERTIES CXX_STANDARD @REQUIRED_CXX_STANDARD@ CXX_STANDARD_REQUIRED YES) install(TARGETS ${name} LIBRARY DESTINATION ${CMAKE_INSTALL_DATADIR}/oppt/plugins/transitionPlugins) @@ -21,7 +21,7 @@ endmacro() macro(ADD_OBSERVATION_PLUGIN name src) set(TARGET_LINK_LIBRARIES ${ARGN}) add_library(${name} SHARED ${src}) - target_link_libraries(${name} ${TARGET_LINK_LIBRARIES}) + target_link_libraries(${name} ${TARGET_LINK_LIBRARIES}) oppt target_include_directories(${name} PUBLIC $;${oppt_INCLUDE_TARGETS};${oppt_INCLUDE_DIRS}) set_target_properties(${name} PROPERTIES CXX_STANDARD @REQUIRED_CXX_STANDARD@ CXX_STANDARD_REQUIRED YES) install(TARGETS ${name} LIBRARY DESTINATION ${CMAKE_INSTALL_DATADIR}/oppt/plugins/observationPlugins) @@ -30,7 +30,7 @@ endmacro() macro(ADD_INITIAL_BELIEF_PLUGIN name src) set(TARGET_LINK_LIBRARIES ${ARGN}) add_library(${name} SHARED ${src}) - target_link_libraries(${name} ${TARGET_LINK_LIBRARIES}) + target_link_libraries(${name} ${TARGET_LINK_LIBRARIES}) oppt target_include_directories(${name} PUBLIC $;${oppt_INCLUDE_TARGETS};${oppt_INCLUDE_DIRS}) set_target_properties(${name} PROPERTIES CXX_STANDARD @REQUIRED_CXX_STANDARD@ CXX_STANDARD_REQUIRED YES) install(TARGETS ${name} LIBRARY DESTINATION ${CMAKE_INSTALL_DATADIR}/oppt/plugins/initialBeliefPlugins) @@ -39,7 +39,7 @@ endmacro() macro(ADD_TERMINAL_PLUGIN name src) set(TARGET_LINK_LIBRARIES ${ARGN}) add_library(${name} SHARED ${src}) - target_link_libraries(${name} ${TARGET_LINK_LIBRARIES}) + target_link_libraries(${name} ${TARGET_LINK_LIBRARIES} oppt) target_include_directories(${name} PUBLIC $;${oppt_INCLUDE_TARGETS};${oppt_INCLUDE_DIRS}) set_target_properties(${name} PROPERTIES CXX_STANDARD @REQUIRED_CXX_STANDARD@ CXX_STANDARD_REQUIRED YES) install(TARGETS ${name} LIBRARY DESTINATION ${CMAKE_INSTALL_DATADIR}/oppt/plugins/terminalPlugins) @@ -48,7 +48,7 @@ endmacro() macro(ADD_HEURISTIC_PLUGIN name src) set(TARGET_LINK_LIBRARIES ${ARGN}) add_library(${name} SHARED ${src}) - target_link_libraries(${name} ${TARGET_LINK_LIBRARIES}) + target_link_libraries(${name} ${TARGET_LINK_LIBRARIES} oppt) target_include_directories(${name} PUBLIC $;${oppt_INCLUDE_TARGETS};${oppt_INCLUDE_DIRS}) set_target_properties(${name} PROPERTIES CXX_STANDARD @REQUIRED_CXX_STANDARD@ CXX_STANDARD_REQUIRED YES) install(TARGETS ${name} LIBRARY DESTINATION ${CMAKE_INSTALL_DATADIR}/oppt/plugins/heuristicPlugins) @@ -57,7 +57,7 @@ endmacro() macro(ADD_REWARD_PLUGIN name src) set(TARGET_LINK_LIBRARIES ${ARGN}) add_library(${name} SHARED ${src}) - target_link_libraries(${name} ${TARGET_LINK_LIBRARIES}) + target_link_libraries(${name} ${TARGET_LINK_LIBRARIES} oppt) target_include_directories(${name} PUBLIC $;${oppt_INCLUDE_TARGETS};${oppt_INCLUDE_DIRS}) set_target_properties(${name} PROPERTIES CXX_STANDARD @REQUIRED_CXX_STANDARD@ CXX_STANDARD_REQUIRED YES) install(TARGETS ${name} LIBRARY DESTINATION ${CMAKE_INSTALL_DATADIR}/oppt/plugins/rewardPlugins) diff --git a/install_dependencies.sh b/install_dependencies.sh index 4881282cf..e3b02ba67 100755 --- a/install_dependencies.sh +++ b/install_dependencies.sh @@ -134,10 +134,6 @@ install_common_dependencies() { echo "Installing remaining packages" checkAndAppend libboost-all-dev - checkAndAppend libtinyxml-dev - checkPKGAndAppend eigen3 libeigen3-dev - checkPKGAndAppend assimp libassimp-dev - checkPKGAndAppend fcl libfcl-dev checkAndAppend gazebo${GZ_VERSION} checkAndAppend libgazebo${GZ_VERSION}-dev diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 000000000..8b73d86f0 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,24 @@ + +[build-system] +requires = [ + "scikit-build-core", + "pybind11-stubgen", + "black" +] +build-backend = "scikit_build_core.build" + +[tool.setuptools.packages.find] +where = ["src"] + +[tool.cibuildwheel] +manylinux-x86_64-image = "manylinux_2_28" +manylinux-aarch64-image = "manylinux_2_28" +manylinux-pypy_x86_64-image = "manylinux_2_28" +manylinux-pypy_aarch64-image = "manylinux_2_28" + +[project] +name = "oppt_py" +version = "0.0.1" +description = "Python bindings for OPPT (CMake/pybind11)" +authors = [{ name = "Your Name" }] +requires-python = ">=3.7" \ No newline at end of file diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt deleted file mode 100644 index f42d0af58..000000000 --- a/src/CMakeLists.txt +++ /dev/null @@ -1,159 +0,0 @@ -cmake_minimum_required (VERSION 2.6) -project (oppt) - -cmake_policy(SET CMP0012 NEW) -set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} -DEIGEN_NO_DEBUG") -set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -DEIGEN_NO_DEBUG") - -# RelWithDebInfo is the default build type -if (NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE RelWithDebInfo) -endif() - -string(TOUPPER ${CMAKE_BUILD_TYPE} CMAKE_BUILD_TYPE) -if (NOT CMAKE_BUILD_TYPE MATCHES RELWITHDEBINFO AND NOT CMAKE_BUILD_TYPE MATCHES DEBUG AND NOT CMAKE_BUILD_TYPE MATCHES RELEASE) - message(FATAL_ERROR "${CMAKE_BUILD_TYPE} is not a valid build type. Supported types are Debug, Release and RelWithDebInfo (default)") -endif() - -option(BUILD_PLUGINS "Build the POMDP model plugins" ON) -option(BUILD_SOLVERS "Build the POMDP solvers" ON) -option(BUILD_VIEWER "Build the viewer" ON) - -add_definitions(-DUSE_DOUBLE_PRECISION=true) - -include(FindPkgConfig) -include(GNUInstallDirs) -include(CMakePackageConfigHelpers) - - -set(ROOT_PATH "${CMAKE_CURRENT_SOURCE_DIR}") -include_directories(${CMAKE_CURRENT_SOURCE_DIR}) -add_definitions(-DROOT_PATH=${ROOT_PATH}) - -set (CMAKE_RUNTIME_OUTPUT_DIRECTORY "${ROOT_PATH}/../bin") -set (OPPT_CMAKE_DIR ${ROOT_PATH}/../cmake) - -include (${OPPT_CMAKE_DIR}/SearchForStuff.cmake) - -if(GZ_GT_10) - set(REQUIRED_CXX_STANDARD 17) -else() - set(REQUIRED_CXX_STANDARD 11) -endif() - -configure_package_config_file("${OPPT_CMAKE_DIR}/opptMacros.cmake.in" - "${CMAKE_CURRENT_BINARY_DIR}/opptMacros.cmake" - INSTALL_DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/oppt" -) - -include(${CMAKE_CURRENT_BINARY_DIR}/opptMacros.cmake) -include(${ROOT_PATH}/oppt/options/CMakeLists.txt) -include(${ROOT_PATH}/oppt/opptCore/CMakeLists.txt) -include(${ROOT_PATH}/oppt/plugin/CMakeLists.txt) -include(${ROOT_PATH}/oppt/robotEnvironment/CMakeLists.txt) -include(${ROOT_PATH}/oppt/robots/CMakeLists.txt) -include(${ROOT_PATH}/oppt/gazeboInterface/CMakeLists.txt) -include(${ROOT_PATH}/oppt/utils/CMakeLists.txt) -include(${ROOT_PATH}/oppt/filter/CMakeLists.txt) -include(${ROOT_PATH}/oppt/problemEnvironment/CMakeLists.txt) -include(${ROOT_PATH}/oppt/robotHeaders/CMakeLists.txt) -include(${ROOT_PATH}/oppt/solver/CMakeLists.txt) -include(${ROOT_PATH}/oppt/CMakeLists.txt) - -if (USE_RVIZ AND BUILD_VIEWER) - include(${ROOT_PATH}/oppt/viewerPublisher/CMakeLists.txt) -endif() - -######################################## - -set(COMMON_SRCS - ${THIRD_PARTY_SRC} - ${CMAKE_CURRENT_LIST_DIR}/oppt/global.cpp) - -set(OPPT_SRC - ${COMMON_SRCS} - ${OPTIONS_SRC} - ${ROBOT_PROBLEM_SRC} - ${ROBOT_ENVIRONMENT_SRC} - ${GAZEBO_INTERFACE_SRC} - ${ROBOTS_SRC} - ${OPPT_UTILS_SRC} - ${FILTER_SRC} - ${VIEWER_PUBLISHER_SRC} - ${CHANGES_PARSER_SRC}) - -set(THIRD_PARTY_LIBS - ${Boost_LIBRARIES} - ${FCL_LIBRARIES} - ${TCLAP_LIBRARIES} - ${LIBSPATIALINDEX_LIBRARIES} - ${catkin_LIBRARIES} - ${SDF_LIBRARIES} - ${GAZEBO_LIBRARIES} - ${URDFDOM_LIBRARIES} - ${ASSIMP_LIBRARIES} - ${TINYXML_LIBRARIES} - ${SPATIALINDEX_LIBRARIES} - ${THIRD_PARTY_LIBRARIES} - ${roscpp_LIBRARIES} - ${kdl_parser_LIBRARIES} - ${trac_ik_lib_LIBRARIES} - ${CMAKE_DL_LIBS}) - -# Add the OPPT library target -add_library( oppt SHARED ${OPPT_SRC} ) -target_include_directories(oppt PUBLIC $;${oppt_INCLUDE_TARGETS}) -target_link_libraries (oppt ${THIRD_PARTY_LIBS}) -set_target_properties(oppt PROPERTIES CXX_STANDARD ${REQUIRED_CXX_STANDARD} CXX_STANDARD_REQUIRED YES) -install(TARGETS oppt - EXPORT "opptTargets" - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}) - -if (BUILD_PLUGINS) - message(STATUS "Compiling OPPT plugins") - add_subdirectory(${ROOT_PATH}/plugins/) -endif() - -if (BUILD_SOLVERS) - message(STATUS "Compiling solvers") - add_subdirectory(${ROOT_PATH}/solvers/) -endif() - -if(${USE_RVIZ} AND BUILD_VIEWER) - message(STATUS "Compiling OPPT viewer") - add_subdirectory(${ROOT_PATH}/viewer/) -endif() - -configure_package_config_file("${OPPT_CMAKE_DIR}/opptConfig.cmake.in" - "${CMAKE_CURRENT_BINARY_DIR}/opptConfig.cmake" - INSTALL_DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/oppt" - PATH_VARS CMAKE_INSTALL_INCLUDEDIR -) - -install(EXPORT opptTargets - FILE "opptTargets.cmake" - DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/oppt" -) - -install(FILES "${CMAKE_CURRENT_BINARY_DIR}/opptConfig.cmake" - DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/oppt" -) - -install(FILES "${CMAKE_CURRENT_BINARY_DIR}/opptMacros.cmake" - DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/oppt") - -install(FILES ${ROOT_PATH}/../setup.sh DESTINATION ${CMAKE_INSTALL_DATADIR}/oppt/) -install(DIRECTORY ${ROOT_PATH}/../models DESTINATION ${CMAKE_INSTALL_DATADIR}/oppt/) - -find_package(Doxygen) -if (DOXYGEN_FOUND) - set(DOXYGEN_IN ${CMAKE_CURRENT_SOURCE_DIR}/../docs/Doxyfile.in) - set(DOXYGEN_OUT ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile) - configure_file(${DOXYGEN_IN} ${DOXYGEN_OUT} @ONLY) - add_custom_target( doc - COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYGEN_OUT} - WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} - COMMENT "Generating API documentation with Doxygen" - VERBATIM ) -endif() - diff --git a/src/bindings.cpp b/src/bindings.cpp new file mode 100644 index 000000000..debc832fe --- /dev/null +++ b/src/bindings.cpp @@ -0,0 +1,88 @@ +#include +#include +#include + + +#include "oppt/problemEnvironment/ProblemEnvironmentOptions.hpp" +#include "oppt/problemEnvironment/ProblemEnvironment.hpp" +#include "solvers/ABT/solverABT.hpp" +#include "solvers/ABT/ABTOptions.hpp" +#include "solvers/ABT/robotModel/solver/Solver.hpp" + +namespace py = pybind11; +using namespace oppt; + +std::shared_ptr parse_abt_options_from_file(py::object configPathObj) { + std::string configPath = py::str(configPathObj.attr("__fspath__")()); + + ProblemEnvironmentOptionsParser parser; + auto basePtr = parser.parseOptions(configPath); + return std::dynamic_pointer_cast(basePtr); +} + +std::shared_ptr create_abt_env() { + auto env = std::make_shared(); + env->create(); + return env; +} + + +PYBIND11_MODULE(_oppt_py, m) { + + py::class_(m, "RunSummary") + .def_readonly("mean_num_steps", &RunSummary::meanNumSteps) + .def_readonly("mean_planning_time_per_step", &RunSummary::meanPlanningTimePerStep) + .def_readonly("num_successful_runs", &RunSummary::numSuccessfulRuns) + .def_readonly("percentage_successful_runs", &RunSummary::percentageSuccRuns) + .def_readonly("average_total_discounted_reward", &RunSummary::averageTotalDiscountedReward); + + + + py::class_>(m, "ProblemEnvironment") + .def(py::init<>()) + .def("initialize_with_options", &ProblemEnvironment::initializeWithOptions) + .def("run_environment", &ProblemEnvironment::runEnvironment); + + m.def("create_abt_environment", &create_abt_env, "Create ABT solver environment"); + + + py::class_>(m, "ProblemEnvironmentOptions") + .def(py::init<>()) + .def_readwrite("robotName", &ProblemEnvironmentOptions::robotName) + .def_readwrite("planningEnvironmentPath", &ProblemEnvironmentOptions::planningEnvironmentPath) + .def_readwrite("executionEnvironmentPath", &ProblemEnvironmentOptions::executionEnvironmentPath) + .def_readwrite("discountFactor", &ProblemEnvironmentOptions::discountFactor) + .def_readwrite("stepTimeout", &ProblemEnvironmentOptions::stepTimeout) + .def_readwrite("nRuns", &ProblemEnvironmentOptions::nRuns) + .def_readwrite("nSimulationSteps", &ProblemEnvironmentOptions::nSimulationSteps) + .def_readwrite("planningRewardPlugin", &ProblemEnvironmentOptions::planningRewardPlugin) + .def_readwrite("executionRewardPlugin", &ProblemEnvironmentOptions::executionRewardPlugin) + .def_readwrite("planningTerminalPlugin", &ProblemEnvironmentOptions::planningTerminalPlugin) + .def_readwrite("executionTerminalPlugin", &ProblemEnvironmentOptions::executionTerminalPlugin) + .def_readwrite("planningTransitionPlugin", &ProblemEnvironmentOptions::planningTransitionPlugin) + .def_readwrite("executionTransitionPlugin", &ProblemEnvironmentOptions::executionTransitionPlugin) + .def_readwrite("planningObservationPlugin", &ProblemEnvironmentOptions::planningObservationPlugin) + .def_readwrite("executionObservationPlugin", &ProblemEnvironmentOptions::executionObservationPlugin) + .def_readwrite("initialBeliefPluginPlanning", &ProblemEnvironmentOptions::initialBeliefPluginPlanning) + .def_readwrite("initialBeliefPluginExecution", &ProblemEnvironmentOptions::initialBeliefPluginExecution) + .def_readwrite("heuristicPlugin", &ProblemEnvironmentOptions::heuristicPlugin) + .def_readwrite("saveParticles", &ProblemEnvironmentOptions::saveParticles) + .def_readwrite("overwriteExistingLogFiles", &ProblemEnvironmentOptions::overwriteExistingLogFiles) + .def_readwrite("logPath", &ProblemEnvironmentOptions::logPath); + + py::class_>(m, "ABTExtendedOptions") + .def(py::init<>()) + .def_readwrite("configPath", &ABTExtendedOptions::configPath) + .def_readwrite("logPath", &ABTExtendedOptions::logPath) + .def_readwrite("seed", &ABTExtendedOptions::seed) + .def_readwrite("heuristicPlugin", &ABTExtendedOptions::heuristicPlugin) + .def_readwrite("enableGazeboStateLogging", &ABTExtendedOptions::enableGazeboStateLogging) + .def_readwrite("hasVerboseOutput", &ABTExtendedOptions::hasVerboseOutput); + + + m.def("parse_abt_options_from_file", &parse_abt_options_from_file, + py::arg("configPath"), + "Parse ABTExtendedOptions from a config file."); + + +} \ No newline at end of file diff --git a/src/oppt/problemEnvironment/ProblemEnvironment.hpp b/src/oppt/problemEnvironment/ProblemEnvironment.hpp index bd3afad22..e9378f830 100644 --- a/src/oppt/problemEnvironment/ProblemEnvironment.hpp +++ b/src/oppt/problemEnvironment/ProblemEnvironment.hpp @@ -13,8 +13,7 @@ * You should have received a copy of the GNU General Public License along with OPPT. * If not, see http://www.gnu.org/licenses/. */ -#ifndef _PROBLEM_ENVIRONMENT_HPP_ -#define _PROBLEM_ENVIRONMENT_HPP_ +#pragma once #include "oppt/global.hpp" #include "ProblemEnvironmentOptions.hpp" #include "oppt/opptCore/core.hpp" @@ -36,23 +35,29 @@ namespace oppt { +struct RunSummary { + unsigned int meanNumSteps; + double meanPlanningTimePerStep; + unsigned int numSuccessfulRuns; + double percentageSuccRuns; + double averageTotalDiscountedReward; +}; + /** * A parser that reads a configuration file, and constructs a ProblemEnvironmentOptions objects */ struct ProblemEnvironmentOptionsParser { + // Parse from command line (uses configPath from parsed args) template ProblemEnvironmentOptionsPtr parseOptions(int argc, char const* argv[]) { - std::unique_ptr options_ = std::make_unique(); - std::unique_ptr parser = - OptionsType::makeParser(true); + auto options_ = std::make_unique(); + auto parser = OptionsType::makeParser(true); - std::string workingDir = oppt::get_current_directory(); try { parser->setOptions(options_.get()); parser->parseCmdLine(argc, argv); - if (!options_->configPath.empty()) { + if (!options_->configPath.empty()) parser->parseCfgFile(options_->configPath); - } parser->finalize(); } catch (options::OptionParsingException const& e) { @@ -60,15 +65,40 @@ struct ProblemEnvironmentOptionsParser { return nullptr; } - if (options_->planningEnvironmentPath.empty() and options_->executionEnvironmentPath.empty() == false) { - ERROR("You have specified 'executionEnvironmentPath' in your problem configuration file but not 'planningEnvironmentPath'"); - } else if (options_->planningEnvironmentPath.empty() == false and options_->executionEnvironmentPath.empty()) { - ERROR("You have specified 'planningEnvironmentPath' in your problem configuration file but not 'executionEnvironmentPath'"); - } else if (options_->planningEnvironmentPath.empty() and options_->executionEnvironmentPath.empty()) { + return finalizeOptions(std::move(options_)); + } + + // Parse from a given config file path + template + ProblemEnvironmentOptionsPtr parseOptions(const std::string& configPath) { + auto options_ = std::make_unique(); + auto parser = OptionsType::makeParser(true); + + try { + parser->setOptions(options_.get()); + options_->configPath = configPath; + + parser->parseCfgFile(configPath); + parser->finalize(); + } catch (options::OptionParsingException const& e) { + std::cerr << e.what() << std::endl; + return nullptr; + } + + return finalizeOptions(std::move(options_)); + } + +private: + ProblemEnvironmentOptionsPtr finalizeOptions(std::unique_ptr options_) { + if (options_->planningEnvironmentPath.empty() && !options_->executionEnvironmentPath.empty()) { + ERROR("You have specified 'executionEnvironmentPath' but not 'planningEnvironmentPath'"); + } else if (!options_->planningEnvironmentPath.empty() && options_->executionEnvironmentPath.empty()) { + ERROR("You have specified 'planningEnvironmentPath' but not 'executionEnvironmentPath'"); + } else if (options_->planningEnvironmentPath.empty() && options_->executionEnvironmentPath.empty()) { options_->deactivateVisualization = true; } - if (options_->planningEnvironmentPath.empty() == false) { + if (!options_->planningEnvironmentPath.empty()) { if (!resources::FileExists(options_->planningEnvironmentPath)) { ERROR("Environment file '" + options_->planningEnvironmentPath + "' doesn't exist"); } @@ -77,7 +107,6 @@ struct ProblemEnvironmentOptionsParser { ERROR("Environment file '" + options_->executionEnvironmentPath + "' doesn't exist"); } - // Replace paths with their full path options_->planningEnvironmentPath = resources::FindFile(options_->planningEnvironmentPath); options_->executionEnvironmentPath = resources::FindFile(options_->executionEnvironmentPath); } @@ -96,18 +125,12 @@ class ProblemEnvironment /** * The setup method that has to be called from the main method before running the environment */ - template - int setup(int argc, char const* argv[]) { + int initializeWithOptions(std::shared_ptr options) { try { - solver_ = std::make_unique(); - ProblemEnvironmentOptionsParser problemEnvironmentOptionsParser; - problemEnvironmentOptions_ = - problemEnvironmentOptionsParser.parseOptions(argc, argv); - if (!problemEnvironmentOptions_) { - ERROR("Couldn't parse options"); - } + problemEnvironmentOptions_ = std::move(options); setVerbose(problemEnvironmentOptions_->hasVerboseOutput); + if (problemEnvironmentOptions_->seed == 0) { std::ifstream random("/dev/urandom", std::ios_base::in); int t; @@ -118,6 +141,7 @@ class ProblemEnvironment oppt::globalSeed = problemEnvironmentOptions_->seed; randGen_ = std::make_shared(problemEnvironmentOptions_->seed); randGen_->discard(100); + if (problemEnvironmentOptions_->rngState > 0) { std::stringstream sstr; sstr << problemEnvironmentOptions_->rngState; @@ -188,11 +212,14 @@ class ProblemEnvironment parseChanges(); solver_->setRobotPlanningEnvironment(robotPlanningEnvironment_.get()); + solver_->setProblemEnvironmentOptions(problemEnvironmentOptions_.get()); + auto heuristicPlugin = std::move(solver_->createHeuristicPlugin(problemEnvironmentOptions_->heuristicPlugin, solver_->robotPlanningEnvironment_)); loadPlugins_("exec"); loadPlugins_("planning"); + heuristicPlugin->load(problemEnvironmentOptions_->configPath); solver_->heuristicPlugin_ = std::move(heuristicPlugin); @@ -265,6 +292,27 @@ class ProblemEnvironment } } + template + int setup(int argc, char const* argv[]) { + solver_ = std::make_unique(); + + ProblemEnvironmentOptionsParser parser; + auto opts = parser.parseOptions(argc, argv); + if (!opts) { + ERROR("Couldn't parse options"); + return 1; + } + + return initializeWithOptions(std::move(opts)); + } + + template + int create() { + solver_ = std::make_unique(); + return 0; + } + + virtual void handleEnvironmentChanges(const EnvironmentChangeSharedPtr & environmentChange) { environmentUserChanges_.push_back(environmentChange); } @@ -272,60 +320,62 @@ class ProblemEnvironment /** * @brief Run the environment. This method should be called from the main method after setup has been called. */ - virtual int runEnvironment(int argc, char const * argv[]) { - try { - if (!problemEnvironmentOptions_) { - cout << "Error: You can't run an environment before calling the setup() method" << endl; - return 2; - } +virtual RunSummary runEnvironment() { + RunSummary summary; - std::string resultsDir = problemEnvironmentOptions_->logPath; - if (!oppt::createDir(resultsDir)) { - cout << "Error: results directory couldn't be created: " << resultsDir << endl; - return 2; - } + try { + if (!problemEnvironmentOptions_) { + throw std::runtime_error("You must call setup() before runEnvironment()"); + } + + std::string resultsDir = problemEnvironmentOptions_->logPath; + if (!oppt::createDir(resultsDir)) { + std::cerr << "Error: results directory couldn't be created: " << resultsDir << std::endl; + return summary; // Empty result + } + + std::string finalLogFilePath = getLogFilePath(resultsDir); + if (oppt::fileExists(finalLogFilePath)) { + oppt::LOGGING("A logfile with the same covariance parameters exists. Skipping"); + return summary; + } + + std::ofstream os(finalLogFilePath, std::ios_base::app | std::ios_base::out); + + os << "seed: " << problemEnvironmentOptions_->seed << std::endl; + os << "Robot: " << robotPlanningEnvironment_->getRobot()->getName() << std::endl; + os << "Planning environment: " << problemEnvironmentOptions_->planningEnvironmentPath << std::endl; + os << "Execution environment: " << problemEnvironmentOptions_->executionEnvironmentPath << std::endl; + os << "solver: " << solver_->getName() << std::endl; + + FloatType totalDiscountedReward = 0; + unsigned int totalNumSteps = 0; + unsigned int meanNumSteps = 0; + FloatType meanPlanningTimePerStep = 0; + unsigned int numSuccessfulRuns = 0; - std::string finalLogFilePath = getLogFilePath(resultsDir); - if (!oppt::fileExists(finalLogFilePath)) { - std::ofstream os(finalLogFilePath, - std::ios_base::app | std::ios_base::out); - - // Check the validity of the goal states - //goalStatesValid(); - - // Put some initial information into the logfile - os << "seed: " << problemEnvironmentOptions_->seed << endl; - //os << "Process error: " << problemEnvironmentOptions_->processError << endl; - //os << "Observation error: " << problemEnvironmentOptions_->observationError << endl; - os << "Robot: " << robotPlanningEnvironment_->getRobot()->getName() << endl; - os << "Planning environment: " << problemEnvironmentOptions_->planningEnvironmentPath << endl; - os << "Execution environment: " << problemEnvironmentOptions_->executionEnvironmentPath << endl; - os << "solver: " << solver_->getName() << endl; - - FloatType totalDiscountedReward = 0; - unsigned int totalNumSteps = 0; - unsigned int meanNumSteps = 0; - FloatType meanPlanningTimePerStep = 0; - unsigned int numSuccessfulRuns = 0; - for (size_t i = 0; i != problemEnvironmentOptions_->nRuns; ++i) { - os << "Run #" << i + 1 << endl; - cout << "Run # " << i + 1 << endl; - SimulationResult simulationResult = run(i + 1, os, argc, argv); - totalDiscountedReward += simulationResult.discountedReward; - totalNumSteps += simulationResult.stepsTaken; + for (size_t i = 0; i < problemEnvironmentOptions_->nRuns; ++i) { + os << "Run #" << i + 1 << std::endl; + std::cout << "Run # " << i + 1 << std::endl; + + SimulationResult simulationResult = run(i + 1, os); + + totalDiscountedReward += simulationResult.discountedReward; + totalNumSteps += simulationResult.stepsTaken; meanNumSteps += simulationResult.stepsTaken; - meanPlanningTimePerStep += simulationResult.totalPlanningTime; - if (simulationResult.successfulRun) - numSuccessfulRuns++; - onRunFinishedFn_(i + 1); - - os << "RUN_FINISHED_USER_DATA_BEGIN" << endl; - // We're done with this run. Let the solver know about it - solver_->runFinished(os, i + 1, simulationResult); - os << "RUN_FINISHED_USER_DATA_END" << endl; - cout << "Run finished \n"; - cout << "Discounted reward: " << simulationResult.discountedReward << endl; - } + meanPlanningTimePerStep += simulationResult.totalPlanningTime; + if (simulationResult.successfulRun) + numSuccessfulRuns++; + + onRunFinishedFn_(i + 1); + + os << "RUN_FINISHED_USER_DATA_BEGIN" << std::endl; + solver_->runFinished(os, i + 1, simulationResult); + os << "RUN_FINISHED_USER_DATA_END" << std::endl; + + std::cout << "Run finished\n"; + std::cout << "Discounted reward: " << simulationResult.discountedReward << std::endl; + } meanNumSteps /= (FloatType)problemEnvironmentOptions_->nRuns; meanPlanningTimePerStep /= (FloatType)totalNumSteps; @@ -333,26 +383,30 @@ class ProblemEnvironment FloatType percentageSuccRuns = (100.0 / (FloatType)problemEnvironmentOptions_->nRuns) * (FloatType)numSuccessfulRuns; - os << "##################################\n"; + os << "##################################\n"; os << "Mean number of steps: " << meanNumSteps << endl; os << "Mean planning time per step: " << meanPlanningTimePerStep * 1000 << "ms\n"; os << "Num successful runs: " << numSuccessfulRuns << endl; os << "Percentage of successful runs: " << percentageSuccRuns << endl; os << "Average total discounted reward: " << totalDiscountedReward / (FloatType)problemEnvironmentOptions_->nRuns << endl; - os.close(); - } else { - oppt::LOGGING("A logfile with the same covariance parameters exists. Skipping"); - } + os.close(); + // Populate result struct + summary.meanNumSteps = meanNumSteps; + summary.meanPlanningTimePerStep = meanPlanningTimePerStep * 1000; + summary.numSuccessfulRuns = numSuccessfulRuns; + summary.percentageSuccRuns = percentageSuccRuns; + summary.averageTotalDiscountedReward = totalDiscountedReward / (FloatType)problemEnvironmentOptions_->nRuns; - oppt::LOGGING("Done."); - return 0; - } catch (const std::runtime_error& re) { - return 2; - } + oppt::LOGGING("Done."); + } catch (const std::exception& e) { + std::cerr << "Exception in runEnvironment(): " << e.what() << std::endl; } + return summary; +} + /** * @brief Returns a pointer to the ProblemEnvironmentOptions object */ @@ -740,7 +794,7 @@ class ProblemEnvironment enableGazeboStateLogging(logTmp); } - SimulationResult run(const unsigned int& run, std::ofstream & os, int argc, char const * argv[]) { + SimulationResult run(const unsigned int& run, std::ofstream & os) { SimulationResult simulationResult; if (!solver_->reset()) return simulationResult; @@ -888,7 +942,7 @@ class ProblemEnvironment solver_->stepFinished(currentStep); onStepFinishedInternalFn_(run, currentStep); - // Update the viewer + // Update the viewer if (viewer && viewer->viewerRunning()) updateViewer(currentState, beliefParticles); @@ -899,7 +953,7 @@ class ProblemEnvironment break; } - FloatType totalTimeTaken = (oppt::clock_ms() - t0) / 1000.0; + FloatType totalTimeTaken = (oppt::clock_ms() - t0) / 1000.0; stepResult.serialize(os); @@ -928,11 +982,8 @@ class ProblemEnvironment simulationResult.totalPlanningTime = totalPlanningTime; simulationResult.successfulRun = success; - //solver_->run(os, argc, argv); return simulationResult; } }; } - -#endif diff --git a/src/oppt/problemEnvironment/ProblemEnvironmentOptions.hpp b/src/oppt/problemEnvironment/ProblemEnvironmentOptions.hpp index 0db489243..512815140 100644 --- a/src/oppt/problemEnvironment/ProblemEnvironmentOptions.hpp +++ b/src/oppt/problemEnvironment/ProblemEnvironmentOptions.hpp @@ -235,8 +235,7 @@ struct ProblemEnvironmentOptions : public oppt::Options { }; -typedef std::unique_ptr ProblemEnvironmentOptionsPtr; - +typedef std::shared_ptr ProblemEnvironmentOptionsPtr; } #endif diff --git a/src/oppt_py/__init__.py b/src/oppt_py/__init__.py new file mode 100644 index 000000000..863d7f8da --- /dev/null +++ b/src/oppt_py/__init__.py @@ -0,0 +1,12 @@ +import os +from pathlib import Path + +if "OPPT_RESOURCE_PATH" not in os.environ: + base = Path(__file__).resolve().parent.parent # site-packages + share = base / "share" / "oppt" + plugins = share / "plugins" + models = share / "models" + os.environ["OPPT_RESOURCE_PATH"] = f"{plugins}:{models}" + os.environ["GAZEBO_MODEL_PATH"] = f"{models}:" + +from ._oppt_py import * # exposes all C++ bindings \ No newline at end of file diff --git a/src/solvers/ABT/CMakeLists.txt b/src/solvers/ABT/CMakeLists.txt index 79e4b8745..91e597cb2 100644 --- a/src/solvers/ABT/CMakeLists.txt +++ b/src/solvers/ABT/CMakeLists.txt @@ -32,9 +32,11 @@ set(ABT_SRC ${SOLVER_SRC_PATH}/StateInfo.cpp ${SOLVER_SRC_PATH}/StatePool.cpp) -add_executable(abt - ${CMAKE_CURRENT_LIST_DIR}/main_abt_robot.cpp - ${ABT_SRC}) -target_link_libraries (abt - oppt) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) +add_library(abt_solver STATIC ${ABT_SRC}) +target_include_directories(abt_solver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(abt_solver PUBLIC oppt spatialindex_iface ${SDF_LIBRARIES}) + +add_executable(abt ${CMAKE_CURRENT_LIST_DIR}/main_abt_robot.cpp ${ABT_SRC}) +target_link_libraries(abt oppt spatialindex_iface ${SDF_LIBRARIES} ${Boost_LIBRARIES} ${SDF_LIBRARIES}) \ No newline at end of file diff --git a/src/solvers/ABT/main_abt_dubin.cpp b/src/solvers/ABT/main_abt_dubin.cpp index 2db7ac098..834647fd1 100644 --- a/src/solvers/ABT/main_abt_dubin.cpp +++ b/src/solvers/ABT/main_abt_dubin.cpp @@ -9,5 +9,6 @@ int main(int argc, char const* argv[]) int ret = problemEnvironment.setup(argc, argv); if (ret != 0) return ret; - return problemEnvironment.runEnvironment(argc, argv); + problemEnvironment.runEnvironment(); + return 0 } diff --git a/src/solvers/ABT/main_abt_robot.cpp b/src/solvers/ABT/main_abt_robot.cpp index 43c168662..78679e2b3 100644 --- a/src/solvers/ABT/main_abt_robot.cpp +++ b/src/solvers/ABT/main_abt_robot.cpp @@ -3,12 +3,11 @@ #include "ABTOptions.hpp" int main(int argc, char const* argv[]) -{ +{ oppt::ProblemEnvironment problemEnvironment; int ret = problemEnvironment.setup(argc, argv); if (ret != 0) return ret; - return problemEnvironment.runEnvironment(argc, argv); -} - - + problemEnvironment.runEnvironment(); + return 0; +} \ No newline at end of file diff --git a/test/test.py b/test/test.py new file mode 100644 index 000000000..ad2f910fd --- /dev/null +++ b/test/test.py @@ -0,0 +1,13 @@ +import oppt_py +from oppt_py import ( + parse_abt_options_from_file, + ProblemEnvironment, + create_abt_environment, +) +from pathlib import Path + +for p in Path("/app/cfg").glob("*.cfg"): + options = parse_abt_options_from_file(p) + env = create_abt_environment() + env.initialize_with_options(options) + result = env.run_environment() \ No newline at end of file