diff --git a/ocs2/CMakeLists.txt b/ocs2/CMakeLists.txt index 073eab27f..8453e751b 100644 --- a/ocs2/CMakeLists.txt +++ b/ocs2/CMakeLists.txt @@ -1,5 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2) -find_package(catkin REQUIRED) -catkin_metapackage() - +find_package(ament_cmake REQUIRED) +ament_package() diff --git a/ocs2/package.xml b/ocs2/package.xml index 834b4090e..1a999fc85 100644 --- a/ocs2/package.xml +++ b/ocs2/package.xml @@ -9,24 +9,24 @@ Ruben Grandia BSD3 - catkin + ament_cmake ocs2_core ocs2_mpc - ocs2_frank_wolfe + ocs2_oc ocs2_qp_solver ocs2_ddp ocs2_sqp ocs2_ros_interfaces - ocs2_python_interface + ocs2_pinocchio ocs2_robotic_tools - ocs2_robotic_examples + ocs2_thirdparty - ocs2_raisim + - - - + + ament_cmake + diff --git a/ocs2_core/CMakeLists.txt b/ocs2_core/CMakeLists.txt index 6a1d83e9c..de6dacc16 100644 --- a/ocs2_core/CMakeLists.txt +++ b/ocs2_core/CMakeLists.txt @@ -1,10 +1,10 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_core) -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ocs2_thirdparty -) + +find_package(ament_cmake REQUIRED) +find_package(ocs2_thirdparty REQUIRED) +find_package(OpenMP REQUIRED) find_package(Boost REQUIRED COMPONENTS system @@ -12,6 +12,7 @@ find_package(Boost REQUIRED COMPONENTS log_setup log ) + find_package(Eigen3 3.3 REQUIRED NO_MODULE) # Generate compile_commands.json for clang tools @@ -21,7 +22,7 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(CMAKE_THREAD_PREFER_PTHREAD TRUE) set(THREADS_PREFER_PTHREAD_FLAG TRUE) find_package(Threads REQUIRED) -if (Threads_FOUND) # Rename for catkin +if (Threads_FOUND) # set(Threads_INCLUDE_DIRS ${THREADS_PTHREADS_INCLUDE_DIR}) set(Threads_LIBRARIES ${CMAKE_THREAD_LIBS_INIT}) endif (Threads_FOUND) @@ -31,39 +32,10 @@ find_package(OpenMP REQUIRED) include(cmake/ocs2_cxx_flags.cmake) message(STATUS "OCS2_CXX_FLAGS: " ${OCS2_CXX_FLAGS}) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - test/include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ocs2_thirdparty - DEPENDS - Boost - OpenMP_CXX - Threads - CFG_EXTRAS - ocs2_cxx_flags.cmake -) - ########### ## Build ## ########### -include_directories( - include - test/include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - # Declare a C++ library add_library(${PROJECT_NAME} src/Types.cpp @@ -138,18 +110,40 @@ add_library(${PROJECT_NAME} src/thread_support/ThreadPool.cpp ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} + Eigen3::Eigen OpenMP::OpenMP_CXX Threads::Threads + dl +) + +ament_target_dependencies(${PROJECT_NAME} + ocs2_thirdparty +) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ ) + target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) add_executable(${PROJECT_NAME}_lintTarget src/lintTarget.cpp ) + +ament_target_dependencies(${PROJECT_NAME}_lintTarget + ocs2_thirdparty +) + +target_include_directories(${PROJECT_NAME}_lintTarget PUBLIC + $ + $ +) + target_link_libraries(${PROJECT_NAME}_lintTarget ${Boost_LIBRARIES} + Eigen3::Eigen ) ######################### @@ -171,53 +165,64 @@ endif(cmake_clang_tools_FOUND) ############# ## Install ## ############# - -install( - TARGETS - ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install(DIRECTORY include/ + DESTINATION include +) +install(DIRECTORY test/include/ + DESTINATION include +) -install(DIRECTORY test/include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets( + export_${PROJECT_NAME} ) +ament_export_dependencies(ocs2_thirdparty) +ament_export_dependencies(OpenMP) ############# ## Testing ## ############# -## Info ============================== -## to run tests, cd package folder and run -## $ catkin build -DCMAKE_BUILD_TYPE=RelWithDebInfo --this -## $ catkin run_tests --no-deps --this -## to see the summary of unit test results run -## $ catkin_test_results ../../../build/ocs2_core - -catkin_add_gtest(test_control + +if(BUILD_TESTING) + +# Include linting tests +find_package(ament_lint_auto REQUIRED) +find_package(ament_cmake_gtest REQUIRED) +ament_lint_auto_find_test_dependencies() + + +# test_control +ament_add_gtest(test_control test/control/testLinearController.cpp test/control/testFeedforwardController.cpp ) target_link_libraries(test_control ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(initialization_unittest + +# initialization_unittest +ament_add_gtest(initialization_unittest test/initialization/InitializationTest.cpp ) target_link_libraries(initialization_unittest ${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest_main ) -catkin_add_gtest(test_integration + +# test_integration +ament_add_gtest(test_integration test/integration/testSensitivityIntegrator.cpp test/integration/IntegrationTest.cpp test/integration/testRungeKuttaDormandPrince5.cpp @@ -225,43 +230,50 @@ catkin_add_gtest(test_integration ) target_link_libraries(test_integration ${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest_main ) -catkin_add_gtest(interpolation_unittest + +# interpolation_unittest +ament_add_gtest(interpolation_unittest test/misc/testInterpolation.cpp ) target_link_libraries(interpolation_unittest ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(${PROJECT_NAME}_cppadcg + +# ocs2_core_cppadcg +ament_add_gtest(${PROJECT_NAME}_cppadcg test/cppad_cg/testCppADCG_dynamics.cpp test/cppad_cg/testSparsityHelpers.cpp test/cppad_cg/testCppAdInterface.cpp ) + +target_include_directories(${PROJECT_NAME}_cppadcg PUBLIC + $ + $ +) + target_link_libraries(${PROJECT_NAME}_cppadcg ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} -lm -ldl gtest_main ) -catkin_add_gtest(test_transferfunctionbase +# test_transferfunctionbase +ament_add_gtest(test_transferfunctionbase test/dynamics/testTransferfunctionBase.cpp ) target_link_libraries(test_transferfunctionbase ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(${PROJECT_NAME}_loopshaping +ament_add_gtest(${PROJECT_NAME}_loopshaping test/loopshaping/testLoopshapingConfiguration.cpp test/loopshaping/testLoopshapingConstraint.cpp test/loopshaping/testLoopshapingCost.cpp @@ -271,15 +283,21 @@ catkin_add_gtest(${PROJECT_NAME}_loopshaping test/loopshaping/testLoopshapingFilterDynamics.cpp test/loopshaping/testLoopshapingPreComputation.cpp ) + +target_include_directories(${PROJECT_NAME}_loopshaping PUBLIC + $ + $ +) + target_link_libraries(${PROJECT_NAME}_loopshaping ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} -lstdc++fs gtest_main ) -catkin_add_gtest(${PROJECT_NAME}_test_misc +# ocs2_core_test_misc +ament_add_gtest(${PROJECT_NAME}_test_misc test/misc/testInterpolation.cpp test/misc/testLinearAlgebra.cpp test/misc/testLogging.cpp @@ -289,21 +307,23 @@ catkin_add_gtest(${PROJECT_NAME}_test_misc target_link_libraries(${PROJECT_NAME}_test_misc ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_dynamics + +# test_dynamics +ament_add_gtest(test_dynamics test/dynamics/testSystemDynamicsLinearizer.cpp test/dynamics/testSystemDynamicsPreComputation.cpp ) target_link_libraries(test_dynamics ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_cost + +# test_cost +ament_add_gtest(test_cost test/cost/testCostCollection.cpp test/cost/testCostCppAd.cpp test/cost/testQuadraticCostFunction.cpp @@ -311,11 +331,13 @@ catkin_add_gtest(test_cost target_link_libraries(test_cost ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} gtest_main + -ldl ) -catkin_add_gtest(test_constraint + +# test_constraint +ament_add_gtest(test_constraint test/constraint/testConstraintCollection.cpp test/constraint/testConstraintCppAd.cpp test/constraint/testLinearConstraint.cpp @@ -323,40 +345,46 @@ catkin_add_gtest(test_constraint target_link_libraries(test_constraint ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} gtest_main + -ldl ) -catkin_add_gtest(test_ModelData + +# test_ModelData +ament_add_gtest(test_ModelData test/model_data/testModelData.cpp ) target_link_libraries(test_ModelData ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_ModeSchedule - test/reference/testModeSchedule.cpp + +# test_Logging +ament_add_gtest(test_Logging + test/misc/testLogging.cpp ) -target_link_libraries(test_ModeSchedule +target_link_libraries(test_Logging ${PROJECT_NAME} - ${catkin_LIBRARIES} + ${Boost_LIBRARIES} gtest_main ) -catkin_add_gtest(test_softConstraint + +# test_softConstraint +ament_add_gtest(test_softConstraint test/soft_constraint/testSoftConstraint.cpp test/soft_constraint/testDoubleSidedPenalty.cpp ) target_link_libraries(test_softConstraint ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(${PROJECT_NAME}_test_thread_support + +# ocs2_core_test_thread_support +ament_add_gtest(${PROJECT_NAME}_test_thread_support test/thread_support/testBufferedValue.cpp test/thread_support/testSynchronized.cpp test/thread_support/testThreadPool.cpp @@ -364,16 +392,22 @@ catkin_add_gtest(${PROJECT_NAME}_test_thread_support target_link_libraries(${PROJECT_NAME}_test_thread_support ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(${PROJECT_NAME}_test_precomputation + +# ocs2_core_test_precomputation +ament_add_gtest(${PROJECT_NAME}_test_precomputation test/testPrecomputation.cpp ) target_link_libraries(${PROJECT_NAME}_test_precomputation ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} gtest_main ) + +endif(BUILD_TESTING) + + +ament_package() + diff --git a/ocs2_core/cmake/ocs2_cxx_flags.cmake b/ocs2_core/cmake/ocs2_cxx_flags.cmake index 1735d016f..5d592a3d0 100644 --- a/ocs2_core/cmake/ocs2_cxx_flags.cmake +++ b/ocs2_core/cmake/ocs2_cxx_flags.cmake @@ -1,11 +1,9 @@ -# The list of compiler flags used in ocs2 can be prefixed with catkin config +# The list of compiler flags used in ocs2 can be prefixed with config # Addition flags are to be separated by \; # For example, to turn on architecture specific optimizations: -# catkin config --cmake-args -DOCS2_CXX_FLAGS=-march=native\;-mtune=native list(APPEND OCS2_CXX_FLAGS "-pthread" "-Wfatal-errors" - "-Wl,--no-as-needed" ) # Force Boost dynamic linking diff --git a/ocs2_core/package.xml b/ocs2_core/package.xml index 4d1e695a3..5e528ee57 100644 --- a/ocs2_core/package.xml +++ b/ocs2_core/package.xml @@ -10,10 +10,13 @@ BSD - catkin + ament_cmake + ament_cmake_gtest cmake_modules cmake_clang_tools ocs2_thirdparty - + + ament_cmake + diff --git a/ocs2_core/src/penalties/MultidimensionalPenalty.cpp b/ocs2_core/src/penalties/MultidimensionalPenalty.cpp index 8cbe1bd84..417ebda05 100644 --- a/ocs2_core/src/penalties/MultidimensionalPenalty.cpp +++ b/ocs2_core/src/penalties/MultidimensionalPenalty.cpp @@ -76,7 +76,7 @@ scalar_t getMultiplier(const vector_t* l, size_t ind) { /******************************************************************************************************/ /******************************************************************************************************/ template <> -MultidimensionalPenalty::MultidimensionalPenalty(std::vector> penaltyPtrArray) +MultidimensionalPenalty::MultidimensionalPenalty(std::vector> penaltyPtrArray) : penaltyPtrArray_(std::move(penaltyPtrArray)) { if (penaltyPtrArray_.empty()) { throw std::runtime_error("[MultidimensionalPenalty::MultidimensionalPenalty] The penalty array cannot be empty!"); @@ -84,7 +84,7 @@ MultidimensionalPenalty::MultidimensionalPenalty(std::vect } template <> -MultidimensionalPenalty::MultidimensionalPenalty(std::vector> penaltyPtrArray) { +MultidimensionalPenalty::MultidimensionalPenalty(std::vector> penaltyPtrArray) { for (auto& penaltyPtr : penaltyPtrArray) { penaltyPtrArray_.push_back(createWrapper(std::move(penaltyPtr))); } @@ -98,12 +98,12 @@ MultidimensionalPenalty::MultidimensionalPenalty(std::vector -MultidimensionalPenalty::MultidimensionalPenalty(std::unique_ptr penaltyPtr) { +MultidimensionalPenalty::MultidimensionalPenalty(std::unique_ptr penaltyPtr) { penaltyPtrArray_.push_back(std::move(penaltyPtr)); } template <> -MultidimensionalPenalty::MultidimensionalPenalty(std::unique_ptr penaltyPtr) { +MultidimensionalPenalty::MultidimensionalPenalty(std::unique_ptr penaltyPtr) { penaltyPtrArray_.push_back(createWrapper(std::move(penaltyPtr))); } diff --git a/ocs2_ddp/CMakeLists.txt b/ocs2_ddp/CMakeLists.txt index 4b6a16bb9..b3909381c 100644 --- a/ocs2_ddp/CMakeLists.txt +++ b/ocs2_ddp/CMakeLists.txt @@ -1,55 +1,31 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_ddp) -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ocs2_core - ocs2_oc - ocs2_mpc - ocs2_qp_solver -) +find_package(ament_cmake) +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_qp_solver REQUIRED) +find_package(OpenMP REQUIRED) + find_package(Boost REQUIRED COMPONENTS system filesystem + log + log_setup ) find_package(Eigen3 3.3 REQUIRED NO_MODULE) -# Generate compile_commands.json for clang tools -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ocs2_core - ocs2_oc - ocs2_mpc - ocs2_qp_solver - DEPENDS - Boost -) +set(AMENT_DEPENDENCIES + ocs2_core + ocs2_oc + ocs2_qp_solver ) ########### ## Build ## ########### -include_directories( - include - test/include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - add_library(${PROJECT_NAME} src/riccati_equations/ContinuousTimeRiccatiEquations.cpp src/riccati_equations/DiscreteTimeRiccatiEquations.cpp @@ -65,15 +41,32 @@ add_library(${PROJECT_NAME} src/DDP_Settings.cpp src/DDP_HelperFunctions.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} + +ament_target_dependencies(${PROJECT_NAME} + ${AMENT_DEPENDENCIES} ) + target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) + + add_executable(${PROJECT_NAME}_lintTarget src/lintTarget.cpp ) +ament_target_dependencies(${PROJECT_NAME}_lintTarget + ${AMENT_DEPENDENCIES} +) + +target_include_directories(${PROJECT_NAME}_lintTarget PUBLIC + $ + $ +) + ######################### ### CLANG TOOLING ### ######################### @@ -93,13 +86,15 @@ endif(cmake_clang_tools_FOUND) ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install(DIRECTORY include/ + DESTINATION include +) ############# ## Testing ## @@ -111,83 +106,95 @@ install(DIRECTORY include/${PROJECT_NAME}/ ## to see the summary of unit test results run ## $ catkin_test_results ../../../build/ocs2_ddp -catkin_add_gtest(exp0_ddp_test - test/Exp0Test.cpp -) -target_link_libraries(exp0_ddp_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - gtest_main -) - -catkin_add_gtest(exp1_ddp_test - test/Exp1Test.cpp -) -target_link_libraries(exp1_ddp_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - gtest_main -) - -catkin_add_gtest(correctness_test - test/CorrectnessTest.cpp -) -target_link_libraries(correctness_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - gtest_main -) - -catkin_add_gtest(riccati_ode_test - test/RiccatiTest.cpp -) -target_link_libraries(riccati_ode_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main -) - -catkin_add_gtest(circular_kinematics_ddp_test - test/CircularKinematicsTest.cpp -) -target_link_libraries(circular_kinematics_ddp_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - gtest_main -) - -catkin_add_gtest(hybrid_slq_test - test/HybridSlqTest.cpp -) -target_link_libraries(hybrid_slq_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - gtest_main -) - -catkin_add_gtest(bouncing_mass_test - test/bouncingmass/BouncingMassTest.cpp - test/bouncingmass/OverallReference.cpp - test/bouncingmass/Reference.cpp -) -target_link_libraries(bouncing_mass_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - gtest_main -) - -catkin_add_gtest(testContinuousTimeLqr - test/testContinuousTimeLqr.cpp -) -target_link_libraries(testContinuousTimeLqr - ${Boost_LIBRARIES} - ${catkin_LIBRARIES} - ${PROJECT_NAME} - gtest_main -) +#catkin_add_gtest(exp0_ddp_test +# test/Exp0Test.cpp +#) +#target_link_libraries(exp0_ddp_test +# ${PROJECT_NAME} +# ${catkin_LIBRARIES} +# ${Boost_LIBRARIES} +# gtest_main +#) + +#catkin_add_gtest(exp1_ddp_test +# test/Exp1Test.cpp +#) +#target_link_libraries(exp1_ddp_test +# ${PROJECT_NAME} +# ${catkin_LIBRARIES} +# ${Boost_LIBRARIES} +# gtest_main +#) + +#catkin_add_gtest(correctness_test +# test/CorrectnessTest.cpp +#) +#target_link_libraries(correctness_test +# ${PROJECT_NAME} +# ${catkin_LIBRARIES} +# ${Boost_LIBRARIES} +# gtest_main +#) + +#catkin_add_gtest(riccati_ode_test +# test/RiccatiTest.cpp +#) +#target_link_libraries(riccati_ode_test +# ${PROJECT_NAME} +# ${catkin_LIBRARIES} +# gtest_main +#) + +#catkin_add_gtest(circular_kinematics_ddp_test +# test/CircularKinematicsTest.cpp +#) +#target_link_libraries(circular_kinematics_ddp_test +# ${PROJECT_NAME} +# ${catkin_LIBRARIES} +# ${Boost_LIBRARIES} +# gtest_main +#) + +#catkin_add_gtest(hybrid_slq_test +# test/HybridSlqTest.cpp +#) +#target_link_libraries(hybrid_slq_test +# ${PROJECT_NAME} +# ${catkin_LIBRARIES} +# ${Boost_LIBRARIES} +# gtest_main +#) + +#catkin_add_gtest(bouncing_mass_test +# test/bouncingmass/BouncingMassTest.cpp +# test/bouncingmass/OverallReference.cpp +# test/bouncingmass/Reference.cpp +#) +#target_link_libraries(bouncing_mass_test +# ${PROJECT_NAME} +# ${catkin_LIBRARIES} +# ${Boost_LIBRARIES} +# gtest_main +#) + +#catkin_add_gtest(testContinuousTimeLqr +# test/testContinuousTimeLqr.cpp +#) +#target_link_libraries(testContinuousTimeLqr +# ${Boost_LIBRARIES} +# ${catkin_LIBRARIES} +# ${PROJECT_NAME} +# gtest_main +#) + + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets( + export_${PROJECT_NAME} +) +ament_export_dependencies(ocs2_oc) +ament_export_dependencies(ocs2_qp_solver) +ament_export_dependencies(ocs2_core) + +ament_package() diff --git a/ocs2_ddp/package.xml b/ocs2_ddp/package.xml index c5f2723cd..66f8a41cb 100644 --- a/ocs2_ddp/package.xml +++ b/ocs2_ddp/package.xml @@ -10,7 +10,7 @@ BSD - catkin + ament_cmake cmake_modules cmake_clang_tools ocs2_qp_solver @@ -18,4 +18,7 @@ ocs2_oc ocs2_mpc + + ament_cmake + diff --git a/ocs2_doc/COLCON_IGNORE b/ocs2_doc/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_doc/package.xml b/ocs2_doc/package.xml index aa76ce754..6ffd162c1 100644 --- a/ocs2_doc/package.xml +++ b/ocs2_doc/package.xml @@ -10,7 +10,7 @@ BSD - catkin + ament_cmake doxygen texlive-latex-base diff --git a/ocs2_frank_wolfe/CMakeLists.txt b/ocs2_frank_wolfe/CMakeLists.txt index cf52219d1..636fbdd7c 100644 --- a/ocs2_frank_wolfe/CMakeLists.txt +++ b/ocs2_frank_wolfe/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project (ocs2_frank_wolfe) ## Find catkin macros and libraries @@ -51,13 +51,13 @@ target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) ## Install ## ############# -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install(DIRECTORY include/ + DESTINATION include) install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) ############# diff --git a/ocs2_frank_wolfe/COLCON_IGNORE b/ocs2_frank_wolfe/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_frank_wolfe/package.xml b/ocs2_frank_wolfe/package.xml index cf9c0a1e5..6d1847bc2 100644 --- a/ocs2_frank_wolfe/package.xml +++ b/ocs2_frank_wolfe/package.xml @@ -36,12 +36,12 @@ - + - catkin + ament_cmake cmake_modules ocs2_core glpk diff --git a/ocs2_mpc/CMakeLists.txt b/ocs2_mpc/CMakeLists.txt index 65e2df0b8..f587c3c96 100644 --- a/ocs2_mpc/CMakeLists.txt +++ b/ocs2_mpc/CMakeLists.txt @@ -1,14 +1,16 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_mpc) -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ocs2_core - ocs2_oc -) +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(OpenMP REQUIRED) + find_package(Boost REQUIRED COMPONENTS system filesystem + log + log_setup ) find_package(Eigen3 3.3 REQUIRED NO_MODULE) @@ -16,34 +18,10 @@ find_package(Eigen3 3.3 REQUIRED NO_MODULE) # Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ocs2_core - ocs2_oc - DEPENDS - Boost -) - ########### ## Build ## ########### -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - add_library(${PROJECT_NAME} src/LoopshapingSystemObservation.cpp src/MPC_BASE.cpp @@ -51,20 +29,20 @@ add_library(${PROJECT_NAME} src/SystemObservation.cpp src/MRT_BASE.cpp src/MPC_MRT_Interface.cpp - # src/MPC_OCS2.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} -) -target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -add_executable(${PROJECT_NAME}_lintTarget - src/lintTarget.cpp +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ ) -target_link_libraries(${PROJECT_NAME}_lintTarget - ${catkin_LIBRARIES} + +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_oc ) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) + ######################### ### CLANG TOOLING ### ######################### @@ -83,26 +61,30 @@ endif(cmake_clang_tools_FOUND) ## Install ## ############# -install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install(DIRECTORY include/ + DESTINATION include +) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_export_dependencies(ocs2_core) +ament_export_dependencies(ocs2_oc) ############# ## Testing ## ############# -#catkin_add_gtest(testMPC_OCS2 -# test/testMPC_OCS2.cpp -#) -#target_link_libraries(testMPC_OCS2 -# ${catkin_LIBRARIES} -# ${Boost_LIBRARIES} -#) -#target_compile_options(testMPC_OCS2 PRIVATE ${OCS2_CXX_FLAGS}) + +ament_package() diff --git a/ocs2_mpc/package.xml b/ocs2_mpc/package.xml index 9a4968d24..4c6c6ba7b 100644 --- a/ocs2_mpc/package.xml +++ b/ocs2_mpc/package.xml @@ -10,12 +10,16 @@ BSD3 - catkin + ament_cmake cmake_modules cmake_clang_tools ocs2_core ocs2_oc + + + ament_cmake + diff --git a/ocs2_msgs/CMakeLists.txt b/ocs2_msgs/CMakeLists.txt index 4af065c5a..f6d1738e3 100644 --- a/ocs2_msgs/CMakeLists.txt +++ b/ocs2_msgs/CMakeLists.txt @@ -1,36 +1,25 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_msgs) -find_package(catkin REQUIRED - COMPONENTS - std_msgs - message_generation -) +find_package(ament_cmake REQUIRED) +find_package(std_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) -add_message_files( - FILES - mpc_state.msg - mpc_input.msg - mode_schedule.msg - mpc_observation.msg - mpc_performance_indices.msg - mpc_target_trajectories.msg - controller_data.msg - mpc_flattened_controller.msg +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/MPCState.msg" + "msg/MPCInput.msg" + "msg/ModeSchedule.msg" + "msg/MPCObservation.msg" + "msg/MPCPerformanceIndices.msg" + "msg/MPCTargetTrajectories.msg" + "msg/ControllerData.msg" + "msg/MPCFlattenedController.msg" + "srv/Reset.srv" + DEPENDENCIES builtin_interfaces std_msgs ) -add_service_files( - FILES - reset.srv -) +ament_export_dependencies(std_msgs) +ament_export_dependencies(rosidl_default_runtime) -generate_messages( - DEPENDENCIES - std_msgs -) - -catkin_package( - CATKIN_DEPENDS - std_msgs - message_runtime -) +ament_package() diff --git a/ocs2_msgs/msg/controller_data.msg b/ocs2_msgs/msg/ControllerData.msg similarity index 100% rename from ocs2_msgs/msg/controller_data.msg rename to ocs2_msgs/msg/ControllerData.msg diff --git a/ocs2_msgs/msg/MPCFlattenedController.msg b/ocs2_msgs/msg/MPCFlattenedController.msg new file mode 100644 index 000000000..d30af7145 --- /dev/null +++ b/ocs2_msgs/msg/MPCFlattenedController.msg @@ -0,0 +1,21 @@ +# Flattened controller: A serialized controller + +# define controllerType Enum values +uint8 CONTROLLER_UNKNOWN=0 # safety mechanism: message initalization to zero +uint8 CONTROLLER_FEEDFORWARD=1 +uint8 CONTROLLER_LINEAR=2 + +uint8 controller_type # what type of controller is this + +MPCObservation init_observation # plan initial observation + +MPCTargetTrajectories plan_target_trajectories # target trajectory in cost function +MPCState[] state_trajectory # optimized state trajectory from planner +MPCInput[] input_trajectory # optimized input trajectory from planner +float64[] time_trajectory # time trajectory for stateTrajectory and inputTrajectory +uint16[] post_event_indices # array of indices indicating the index of post-event time in the trajectories +ModeSchedule mode_schedule # optimal/predefined MPC mode sequence and event times + +ControllerData[] data # the actual payload from flatten method: one vector of data per time step + +MPCPerformanceIndices performance_indices # solver performance indices diff --git a/ocs2_msgs/msg/mpc_input.msg b/ocs2_msgs/msg/MPCInput.msg similarity index 100% rename from ocs2_msgs/msg/mpc_input.msg rename to ocs2_msgs/msg/MPCInput.msg diff --git a/ocs2_msgs/msg/mpc_observation.msg b/ocs2_msgs/msg/MPCObservation.msg similarity index 54% rename from ocs2_msgs/msg/mpc_observation.msg rename to ocs2_msgs/msg/MPCObservation.msg index 675ec39b8..779613fa5 100644 --- a/ocs2_msgs/msg/mpc_observation.msg +++ b/ocs2_msgs/msg/MPCObservation.msg @@ -1,5 +1,5 @@ # MPC observation float64 time # Current time -mpc_state state # Current state -mpc_input input # Current input +MPCState state # Current state +MPCInput input # Current input int8 mode # Current mode diff --git a/ocs2_msgs/msg/MPCPerformanceIndices.msg b/ocs2_msgs/msg/MPCPerformanceIndices.msg new file mode 100644 index 000000000..9897f0aaa --- /dev/null +++ b/ocs2_msgs/msg/MPCPerformanceIndices.msg @@ -0,0 +1,8 @@ +# MPC performance indices +float32 init_time +float32 merit +float32 cost +float32 dynamics_violation_sse +float32 equality_constraints_sse +float32 equality_lagrangian +float32 inequality_lagrangian diff --git a/ocs2_msgs/msg/mpc_state.msg b/ocs2_msgs/msg/MPCState.msg similarity index 100% rename from ocs2_msgs/msg/mpc_state.msg rename to ocs2_msgs/msg/MPCState.msg diff --git a/ocs2_msgs/msg/MPCTargetTrajectories.msg b/ocs2_msgs/msg/MPCTargetTrajectories.msg new file mode 100644 index 000000000..72dee8731 --- /dev/null +++ b/ocs2_msgs/msg/MPCTargetTrajectories.msg @@ -0,0 +1,6 @@ +# MPC target trajectories + +float64[] time_trajectory # MPC target time trajectory +MPCState[] state_trajectory # MPC target state trajectory +MPCInput[] input_trajectory # MPC target input trajectory + diff --git a/ocs2_msgs/msg/ModeSchedule.msg b/ocs2_msgs/msg/ModeSchedule.msg new file mode 100644 index 000000000..abc26ad96 --- /dev/null +++ b/ocs2_msgs/msg/ModeSchedule.msg @@ -0,0 +1,4 @@ +# MPC mode sequence + +float64[] event_times # event times: its size is equal to the size of phaseSequence minus one +int8[] mode_sequence # mode sequence: e.g., for a quadrupedal robot, it is in the set {0, 1,..., 15} diff --git a/ocs2_msgs/msg/mode_schedule.msg b/ocs2_msgs/msg/mode_schedule.msg deleted file mode 100644 index 79bb61ce6..000000000 --- a/ocs2_msgs/msg/mode_schedule.msg +++ /dev/null @@ -1,4 +0,0 @@ -# MPC mode sequence - -float64[] eventTimes # event times: its size is equal to the size of phaseSequence minus one -int8[] modeSequence # mode sequence: e.g., for a quadrupedal robot, it is in the set {0, 1,..., 15} \ No newline at end of file diff --git a/ocs2_msgs/msg/mpc_flattened_controller.msg b/ocs2_msgs/msg/mpc_flattened_controller.msg deleted file mode 100644 index d1dff6f1c..000000000 --- a/ocs2_msgs/msg/mpc_flattened_controller.msg +++ /dev/null @@ -1,21 +0,0 @@ -# Flattened controller: A serialized controller - -# define controllerType Enum values -uint8 CONTROLLER_UNKNOWN=0 # safety mechanism: message initalization to zero -uint8 CONTROLLER_FEEDFORWARD=1 -uint8 CONTROLLER_LINEAR=2 - -uint8 controllerType # what type of controller is this - -mpc_observation initObservation # plan initial observation - -mpc_target_trajectories planTargetTrajectories # target trajectory in cost function -mpc_state[] stateTrajectory # optimized state trajectory from planner -mpc_input[] inputTrajectory # optimized input trajectory from planner -float64[] timeTrajectory # time trajectory for stateTrajectory and inputTrajectory -uint16[] postEventIndices # array of indices indicating the index of post-event time in the trajectories -mode_schedule modeSchedule # optimal/predefined MPC mode sequence and event times - -controller_data[] data # the actual payload from flatten method: one vector of data per time step - -mpc_performance_indices performanceIndices # solver performance indices diff --git a/ocs2_msgs/msg/mpc_performance_indices.msg b/ocs2_msgs/msg/mpc_performance_indices.msg deleted file mode 100644 index 2eb313e57..000000000 --- a/ocs2_msgs/msg/mpc_performance_indices.msg +++ /dev/null @@ -1,8 +0,0 @@ -# MPC performance indices -float32 initTime -float32 merit -float32 cost -float32 dynamicsViolationSSE -float32 equalityConstraintsSSE -float32 equalityLagrangian -float32 inequalityLagrangian diff --git a/ocs2_msgs/msg/mpc_target_trajectories.msg b/ocs2_msgs/msg/mpc_target_trajectories.msg deleted file mode 100644 index 8b375141d..000000000 --- a/ocs2_msgs/msg/mpc_target_trajectories.msg +++ /dev/null @@ -1,6 +0,0 @@ -# MPC target trajectories - -float64[] timeTrajectory # MPC target time trajectory -mpc_state[] stateTrajectory # MPC target state trajectory -mpc_input[] inputTrajectory # MPC target input trajectory - diff --git a/ocs2_msgs/package.xml b/ocs2_msgs/package.xml index 496d6563f..856391774 100644 --- a/ocs2_msgs/package.xml +++ b/ocs2_msgs/package.xml @@ -1,6 +1,6 @@ - + ocs2_msgs 0.0.0 @@ -11,12 +11,14 @@ Jan Carius Ruben Grandia - catkin + rosidl_default_generators - message_generation + ament_cmake + rosidl_default_runtime + rosidl_interface_packages std_msgs - - message_runtime - + + ament_cmake + diff --git a/ocs2_msgs/srv/Reset.srv b/ocs2_msgs/srv/Reset.srv new file mode 100644 index 000000000..bd0da7ae6 --- /dev/null +++ b/ocs2_msgs/srv/Reset.srv @@ -0,0 +1,5 @@ +# Reset service +bool reset +MPCTargetTrajectories target_trajectories +--- +bool done diff --git a/ocs2_msgs/srv/reset.srv b/ocs2_msgs/srv/reset.srv deleted file mode 100644 index a3a28420a..000000000 --- a/ocs2_msgs/srv/reset.srv +++ /dev/null @@ -1,5 +0,0 @@ -# Reset service -bool reset -mpc_target_trajectories targetTrajectories ---- -bool done \ No newline at end of file diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index 0c99d0ee8..447172791 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -1,15 +1,15 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_oc) -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ocs2_core - # ocs2_qp_solver -) +## Find libraries +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(OpenMP REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem + log + log_setup ) find_package(Eigen3 3.3 REQUIRED NO_MODULE) @@ -17,35 +17,11 @@ find_package(Eigen3 3.3 REQUIRED NO_MODULE) # Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - test/include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ocs2_core - DEPENDS - Boost -) ########### ## Build ## ########### -include_directories( - include - test/include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - add_library(${PROJECT_NAME} src/approximate_model/ChangeOfInputVariables.cpp src/approximate_model/LinearQuadraticApproximator.cpp @@ -67,16 +43,28 @@ add_library(${PROJECT_NAME} src/synchronized_module/LoopshapingSynchronizedModule.cpp src/trajectory_adjustment/TrajectorySpreading.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} + ocs2_core ) -target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) add_executable(${PROJECT_NAME}_lintTarget src/lintTarget.cpp ) +ament_target_dependencies(${PROJECT_NAME}_lintTarget + ocs2_core +) + +target_include_directories(${PROJECT_NAME}_lintTarget PUBLIC + $ + $ +) + ######################### ### CLANG TOOLING ### ######################### @@ -95,67 +83,97 @@ endif(cmake_clang_tools_FOUND) ## Install ## ############# -install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) -install(DIRECTORY test/include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY test/include/ + DESTINATION include ) +ament_export_include_directories(include) + +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_export_dependencies(ocs2_core) + ############# ## Testing ## ############# -## Info ============================== -## to run tests, cd package folder and run -## $ catkin build -DCMAKE_BUILD_TYPE=RelWithDebInfo --this -## $ catkin run_tests --no-deps --this -## to see the summary of unit test results run -## $ catkin_test_results ../../../build/ocs2_oc - -catkin_add_gtest(test_time_triggered_rollout + +if(BUILD_TESTING) + +# Include linting tests +find_package(ament_lint_auto REQUIRED) +find_package(ament_cmake_gtest REQUIRED) +ament_lint_auto_find_test_dependencies() + +# test_time_triggered_rollout +ament_add_gtest(test_time_triggered_rollout test/rollout/testTimeTriggeredRollout.cpp ) target_link_libraries(test_time_triggered_rollout ${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest_main ) +target_include_directories(test_time_triggered_rollout PUBLIC + $ + $ +) -catkin_add_gtest(test_state_triggered_rollout +# test_state_triggered_rollout +ament_add_gtest(test_state_triggered_rollout test/rollout/testStateTriggeredRollout.cpp ) target_link_libraries(test_state_triggered_rollout ${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest_main ) +target_include_directories(test_state_triggered_rollout PUBLIC + $ + $ +) + -catkin_add_gtest(test_change_of_variables +# test_change_of_variables +ament_add_gtest(test_change_of_variables test/testChangeOfInputVariables.cpp ) target_link_libraries(test_change_of_variables ${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest_main ) +target_include_directories(test_change_of_variables PUBLIC + $ + $ +) -catkin_add_gtest(test_trajectory_spreading +ament_add_gtest(test_trajectory_spreading test/trajectory_adjustment/TrajectorySpreadingTest.cpp ) target_link_libraries(test_trajectory_spreading ${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest_main ) +target_include_directories(test_trajectory_spreading PUBLIC + $ + $ +) + +endif(BUILD_TESTING) + +ament_package() diff --git a/ocs2_oc/package.xml b/ocs2_oc/package.xml index 00f0fbbfc..458d00b92 100644 --- a/ocs2_oc/package.xml +++ b/ocs2_oc/package.xml @@ -10,7 +10,7 @@ BSD - catkin + ament_cmake cmake_modules cmake_clang_tools ocs2_core diff --git a/ocs2_oc/src/trajectory_adjustment/TrajectorySpreading.cpp b/ocs2_oc/src/trajectory_adjustment/TrajectorySpreading.cpp index 35c50cc6a..f56296178 100644 --- a/ocs2_oc/src/trajectory_adjustment/TrajectorySpreading.cpp +++ b/ocs2_oc/src/trajectory_adjustment/TrajectorySpreading.cpp @@ -44,23 +44,6 @@ std::ostream& operator<<(std::ostream& os, const std::pair& ind) { os << "(" << ind.first << ", " << ind.second << ")"; return os; } - -/** - * Returns the first mismatching pair of elements from two ranges: one defined by [first1, last1) and - * another defined by [first2,last2). - * - * TODO deprecate this function once switched to c++14 and use: std::mismatch(first1, last1, first2, last2) - */ -template -std::pair mismatch(InputIt1 first1, InputIt1 last1, InputIt2 first2, InputIt2 last2) { - auto mismatchedIndex = std::mismatch(first1, last1, first2); - while (std::distance(last2, mismatchedIndex.second) > 0) { - --mismatchedIndex.first; - --mismatchedIndex.second; - } - return mismatchedIndex; -} - } // anonymous namespace /******************************************************************************************************/ @@ -90,12 +73,11 @@ auto TrajectorySpreading::set(const ModeSchedule& oldModeSchedule, const ModeSch // this means: mode[firstMatchingModeIndex + w] != updatedMode[updatedFirstMatchingModeIndex + w] size_t w = 0; while (oldStartIndexOfMatchedSequence < oldModeSchedule.modeSequence.size()) { - // TODO: change to std::mismatch(first1, last1, first2, last2). It is supported since c++14 // +1 to include the last active mode - const auto mismatchedIndex = mismatch(oldModeSchedule.modeSequence.cbegin() + oldStartIndexOfMatchedSequence, - oldModeSchedule.modeSequence.cbegin() + oldLastActiveModeIndex + 1, - newModeSchedule.modeSequence.cbegin() + newStartIndexOfMatchedSequence, - newModeSchedule.modeSequence.cbegin() + newLastActiveModeIndex + 1); + const auto mismatchedIndex = std::mismatch(oldModeSchedule.modeSequence.cbegin() + oldStartIndexOfMatchedSequence, + oldModeSchedule.modeSequence.cbegin() + oldLastActiveModeIndex + 1, + newModeSchedule.modeSequence.cbegin() + newStartIndexOfMatchedSequence, + newModeSchedule.modeSequence.cbegin() + newLastActiveModeIndex + 1); w = std::distance(oldModeSchedule.modeSequence.begin() + oldStartIndexOfMatchedSequence, mismatchedIndex.first); diff --git a/ocs2_ocs2/CMakeLists.txt b/ocs2_ocs2/CMakeLists.txt index 66a5d4eb0..ebfb610d0 100644 --- a/ocs2_ocs2/CMakeLists.txt +++ b/ocs2_ocs2/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_ocs2) find_package(catkin REQUIRED COMPONENTS @@ -70,8 +70,8 @@ endif(cmake_clang_tools_FOUND) ## Install ## ############# -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) ############# diff --git a/ocs2_ocs2/COLCON_IGNORE b/ocs2_ocs2/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_ocs2/package.xml b/ocs2_ocs2/package.xml index 5d37e875a..a72a9e913 100644 --- a/ocs2_ocs2/package.xml +++ b/ocs2_ocs2/package.xml @@ -36,12 +36,12 @@ - + - catkin + ament_cmake cmake_modules ocs2_core ocs2_ddp diff --git a/ocs2_pinocchio/ocs2_centroidal_model/CMakeLists.txt b/ocs2_pinocchio/ocs2_centroidal_model/CMakeLists.txt index 0dbe51fb5..73b97f736 100644 --- a/ocs2_pinocchio/ocs2_centroidal_model/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_centroidal_model/CMakeLists.txt @@ -1,45 +1,37 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_centroidal_model) # Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -set(CATKIN_PACKAGE_DEPENDENCIES +set(AMENT_DEPENDENCIES ocs2_core ocs2_robotic_tools - ocs2_robotic_assets ocs2_pinocchio_interface ) -find_package(catkin REQUIRED - ${CATKIN_PACKAGE_DEPENDENCIES} -) +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(OpenMP REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem + log + log_setup ) find_package(Eigen3 3.3 REQUIRED NO_MODULE) -find_package(PkgConfig REQUIRED) -pkg_check_modules(pinocchio REQUIRED pinocchio) - -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - pinocchio - Boost -) +if ( EXISTS "/opt/openrobots/lib/libpinocchio.so") +else() + message(FATAL_ERROR "Install Pinocchio first. Instructions: https://stack-of-tasks.github.io/pinocchio/download.html") +endif() + +list(APPEND CMAKE_PREFIX_PATH "/opt/openrobots/") +find_package(pinocchio REQUIRED) ########### ## Build ## @@ -54,18 +46,6 @@ set(FLAGS -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR ) -include_directories( - include - ${pinocchio_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} -) - -link_directories( - ${pinocchio_LIBRARY_DIRS} -) - add_library(${PROJECT_NAME} src/PinocchioCentroidalDynamics.cpp src/PinocchioCentroidalDynamicsAD.cpp @@ -75,11 +55,22 @@ add_library(${PROJECT_NAME} src/FactoryFunctions.cpp src/ModelHelperFunctions.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ${pinocchio_LIBRARIES} + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) + +ament_target_dependencies(${PROJECT_NAME} + pinocchio + ocs2_pinocchio_interface + ocs2_core + ocs2_robotic_tools ) + +set_target_properties(${PROJECT_NAME} + PROPERTIES INSTALL_RPATH "/opt/openrobots/lib") + target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) ######################### @@ -99,34 +90,46 @@ endif(cmake_clang_tools_FOUND) ############# ## Install ## ############# -install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include ) -install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets( + export_${PROJECT_NAME} ) +ament_export_dependencies(ocs2_core) +ament_export_dependencies(ocs2_robotic_tools) +ament_export_dependencies(ocs2_thirdparty) +ament_export_dependencies(ocs2_pinocchio_interface) + ############# ## Testing ## ############# -catkin_add_gtest(${PROJECT_NAME}_test - # test/testAccessMethods.cpp - test/testAnymalCentroidalModel.cpp -) -target_include_directories(${PROJECT_NAME}_test PRIVATE - test/include -) -target_link_libraries(${PROJECT_NAME}_test - gtest_main - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ${pinocchio_LIBRARIES} -) -target_compile_options(${PROJECT_NAME}_test PRIVATE ${FLAGS}) +#catkin_add_gtest(${PROJECT_NAME}_test +# # test/testAccessMethods.cpp +# test/testAnymalCentroidalModel.cpp +#) +#target_include_directories(${PROJECT_NAME}_test PRIVATE +# test/include +#) +#target_link_libraries(${PROJECT_NAME}_test +# gtest_main +# ${PROJECT_NAME} +# ${Boost_LIBRARIES} +# ${pinocchio_LIBRARIES} +#) +#target_compile_options(${PROJECT_NAME}_test PRIVATE ${FLAGS}) + +ament_package() + diff --git a/ocs2_pinocchio/ocs2_centroidal_model/package.xml b/ocs2_pinocchio/ocs2_centroidal_model/package.xml index c085f142f..b52f375f9 100644 --- a/ocs2_pinocchio/ocs2_centroidal_model/package.xml +++ b/ocs2_pinocchio/ocs2_centroidal_model/package.xml @@ -10,12 +10,16 @@ TODO - catkin + ament_cmake cmake_clang_tools ocs2_core ocs2_robotic_tools - ocs2_robotic_assets ocs2_pinocchio_interface pinocchio - \ No newline at end of file + + + + ament_cmake + + diff --git a/ocs2_pinocchio/ocs2_pinocchio/CMakeLists.txt b/ocs2_pinocchio/ocs2_pinocchio/CMakeLists.txt index 6d655c86d..960cc9c6c 100644 --- a/ocs2_pinocchio/ocs2_pinocchio/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_pinocchio/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_pinocchio) -find_package(catkin REQUIRED) -catkin_metapackage() +find_package(ament_cmake REQUIRED) + diff --git a/ocs2_pinocchio/ocs2_pinocchio/package.xml b/ocs2_pinocchio/ocs2_pinocchio/package.xml index 59c314a67..2fbf21115 100644 --- a/ocs2_pinocchio/ocs2_pinocchio/package.xml +++ b/ocs2_pinocchio/ocs2_pinocchio/package.xml @@ -9,14 +9,16 @@ Ruben Grandia TODO - catkin + ament_cmake ocs2_centroidal_model ocs2_pinocchio_interface + - + diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt b/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt index 0107f0be5..be0de478a 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt @@ -1,55 +1,62 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_pinocchio_interface) # Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_robotic_tools +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_thirdparty REQUIRED) +find_package(OpenMP REQUIRED) + +find_package(Boost REQUIRED COMPONENTS + system + filesystem + log + log_setup ) -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) +if ( EXISTS "/opt/openrobots/lib/libpinocchio.so") +else() + message(FATAL_ERROR "Install Pinocchio first. Instructions: https://stack-of-tasks.github.io/pinocchio/download.html") +endif() -find_package(PkgConfig REQUIRED) -pkg_check_modules(pinocchio REQUIRED pinocchio) +list(APPEND CMAKE_PREFIX_PATH "/opt/openrobots/") +find_package(pinocchio REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) -# Add pinocchio configurations -include(cmake/pinocchio_config.cmake) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${pinocchio_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - pinocchio - CFG_EXTRAS - pinocchio_config.cmake -) - ########### ## Build ## ########### include_directories( include - ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${pinocchio_INCLUDE_DIRS} ) +set(OCS2_PINOCCHIO_FLAGS + ${OCS2_CXX_FLAGS} + ${pinocchio_CFLAGS_OTHER} + -Wno-ignored-attributes + -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR + -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR +) + # ocs2 pinocchio interface library add_library(${PROJECT_NAME} src/PinocchioInterface.cpp @@ -58,15 +65,30 @@ add_library(${PROJECT_NAME} src/PinocchioEndEffectorKinematicsCppAd.cpp src/urdf.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} + +target_include_directories(${PROJECT_NAME} PUBLIC + /opt/openrobots/include + $ + $ +) + +ament_target_dependencies(${PROJECT_NAME} + pinocchio + ocs2_core + ocs2_oc + ocs2_robotic_tools ) + target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} ${pinocchio_LIBRARIES} ) + target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_PINOCCHIO_FLAGS}) +set_target_properties(${PROJECT_NAME} + PROPERTIES INSTALL_RPATH "/opt/openrobots/lib") + + #################### ## Clang tooling ### #################### @@ -87,24 +109,36 @@ endif (cmake_clang_tools_FOUND) ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + +install(DIRECTORY include/ + DESTINATION include ) +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_export_dependencies(ocs2_core) +ament_export_dependencies(ocs2_robotic_tools) + ############ # Testing ## ############ -catkin_add_gtest(testPinocchioInterface - test/testPinocchioInterface.cpp - test/testPinocchioEndEffectorKinematics.cpp -) -target_link_libraries(testPinocchioInterface - gtest_main - ${PROJECT_NAME} - ${catkin_LIBRARIES} -) +#catkin_add_gtest(testPinocchioInterface +# test/testPinocchioInterface.cpp +# test/testPinocchioEndEffectorKinematics.cpp +#) +#target_link_libraries(testPinocchioInterface +# gtest_main +# ${PROJECT_NAME} +#) + +ament_package() diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/package.xml b/ocs2_pinocchio/ocs2_pinocchio_interface/package.xml index 25503afcb..7bb55540d 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/package.xml +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/package.xml @@ -9,11 +9,15 @@ TODO - catkin + ament_cmake cmake_clang_tools ocs2_core + ocs2_oc ocs2_robotic_tools pinocchio + + ament_cmake + diff --git a/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt b/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt index 37e06a0e8..6ef2efc9a 100644 --- a/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt @@ -1,41 +1,41 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_self_collision) # Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -set(CATKIN_PACKAGE_DEPENDENCIES +set(AMENT_DEPENDENCIES ocs2_core ocs2_robotic_tools ocs2_pinocchio_interface ) -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(OpenMP REQUIRED) + +find_package(Boost REQUIRED COMPONENTS + system + filesystem + log + log_setup ) -find_package(PkgConfig REQUIRED) -pkg_check_modules(pinocchio REQUIRED pinocchio) -pkg_check_modules(hpp-fcl REQUIRED hpp-fcl) +if ( EXISTS "/opt/openrobots/lib/libpinocchio.so") +else() + message(FATAL_ERROR "Install Pinocchio first. Instructions: https://stack-of-tasks.github.io/pinocchio/download.html") +endif() + +list(APPEND CMAKE_PREFIX_PATH "/opt/openrobots/") +find_package(pinocchio REQUIRED) +find_package(hpp-fcl REQUIRED) + # requires liboctomap-dev and libassimp-dev find_package(Eigen3 3.3 REQUIRED NO_MODULE) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - pinocchio -) - ########### ## Build ## ########### @@ -44,22 +44,17 @@ set(FLAGS ${OCS2_CXX_FLAGS} ${pinocchio_CFLAGS_OTHER} -Wno-ignored-attributes - -Wno-invalid-partial-specialization # to silence warning with unsupported Eigen Tensor -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR ) include_directories( include - ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${pinocchio_INCLUDE_DIRS} ${hpp-fcl_INCLUDE_DIRS} ) -link_directories( - ${pinocchio_LIBRARY_DIRS} -) # ocs2 pinocchio interface library add_library(${PROJECT_NAME} @@ -69,11 +64,11 @@ add_library(${PROJECT_NAME} src/SelfCollisionConstraint.cpp src/SelfCollisionConstraintCppAd.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} + +ament_target_dependencies(${PROJECT_NAME} + ${AMENT_DEPENDENCIES} ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} ${pinocchio_LIBRARIES} ) target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) @@ -98,14 +93,24 @@ endif (cmake_clang_tools_FOUND) ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(DIRECTORY include/ + DESTINATION include ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets( + export_${PROJECT_NAME} ) +ament_export_dependencies(ocs2_core) +ament_export_dependencies(ocs2_robotic_tools) +ament_export_dependencies(ocs2_pinocchio_interface) + ############ # Testing ## ############ diff --git a/ocs2_pinocchio/ocs2_self_collision/COLCON_IGNORE b/ocs2_pinocchio/ocs2_self_collision/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_pinocchio/ocs2_self_collision/package.xml b/ocs2_pinocchio/ocs2_self_collision/package.xml index 4935695bb..f0dd5f303 100644 --- a/ocs2_pinocchio/ocs2_self_collision/package.xml +++ b/ocs2_pinocchio/ocs2_self_collision/package.xml @@ -9,7 +9,7 @@ TODO - catkin + ament_cmake cmake_clang_tools ocs2_core diff --git a/ocs2_pinocchio/ocs2_self_collision_visualization/CMakeLists.txt b/ocs2_pinocchio/ocs2_self_collision_visualization/CMakeLists.txt index b61f92809..0cff18706 100644 --- a/ocs2_pinocchio/ocs2_self_collision_visualization/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_self_collision_visualization/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_self_collision_visualization) # Generate compile_commands.json for clang tools @@ -102,10 +102,10 @@ endif (cmake_clang_tools_FOUND) ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) diff --git a/ocs2_pinocchio/ocs2_self_collision_visualization/COLCON_IGNORE b/ocs2_pinocchio/ocs2_self_collision_visualization/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_pinocchio/ocs2_self_collision_visualization/package.xml b/ocs2_pinocchio/ocs2_self_collision_visualization/package.xml index 46cb2614b..8129d0a0e 100644 --- a/ocs2_pinocchio/ocs2_self_collision_visualization/package.xml +++ b/ocs2_pinocchio/ocs2_self_collision_visualization/package.xml @@ -8,7 +8,7 @@ TODO - catkin + ament_cmake cmake_clang_tools roscpp diff --git a/ocs2_pinocchio/ocs2_sphere_approximation/CMakeLists.txt b/ocs2_pinocchio/ocs2_sphere_approximation/CMakeLists.txt index c72f39302..3de2d158f 100644 --- a/ocs2_pinocchio/ocs2_sphere_approximation/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_sphere_approximation/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_sphere_approximation) # Generate compile_commands.json for clang tools @@ -95,12 +95,12 @@ endif (cmake_clang_tools_FOUND) ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) ############ diff --git a/ocs2_pinocchio/ocs2_sphere_approximation/COLCON_IGNORE b/ocs2_pinocchio/ocs2_sphere_approximation/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_pinocchio/ocs2_sphere_approximation/package.xml b/ocs2_pinocchio/ocs2_sphere_approximation/package.xml index 934ba99ee..b5809258c 100644 --- a/ocs2_pinocchio/ocs2_sphere_approximation/package.xml +++ b/ocs2_pinocchio/ocs2_sphere_approximation/package.xml @@ -8,7 +8,7 @@ TODO - catkin + ament_cmake cmake_clang_tools ocs2_core diff --git a/ocs2_python_interface/CMakeLists.txt b/ocs2_python_interface/CMakeLists.txt index 558ea0b8e..ef5e5c58c 100644 --- a/ocs2_python_interface/CMakeLists.txt +++ b/ocs2_python_interface/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_python_interface) set(CATKIN_PACKAGE_DEPENDENCIES @@ -50,13 +50,13 @@ add_library(${PROJECT_NAME} ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install(DIRECTORY include/ + DESTINATION include) ############# ## Testing ## diff --git a/ocs2_python_interface/COLCON_IGNORE b/ocs2_python_interface/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_python_interface/package.xml b/ocs2_python_interface/package.xml index 993fced46..a687319da 100644 --- a/ocs2_python_interface/package.xml +++ b/ocs2_python_interface/package.xml @@ -10,7 +10,7 @@ TODO - catkin + ament_cmake cmake_modules cmake_clang_tools diff --git a/ocs2_raisim/COLCON_IGNORE b/ocs2_raisim/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_raisim/ocs2_legged_robot_raisim/CMakeLists.txt b/ocs2_raisim/ocs2_legged_robot_raisim/CMakeLists.txt index d0e4d54d3..f4a63fe62 100644 --- a/ocs2_raisim/ocs2_legged_robot_raisim/CMakeLists.txt +++ b/ocs2_raisim/ocs2_legged_robot_raisim/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_legged_robot_raisim) # Generate compile_commands.json for clang tools @@ -85,17 +85,17 @@ endif(cmake_clang_tools_FOUND) ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) install(TARGETS legged_robot_raisim_dummy - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + RUNTIME DESTINATION bin ) install(DIRECTORY config launch diff --git a/ocs2_raisim/ocs2_legged_robot_raisim/package.xml b/ocs2_raisim/ocs2_legged_robot_raisim/package.xml index 1f0d32b05..1c1cd97e7 100644 --- a/ocs2_raisim/ocs2_legged_robot_raisim/package.xml +++ b/ocs2_raisim/ocs2_legged_robot_raisim/package.xml @@ -11,7 +11,7 @@ BSD-3 - catkin + ament_cmake cmake_clang_tools diff --git a/ocs2_raisim/ocs2_raisim/CMakeLists.txt b/ocs2_raisim/ocs2_raisim/CMakeLists.txt index 5d205bc2b..d4e08de39 100644 --- a/ocs2_raisim/ocs2_raisim/CMakeLists.txt +++ b/ocs2_raisim/ocs2_raisim/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_raisim) -find_package(catkin REQUIRED) +find_package(ament_cmake REQUIRED) catkin_metapackage() diff --git a/ocs2_raisim/ocs2_raisim/package.xml b/ocs2_raisim/ocs2_raisim/package.xml index a79436c1a..74a02edd2 100644 --- a/ocs2_raisim/ocs2_raisim/package.xml +++ b/ocs2_raisim/ocs2_raisim/package.xml @@ -11,7 +11,7 @@ BSD - catkin + ament_cmake ocs2_raisim_core ocs2_raisim_ros diff --git a/ocs2_raisim/ocs2_raisim_core/CMakeLists.txt b/ocs2_raisim/ocs2_raisim_core/CMakeLists.txt index 8f35d9ffd..c58f9c245 100644 --- a/ocs2_raisim/ocs2_raisim_core/CMakeLists.txt +++ b/ocs2_raisim/ocs2_raisim_core/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_raisim_core) # Generate compile_commands.json for clang tools @@ -88,11 +88,11 @@ endif(cmake_clang_tools_FOUND) ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) diff --git a/ocs2_raisim/ocs2_raisim_core/package.xml b/ocs2_raisim/ocs2_raisim_core/package.xml index 9f6ca8293..124b52a4d 100644 --- a/ocs2_raisim/ocs2_raisim_core/package.xml +++ b/ocs2_raisim/ocs2_raisim_core/package.xml @@ -12,7 +12,7 @@ Jan Carius - catkin + ament_cmake cmake_modules cmake_clang_tools diff --git a/ocs2_raisim/ocs2_raisim_ros/CMakeLists.txt b/ocs2_raisim/ocs2_raisim_ros/CMakeLists.txt index ff406c0cc..64e9cefa9 100644 --- a/ocs2_raisim/ocs2_raisim_ros/CMakeLists.txt +++ b/ocs2_raisim/ocs2_raisim_ros/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_raisim_ros) # Generate compile_commands.json for clang tools @@ -59,14 +59,14 @@ endif(cmake_clang_tools_FOUND) ############# ## Install ## ############# -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) ############# diff --git a/ocs2_raisim/ocs2_raisim_ros/package.xml b/ocs2_raisim/ocs2_raisim_ros/package.xml index 05e5b595c..87832dfea 100644 --- a/ocs2_raisim/ocs2_raisim_ros/package.xml +++ b/ocs2_raisim/ocs2_raisim_ros/package.xml @@ -12,7 +12,7 @@ Jan Carius - catkin + ament_cmake cmake_modules cmake_clang_tools diff --git a/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt b/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt index c2dfdd74f..e49951cd7 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_ballbot) # Generate compile_commands.json for clang tools @@ -114,13 +114,13 @@ endif(cmake_clang_tools_FOUND) ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) install(DIRECTORY config diff --git a/ocs2_robotic_examples/ocs2_ballbot/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_ballbot/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_ballbot/package.xml b/ocs2_robotic_examples/ocs2_ballbot/package.xml index 64b1eb1b4..58c46b074 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/package.xml +++ b/ocs2_robotic_examples/ocs2_ballbot/package.xml @@ -10,7 +10,7 @@ TODO - catkin + ament_cmake cmake_clang_tools pybind11_catkin diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt index 5121de843..1115b5316 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_ballbot_ros) # Generate compile_commands.json for clang tools @@ -142,11 +142,11 @@ endif(cmake_clang_tools_FOUND) ## Install ## ############# -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) install(TARGETS ballbot_mpc ballbot_dummy_test ballbot_target ballbot_sqp - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + RUNTIME DESTINATION bin ) install(DIRECTORY launch rviz DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_ballbot_ros/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml b/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml index 3a9e207f5..4b7245035 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml @@ -10,7 +10,7 @@ TODO - catkin + ament_cmake cmake_clang_tools tf diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp index 4d558f2ff..857b56cf8 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp @@ -121,7 +121,7 @@ int main(int argc, char** argv) { ROS_INFO_STREAM("Waiting for the initial policy ..."); mpcMrtInterface.setCurrentObservation(initObservation); mpcMrtInterface.getReferenceManager().setTargetTrajectories(initTargetTrajectories); - while (!mpcMrtInterface.initialPolicyReceived() && ros::ok() && ros::master::check()) { + while (!mpcMrtInterface.initialPolicyReceived() && rclcpp::ok()) { mpcMrtInterface.advanceMpc(); ros::WallRate(ballbotInterface.mpcSettings().mrtDesiredFrequency_).sleep(); } diff --git a/ocs2_robotic_examples/ocs2_cartpole/CMakeLists.txt b/ocs2_robotic_examples/ocs2_cartpole/CMakeLists.txt index 60df896fa..a108c071f 100644 --- a/ocs2_robotic_examples/ocs2_cartpole/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_cartpole/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_cartpole) # Generate compile_commands.json for clang tools @@ -91,13 +91,13 @@ endif(cmake_clang_tools_FOUND) ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) install(DIRECTORY config diff --git a/ocs2_robotic_examples/ocs2_cartpole/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_cartpole/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_cartpole/package.xml b/ocs2_robotic_examples/ocs2_cartpole/package.xml index 09a2d9601..a064e9143 100644 --- a/ocs2_robotic_examples/ocs2_cartpole/package.xml +++ b/ocs2_robotic_examples/ocs2_cartpole/package.xml @@ -10,7 +10,7 @@ TODO - catkin + ament_cmake roslib cmake_clang_tools diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_cartpole_ros/CMakeLists.txt index 0144a76fa..66517ce32 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_cartpole_ros) # Generate compile_commands.json for clang tools @@ -96,11 +96,11 @@ endif(cmake_clang_tools_FOUND) ## Install ## ############# -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) install(TARGETS cartpole_mpc cartpole_dummy_test - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + RUNTIME DESTINATION bin ) install(DIRECTORY launch rviz DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_cartpole_ros/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/package.xml b/ocs2_robotic_examples/ocs2_cartpole_ros/package.xml index ea4defbe7..0f6eb54e6 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/package.xml @@ -10,7 +10,7 @@ TODO - catkin + ament_cmake roslib cmake_clang_tools diff --git a/ocs2_robotic_examples/ocs2_double_integrator/CMakeLists.txt b/ocs2_robotic_examples/ocs2_double_integrator/CMakeLists.txt index 6df44a37f..1a12fc185 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_double_integrator/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_double_integrator) # Generate compile_commands.json for clang tools @@ -112,12 +112,12 @@ endif(cmake_clang_tools_FOUND) ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} diff --git a/ocs2_robotic_examples/ocs2_double_integrator/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_double_integrator/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_double_integrator/package.xml b/ocs2_robotic_examples/ocs2_double_integrator/package.xml index 62828b4a5..3ded141ae 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator/package.xml +++ b/ocs2_robotic_examples/ocs2_double_integrator/package.xml @@ -10,7 +10,7 @@ TODO - catkin + ament_cmake cmake_clang_tools diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_double_integrator_ros/CMakeLists.txt index 7b4057cbd..8dd408019 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_double_integrator_ros) # Generate compile_commands.json for clang tools @@ -109,15 +109,15 @@ endif(cmake_clang_tools_FOUND) ## Install ## ############# -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) install( TARGETS double_integrator_mpc double_integrator_dummy_test double_integrator_target - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + RUNTIME DESTINATION bin ) install(DIRECTORY launch rviz DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_double_integrator_ros/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/package.xml b/ocs2_robotic_examples/ocs2_double_integrator_ros/package.xml index 1cd0de99b..4cfa3b3ba 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/package.xml @@ -10,7 +10,7 @@ TODO - catkin + ament_cmake cmake_clang_tools diff --git a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt index 911cf0cef..fe43b4bba 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt @@ -1,10 +1,42 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_legged_robot) # Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CATKIN_PACKAGE_DEPENDENCIES +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_sqp REQUIRED) +find_package(ocs2_thirdparty REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_msgs REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(ocs2_centroidal_model REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(OpenMP REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(urdf REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(robot_state_publisher REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_srvs REQUIRED) + +set(AMENT_PACKAGE_DEPENDENCIES ocs2_core ocs2_oc ocs2_ddp @@ -14,38 +46,41 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_pinocchio_interface ocs2_centroidal_model ocs2_robotic_assets + ocs2_thirdparty + ocs2_msgs + ocs2_ros_interfaces + rclcpp + kdl_parser + tf2 + urdf + robot_state_publisher + tf2_ros + tf2_geometry_msgs + std_srvs + # trajectory_msgs + visualization_msgs + pinocchio ) -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) find_package(Boost REQUIRED COMPONENTS system filesystem + log + log_setup ) find_package(Eigen3 3.3 REQUIRED NO_MODULE) -find_package(PkgConfig REQUIRED) -pkg_check_modules(pinocchio REQUIRED pinocchio) +if ( EXISTS "/opt/openrobots/lib/libpinocchio.so") +else() + message(FATAL_ERROR "Install Pinocchio first. Instructions: https://stack-of-tasks.github.io/pinocchio/download.html") +endif() -################################### -## catkin specific configuration ## -################################### +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fopenmp -std=c++11 -Wfatal-errors") -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost - pinocchio -) +list(APPEND CMAKE_PREFIX_PATH "/opt/openrobots/") +find_package(pinocchio REQUIRED) ########### ## Build ## @@ -54,7 +89,7 @@ catkin_package( # Resolve for the package path at compile time. configure_file ( "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h.in" - "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY + "${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME}/package_path.h" @ONLY ) set(FLAGS @@ -99,16 +134,30 @@ add_library(${PROJECT_NAME} src/LeggedRobotInterface.cpp src/LeggedRobotPreComputation.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} + +set_target_properties(${PROJECT_NAME} + PROPERTIES INSTALL_RPATH "/opt/openrobots/lib" +) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ ) + target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} + Eigen3::Eigen ${Boost_LIBRARIES} ${pinocchio_LIBRARIES} dl ) + +ament_target_dependencies( + ocs2_legged_robot + ${AMENT_PACKAGE_DEPENDENCIES} +) + target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) +target_compile_definitions(${PROJECT_NAME} PRIVATE BOOST_LOG_DYN_LINK=true) ######################### ### CLANG TOOLING ### @@ -129,36 +178,87 @@ endif(cmake_clang_tools_FOUND) ## Install ## ############# -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# install(TARGETS ${PROJECT_NAME} +# EXPORT export_${PROJECT_NAME} +# ARCHIVE DESTINATION lib +# LIBRARY DESTINATION lib +# RUNTIME DESTINATION bin +# ) + +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + RUNTIME DESTINATION lib/${PROJECT_NAME} + INCLUDES DESTINATION include ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + + +install(DIRECTORY include/ + DESTINATION include ) + install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) +ament_export_include_directories(include) + +ament_export_libraries(${PROJECT_NAME}) + +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_export_dependencies(ocs2_core) +ament_export_dependencies(ocs2_mpc) +ament_export_dependencies(ocs2_robotic_tools) +ament_export_dependencies(ocs2_oc) +ament_export_dependencies(ocs2_thirdparty) +ament_export_dependencies(ocs2_ddp) +ament_export_dependencies(ocs2_msgs) +ament_export_dependencies(ocs2_pinocchio_interface) +ament_export_dependencies(ocs2_centroidal_model) +ament_export_dependencies(ocs2_ros_interfaces) +ament_export_dependencies(tf2) +ament_export_dependencies(urdf) +ament_export_dependencies(kdl_parser) +ament_export_dependencies(robot_state_publisher) +ament_export_dependencies(visualization_msgs) +ament_export_dependencies(nav_msgs) +ament_export_dependencies(std_srvs) + ############# ## Testing ## ############# -catkin_add_gtest(${PROJECT_NAME}_test - test/AnymalFactoryFunctions.cpp - test/constraint/testEndEffectorLinearConstraint.cpp - test/constraint/testFrictionConeConstraint.cpp - test/constraint/testZeroForceConstraint.cpp -) -target_include_directories(${PROJECT_NAME}_test PRIVATE - test/include - ${PROJECT_BINARY_DIR}/include -) -target_link_libraries(${PROJECT_NAME}_test - gtest_main - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} -) -target_compile_options(${PROJECT_NAME}_test PRIVATE ${FLAGS}) +# if(BUILD_TESTING) + +# # Include linting tests +# find_package(ament_lint_auto REQUIRED) +# find_package(ament_cmake_gtest REQUIRED) +# ament_lint_auto_find_test_dependencies() + +# # test_time_triggered_rollout +# ament_add_gtest(${PROJECT_NAME}_test +# test/AnymalFactoryFunctions.cpp +# test/constraint/testEndEffectorLinearConstraint.cpp +# test/constraint/testFrictionConeConstraint.cpp +# test/constraint/testZeroForceConstraint.cpp +# ) + +# target_link_libraries(${PROJECT_NAME}_test +# ${PROJECT_NAME} +# ${Boost_LIBRARIES} +# gtest_main +# ) + +# target_compile_options(${PROJECT_NAME}_test PRIVATE ${FLAGS}) + +# target_include_directories(${PROJECT_NAME}_test PUBLIC +# $ +# $ +# ) +# endif(BUILD_TESTING) + +ament_package() + diff --git a/ocs2_robotic_examples/ocs2_legged_robot/package.xml b/ocs2_robotic_examples/ocs2_legged_robot/package.xml index bb62d1e9d..73dde07fc 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/package.xml +++ b/ocs2_robotic_examples/ocs2_legged_robot/package.xml @@ -1,5 +1,5 @@ - - + + ocs2_legged_robot 0.0.1 The ocs2_legged_robot package @@ -10,17 +10,22 @@ BSD-3 - catkin + ament_cmake - ocs2_core - ocs2_oc - ocs2_ddp - ocs2_mpc - ocs2_sqp - ocs2_robotic_assets - ocs2_robotic_tools - ocs2_pinocchio_interface - ocs2_centroidal_model - pinocchio + ocs2_core + ocs2_oc + ocs2_ddp + ocs2_mpc + ocs2_sqp + ocs2_msgs + ocs2_robotic_assets + ocs2_robotic_tools + ocs2_pinocchio_interface + ocs2_ros_interfaces + ocs2_centroidal_model + pinocchio + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt index d5c30446e..1445658da 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt @@ -1,15 +1,53 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_legged_robot_ros) # Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CATKIN_PACKAGE_DEPENDENCIES - roslib - cmake_modules - tf +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_thirdparty REQUIRED) +find_package(ocs2_sqp REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_msgs REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(ocs2_centroidal_model REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(OpenMP REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(urdf REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(robot_state_publisher REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(ocs2_legged_robot REQUIRED) + + + +set(AMENT_PACKAGE_DEPENDENCIES + # roslib + # cmake_modules + tf2 + tf2_ros urdf kdl_parser + rclcpp robot_state_publisher ocs2_core ocs2_oc @@ -25,36 +63,24 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_legged_robot ) -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - find_package(Boost REQUIRED COMPONENTS system filesystem + log + log_setup ) find_package(Eigen3 3.3 REQUIRED NO_MODULE) -find_package(PkgConfig REQUIRED) -pkg_check_modules(pinocchio REQUIRED pinocchio) +if ( EXISTS "/opt/openrobots/lib/libpinocchio.so") +else() + message(FATAL_ERROR "Install Pinocchio first. Instructions: https://stack-of-tasks.github.io/pinocchio/download.html") +endif() -################################### -## catkin specific configuration ## -################################### +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fopenmp -std=c++11 -Wfatal-errors") -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost - pinocchio -) +list(APPEND CMAKE_PREFIX_PATH "/opt/openrobots/") +find_package(pinocchio REQUIRED) ########### ## Build ## @@ -65,7 +91,6 @@ include_directories( ${pinocchio_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} ) link_directories( @@ -78,25 +103,50 @@ add_library(${PROJECT_NAME} src/gait/GaitReceiver.cpp src/visualization/LeggedRobotVisualizer.cpp ) -add_dependencies(${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} + +set_target_properties(${PROJECT_NAME} + PROPERTIES INSTALL_RPATH "/opt/openrobots/lib") + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ ) + + +# TODO: NOT NEEDED? +# add_dependencies(${PROJECT_NAME} +# ${${PROJECT_NAME}_EXPORTED_TARGETS} +# ${catkin_EXPORTED_TARGETS} +# ) + target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} ) + +ament_target_dependencies(${PROJECT_NAME} + ${AMENT_PACKAGE_DEPENDENCIES} +) + target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) # DDP-MPC node for legged robot add_executable(legged_robot_ddp_mpc src/LeggedRobotDdpMpcNode.cpp ) + +set_target_properties(legged_robot_ddp_mpc + PROPERTIES INSTALL_RPATH "/opt/openrobots/lib") + +target_include_directories(legged_robot_ddp_mpc PUBLIC + $ + $ +) + add_dependencies(legged_robot_ddp_mpc ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} ) + target_link_libraries(legged_robot_ddp_mpc ${PROJECT_NAME} ${catkin_LIBRARIES} @@ -107,26 +157,42 @@ target_compile_options(legged_robot_ddp_mpc PRIVATE ${OCS2_CXX_FLAGS}) add_executable(legged_robot_sqp_mpc src/LeggedRobotSqpMpcNode.cpp ) +set_target_properties(legged_robot_sqp_mpc + PROPERTIES INSTALL_RPATH "/opt/openrobots/lib") + +target_include_directories(legged_robot_sqp_mpc PUBLIC + $ + $ +) + add_dependencies(legged_robot_sqp_mpc ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} ) + target_link_libraries(legged_robot_sqp_mpc ${PROJECT_NAME} ${catkin_LIBRARIES} ) -target_compile_options(legged_robot_sqp_mpc PRIVATE ${OCS2_CXX_FLAGS}) +target_compile_options(legged_robot_sqp_mpc PRIVATE ${OCS2_CXX_FLAGS}) # Dummy node add_executable(legged_robot_dummy src/LeggedRobotDummyNode.cpp ) +set_target_properties(legged_robot_dummy + PROPERTIES INSTALL_RPATH "/opt/openrobots/lib") + +target_include_directories(legged_robot_dummy PUBLIC + $ + $ +) + add_dependencies(legged_robot_dummy ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} ) + target_link_libraries(legged_robot_dummy ${PROJECT_NAME} ${catkin_LIBRARIES} @@ -137,11 +203,19 @@ target_compile_options(legged_robot_dummy PRIVATE ${OCS2_CXX_FLAGS}) add_executable(legged_robot_target src/LeggedRobotPoseCommandNode.cpp ) +set_target_properties(legged_robot_target + PROPERTIES INSTALL_RPATH "/opt/openrobots/lib") + +target_include_directories(legged_robot_target PUBLIC + $ + $ +) + add_dependencies(legged_robot_target ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} ) + target_link_libraries(legged_robot_target ${PROJECT_NAME} ${catkin_LIBRARIES} @@ -152,11 +226,19 @@ target_compile_options(legged_robot_target PRIVATE ${OCS2_CXX_FLAGS}) add_executable(legged_robot_gait_command src/LeggedRobotGaitCommandNode.cpp ) +set_target_properties(legged_robot_gait_command + PROPERTIES INSTALL_RPATH "/opt/openrobots/lib") + +target_include_directories(legged_robot_gait_command PUBLIC + $ + $ +) + add_dependencies(legged_robot_gait_command ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} ) + target_link_libraries(legged_robot_gait_command ${PROJECT_NAME} ${catkin_LIBRARIES} @@ -173,10 +255,10 @@ if(cmake_clang_tools_FOUND) TARGETS ${PROJECT_NAME} legged_robot_ddp_mpc - legged_robot_sqp_mpc - legged_robot_dummy - legged_robot_target - legged_robot_gait_command + # legged_robot_sqp_mpc + # legged_robot_dummy + # legged_robot_target + # legged_robot_gait_command SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include CF_WERROR @@ -186,14 +268,16 @@ endif(cmake_clang_tools_FOUND) ############# ## Install ## ############# -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) + install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) + install( TARGETS legged_robot_ddp_mpc @@ -201,12 +285,34 @@ install( legged_robot_dummy legged_robot_target legged_robot_gait_command - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) -install(DIRECTORY launch rviz - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + RUNTIME DESTINATION lib/${PROJECT_NAME} + INCLUDES DESTINATION include ) +install(DIRECTORY launch rviz DESTINATION share/${PROJECT_NAME}) + +ament_export_include_directories(include) + +ament_export_dependencies(ocs2_core) +ament_export_dependencies(ocs2_mpc) +ament_export_dependencies(ocs2_robotic_tools) +ament_export_dependencies(ocs2_oc) +ament_export_dependencies(ocs2_thirdparty) +ament_export_dependencies(ocs2_ddp) +ament_export_dependencies(ocs2_msgs) +ament_export_dependencies(ocs2_sqp) +ament_export_dependencies(ocs2_robotic_assets) +ament_export_dependencies(ocs2_pinocchio_interface) +ament_export_dependencies(ocs2_centroidal_model) +ament_export_dependencies(ocs2_ros_interfaces) +ament_export_dependencies(tf2) +ament_export_dependencies(urdf) +ament_export_dependencies(kdl_parser) +ament_export_dependencies(robot_state_publisher) +ament_export_dependencies(visualization_msgs) +ament_export_dependencies(nav_msgs) +ament_export_dependencies(std_srvs) +ament_package() ############# ## Testing ## ############# diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitKeyboardPublisher.h b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitKeyboardPublisher.h index bc399bdb5..56ea817dc 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitKeyboardPublisher.h +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitKeyboardPublisher.h @@ -32,8 +32,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include -#include +#include +#include #include @@ -43,7 +43,7 @@ namespace legged_robot { /** This class implements ModeSequence communication using ROS. */ class GaitKeyboardPublisher { public: - GaitKeyboardPublisher(ros::NodeHandle nodeHandle, const std::string& gaitFile, const std::string& robotName, bool verbose = false); + GaitKeyboardPublisher(rclcpp::Node::SharedPtr &nodeHandle, const std::string& gaitFile, const std::string& robotName, bool verbose = false); /** Prints the command line interface and responds to user input. Function returns after one user input. */ void getKeyboardCommand(); @@ -55,7 +55,7 @@ class GaitKeyboardPublisher { std::vector gaitList_; std::map gaitMap_; - ros::Publisher modeSequenceTemplatePublisher_; + rclcpp::Publisher::SharedPtr modeSequenceTemplatePublisher_; }; } // namespace legged_robot diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitReceiver.h b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitReceiver.h index e6327327d..9e37e1abf 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitReceiver.h +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitReceiver.h @@ -31,10 +31,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include +#include #include -#include +#include + #include #include @@ -45,7 +46,7 @@ namespace ocs2 { namespace legged_robot { class GaitReceiver : public SolverSynchronizedModule { public: - GaitReceiver(ros::NodeHandle nodeHandle, std::shared_ptr gaitSchedulePtr, const std::string& robotName); + GaitReceiver(rclcpp::Node::SharedPtr &nodeHandle, std::shared_ptr gaitSchedulePtr, const std::string& robotName); void preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, const ReferenceManagerInterface& referenceManager) override; @@ -53,11 +54,11 @@ class GaitReceiver : public SolverSynchronizedModule { void postSolverRun(const PrimalSolution& primalSolution) override{}; private: - void mpcModeSequenceCallback(const ocs2_msgs::mode_schedule::ConstPtr& msg); + void mpcModeSequenceCallback(const ocs2_msgs::msg::ModeSchedule::SharedPtr msg); std::shared_ptr gaitSchedulePtr_; - ros::Subscriber mpcModeSequenceSubscriber_; + rclcpp::Subscription::SharedPtr mpcModeSequenceSubscriber_; std::mutex receivedGaitMutex_; std::atomic_bool gaitUpdated_; diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/ModeSequenceTemplateRos.h b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/ModeSequenceTemplateRos.h index e4bb1e330..c0cac8f8c 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/ModeSequenceTemplateRos.h +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/ModeSequenceTemplateRos.h @@ -31,7 +31,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include +#include #include @@ -39,17 +39,17 @@ namespace ocs2 { namespace legged_robot { /** Convert mode sequence template to ROS message */ -inline ocs2_msgs::mode_schedule createModeSequenceTemplateMsg(const ModeSequenceTemplate& modeSequenceTemplate) { - ocs2_msgs::mode_schedule modeScheduleMsg; - modeScheduleMsg.eventTimes.assign(modeSequenceTemplate.switchingTimes.begin(), modeSequenceTemplate.switchingTimes.end()); - modeScheduleMsg.modeSequence.assign(modeSequenceTemplate.modeSequence.begin(), modeSequenceTemplate.modeSequence.end()); +inline ocs2_msgs::msg::ModeSchedule createModeSequenceTemplateMsg(const ModeSequenceTemplate& modeSequenceTemplate) { + ocs2_msgs::msg::ModeSchedule modeScheduleMsg; + modeScheduleMsg.event_times.assign(modeSequenceTemplate.switchingTimes.begin(), modeSequenceTemplate.switchingTimes.end()); + modeScheduleMsg.mode_sequence.assign(modeSequenceTemplate.modeSequence.begin(), modeSequenceTemplate.modeSequence.end()); return modeScheduleMsg; } /** Convert ROS message to mode sequence template */ -inline ModeSequenceTemplate readModeSequenceTemplateMsg(const ocs2_msgs::mode_schedule& modeScheduleMsg) { - std::vector switchingTimes(modeScheduleMsg.eventTimes.begin(), modeScheduleMsg.eventTimes.end()); - std::vector modeSequence(modeScheduleMsg.modeSequence.begin(), modeScheduleMsg.modeSequence.end()); +inline ModeSequenceTemplate readModeSequenceTemplateMsg(const ocs2_msgs::msg::ModeSchedule& modeScheduleMsg) { + std::vector switchingTimes(modeScheduleMsg.event_times.begin(), modeScheduleMsg.event_times.end()); + std::vector modeSequence(modeScheduleMsg.mode_sequence.begin(), modeScheduleMsg.mode_sequence.end()); return {switchingTimes, modeSequence}; } diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h index a3c1b9dc6..534394bfb 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h @@ -29,9 +29,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include -#include -#include +#include +#include +#include +#include #include #include @@ -43,7 +44,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace legged_robot { -class LeggedRobotVisualizer : public DummyObserver { +class LeggedRobotVisualizer : public DummyObserver, public robot_state_publisher::RobotStatePublisher { public: /** Visualization settings (publicly available) */ std::string frameId_ = "odom"; // Frame name all messages are published in @@ -63,44 +64,44 @@ class LeggedRobotVisualizer : public DummyObserver { * @param maxUpdateFrequency : maximum publish frequency measured in MPC time. */ LeggedRobotVisualizer(PinocchioInterface pinocchioInterface, CentroidalModelInfo centroidalModelInfo, - const PinocchioEndEffectorKinematics& endEffectorKinematics, ros::NodeHandle& nodeHandle, - scalar_t maxUpdateFrequency = 100.0); + const PinocchioEndEffectorKinematics& endEffectorKinematics, rclcpp::Node::SharedPtr& nodeHandle, + const std::string &urdf_xml, scalar_t maxUpdateFrequency = 100.0); ~LeggedRobotVisualizer() override = default; void update(const SystemObservation& observation, const PrimalSolution& primalSolution, const CommandData& command) override; - void launchVisualizerNode(ros::NodeHandle& nodeHandle); + void launchVisualizerNode(rclcpp::Node::SharedPtr& nodeHandle, const std::string &urdf_xml); void publishTrajectory(const std::vector& system_observation_array, scalar_t speed = 1.0); - void publishObservation(ros::Time timeStamp, const SystemObservation& observation); + void publishObservation(rclcpp::Time timeStamp, const SystemObservation& observation); - void publishDesiredTrajectory(ros::Time timeStamp, const TargetTrajectories& targetTrajectories); + void publishDesiredTrajectory(rclcpp::Time timeStamp, const TargetTrajectories& targetTrajectories); - void publishOptimizedStateTrajectory(ros::Time timeStamp, const scalar_array_t& mpcTimeTrajectory, + void publishOptimizedStateTrajectory(rclcpp::Time timeStamp, const scalar_array_t& mpcTimeTrajectory, const vector_array_t& mpcStateTrajectory, const ModeSchedule& modeSchedule); private: LeggedRobotVisualizer(const LeggedRobotVisualizer&) = delete; - void publishJointTransforms(ros::Time timeStamp, const vector_t& jointAngles) const; - void publishBaseTransform(ros::Time timeStamp, const vector_t& basePose); - void publishCartesianMarkers(ros::Time timeStamp, const contact_flag_t& contactFlags, const std::vector& feetPositions, + void publishJointTransforms(rclcpp::Time timeStamp, const vector_t& jointAngles); + void publishBaseTransform(rclcpp::Time timeStamp, const vector_t& basePose); + void publishCartesianMarkers(rclcpp::Time timeStamp, const contact_flag_t& contactFlags, const std::vector& feetPositions, const std::vector& feetForces) const; PinocchioInterface pinocchioInterface_; const CentroidalModelInfo centroidalModelInfo_; std::unique_ptr endEffectorKinematicsPtr_; - tf::TransformBroadcaster tfBroadcaster_; - std::unique_ptr robotStatePublisherPtr_; + std::unique_ptr tfBroadcaster_; + rclcpp::Node::SharedPtr nodeHandle_; - ros::Publisher costDesiredBasePositionPublisher_; - std::vector costDesiredFeetPositionPublishers_; + rclcpp::Publisher::SharedPtr costDesiredBasePositionPublisher_; + std::vector::SharedPtr> costDesiredFeetPositionPublishers_; - ros::Publisher stateOptimizedPublisher_; + rclcpp::Publisher::SharedPtr stateOptimizedPublisher_; - ros::Publisher currentStatePublisher_; + rclcpp::Publisher::SharedPtr currentStatePublisher_; scalar_t lastTime_; scalar_t minPublishTimeDifference_; diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch.py b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch.py new file mode 100644 index 000000000..4deb64fad --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch.py @@ -0,0 +1,102 @@ + +import os +import sys + +from ament_index_python.packages import get_package_share_directory +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch import LaunchDescription, LaunchService +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + ocs2_robotic_assets_dir = get_package_share_directory('ocs2_robotic_assets') + ocs2_legged_robot_dir = get_package_share_directory('ocs2_legged_robot') + description_name = "legged_robot_description" + + task_file = os.path.join(ocs2_legged_robot_dir, 'config', 'mpc', 'task.info') + reference_file = os.path.join(ocs2_legged_robot_dir, 'config', 'command', 'reference.info') + urdf_file = os.path.join(ocs2_robotic_assets_dir, 'resources', 'anymal_c', 'urdf', 'anymal.urdf') + gait_command_file = os.path.join(ocs2_legged_robot_dir, 'config', 'command', 'gait.info') + robot_name = "legged_robot" + config_name = "mpc" + use_sim_time = False + + urdf_content = "" + with open(urdf_file, 'r') as fp: + urdf_content = fp.read() + + robot_description = {"robot_description": urdf_content} + + # only for rviz + description_name = "legged_robot_description" + + return LaunchDescription([ + + Node( + name='legged_robot_ddp_mpc', + package='ocs2_legged_robot_ros', + executable='legged_robot_ddp_mpc', + parameters=[ + {"use_sim_time": use_sim_time}, + {"urdf_file": urdf_file}, + {"task_file": task_file}, + {"reference_file": reference_file}, + ], + output='screen', + ), + Node( + name='legged_robot_dummy', + package='ocs2_legged_robot_ros', + executable='legged_robot_dummy', + parameters=[ + robot_description, + {"use_sim_time": use_sim_time}, + {"urdf_file": urdf_file}, + {"task_file": task_file}, + {"reference_file": reference_file}, + ], + output='screen', + prefix=["gnome-terminal --"], + ), + Node( + name='legged_robot_target', + package='ocs2_legged_robot_ros', + executable='legged_robot_target', + parameters=[ + {"use_sim_time": use_sim_time}, + {"reference_file": reference_file}, + ], + output='screen', + prefix=["gnome-terminal --"], + ), + Node( + name='legged_robot_gait_command', + package='ocs2_legged_robot_ros', + executable='legged_robot_gait_command', + parameters=[ + {"use_sim_time": use_sim_time}, + {"gait_command_file": gait_command_file}, + ], + output='screen', + prefix=["gnome-terminal --"], + ), + ]) + + +def main(argv=sys.argv[1:]): + """Run lifecycle nodes via launch.""" + ld = generate_launch_description() + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return ls.run() + + +if __name__ == '__main__': + main() diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml b/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml index aa2943272..46c5f999a 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml @@ -1,5 +1,5 @@ - - + + ocs2_legged_robot_ros 0.0.1 The ocs2_legged_robot_ros package @@ -10,28 +10,32 @@ BSD-3 - catkin + ament_cmake - roslib - tf - urdf - kdl_parser - robot_state_publisher - cmake_modules + tf2 + tf2_ros + urdf + kdl_parser + robot_state_publisher + cmake_modules - ocs2_core - ocs2_oc - ocs2_ddp - ocs2_mpc - ocs2_sqp - ocs2_robotic_tools - ocs2_pinocchio_interface - ocs2_centroidal_model - ocs2_robotic_assets - ocs2_msgs - ocs2_legged_robot - ocs2_ros_interfaces + ocs2_core + ocs2_oc + ocs2_ddp + ocs2_mpc + ocs2_sqp + ocs2_robotic_tools + ocs2_pinocchio_interface + ocs2_centroidal_model + ocs2_robotic_assets + ocs2_robotic_assets + ocs2_msgs + ocs2_legged_robot + ocs2_ros_interfaces - pinocchio + pinocchio + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp index b37272d69..294867655 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp @@ -27,8 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include -#include +#include #include #include @@ -40,17 +39,31 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. using namespace ocs2; using namespace legged_robot; +static auto LOGGER = rclcpp::get_logger("LeggedRobotDDPMpcNode"); + +auto declareAndGetStringParam = [] (rclcpp::Node::SharedPtr &node, const std::string ¶m, std::string ¶m_value) { + if (!node->has_parameter(param)) + { + node->declare_parameter(param, std::string("")); + } + rclcpp::Parameter parameter; + node->get_parameter(param, parameter); + param_value = parameter.as_string(); + RCLCPP_INFO_STREAM(LOGGER, "Retrieved parameter " << param << " with value " << param_value); +}; + int main(int argc, char** argv) { const std::string robotName = "legged_robot"; // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr nodeHandle = std::make_shared(robotName + "_mpc"); // Get node parameters std::string taskFile, urdfFile, referenceFile; - nodeHandle.getParam("/taskFile", taskFile); - nodeHandle.getParam("/referenceFile", referenceFile); - nodeHandle.getParam("/urdfFile", urdfFile); + + declareAndGetStringParam(nodeHandle, "task_file", taskFile); + declareAndGetStringParam(nodeHandle, "urdf_file", urdfFile); + declareAndGetStringParam(nodeHandle, "reference_file", referenceFile); // Robot interface LeggedRobotInterface interface(taskFile, urdfFile, referenceFile); diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp index 4fc71d2de..f7da1f5eb 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp @@ -27,8 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include -#include +#include #include #include @@ -40,18 +39,41 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. using namespace ocs2; using namespace legged_robot; +static auto LOGGER = rclcpp::get_logger("LeggedRobotDummyNode"); + +auto declareAndGetStringParam = [] (rclcpp::Node::SharedPtr &node, const std::string ¶m, std::string ¶m_value) { + if (!node->has_parameter(param)) + { + node->declare_parameter(param, std::string("")); + } + rclcpp::Parameter parameter; + node->get_parameter(param, parameter); + param_value = parameter.as_string(); + RCLCPP_INFO_STREAM(LOGGER, "Retrieved parameter " << param << " with value " << param_value); +}; + +auto readFile = [] (const std::string &path, std::string &file_content) { + std::ifstream stream( path.c_str() ); + if (!stream) + { + RCLCPP_ERROR_STREAM(LOGGER, "File " << path << " does not exist"); + return; + } + file_content = std::string((std::istreambuf_iterator(stream)), + std::istreambuf_iterator()); +}; int main(int argc, char** argv) { const std::string robotName = "legged_robot"; // Initialize ros node - ros::init(argc, argv, robotName + "_mrt"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr nodeHandle = std::make_shared(robotName + "_mrt"); // Get node parameters std::string taskFile, urdfFile, referenceFile; - nodeHandle.getParam("/taskFile", taskFile); - nodeHandle.getParam("/urdfFile", urdfFile); - nodeHandle.getParam("/referenceFile", referenceFile); + declareAndGetStringParam(nodeHandle, "task_file", taskFile); + declareAndGetStringParam(nodeHandle, "urdf_file", urdfFile); + declareAndGetStringParam(nodeHandle, "reference_file", referenceFile); // Robot interface LeggedRobotInterface interface(taskFile, urdfFile, referenceFile); @@ -61,12 +83,16 @@ int main(int argc, char** argv) { mrt.initRollout(&interface.getRollout()); mrt.launchNodes(nodeHandle); + std::string urdf_string; + readFile(urdfFile, urdf_string); + // Visualization CentroidalModelPinocchioMapping pinocchioMapping(interface.getCentroidalModelInfo()); PinocchioEndEffectorKinematics endEffectorKinematics(interface.getPinocchioInterface(), pinocchioMapping, interface.modelSettings().contactNames3DoF); std::shared_ptr leggedRobotVisualizer( - new LeggedRobotVisualizer(interface.getPinocchioInterface(), interface.getCentroidalModelInfo(), endEffectorKinematics, nodeHandle)); + new LeggedRobotVisualizer(interface.getPinocchioInterface(), interface.getCentroidalModelInfo(), + endEffectorKinematics, nodeHandle, urdf_string)); // Dummy legged robot MRT_ROS_Dummy_Loop leggedRobotDummySimulator(mrt, interface.mpcSettings().mrtDesiredFrequency_, diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotGaitCommandNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotGaitCommandNode.cpp index e3ac5cbbb..7381cb10c 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotGaitCommandNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotGaitCommandNode.cpp @@ -27,28 +27,40 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include -#include +#include #include "ocs2_legged_robot_ros/gait/GaitKeyboardPublisher.h" using namespace ocs2; using namespace legged_robot; +static auto LOGGER = rclcpp::get_logger("LeggedRobotGaitCommandNode"); + +auto declareAndGetStringParam = [] (rclcpp::Node::SharedPtr &node, const std::string ¶m, std::string ¶m_value) { + if (!node->has_parameter(param)) + { + node->declare_parameter(param, std::string("")); + } + rclcpp::Parameter parameter; + node->get_parameter(param, parameter); + param_value = parameter.as_string(); + RCLCPP_INFO_STREAM(LOGGER, "Retrieved parameter " << param << " with value " << param_value); +}; int main(int argc, char* argv[]) { const std::string robotName = "legged_robot"; // Initialize ros node - ros::init(argc, argv, robotName + "_mpc_mode_schedule"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr nodeHandle = rclcpp::Node::make_shared(robotName + "_mpc_mode_schedule"); + // Get node parameters std::string gaitCommandFile; - nodeHandle.getParam("/gaitCommandFile", gaitCommandFile); + declareAndGetStringParam(nodeHandle, "gait_command_file", gaitCommandFile); std::cerr << "Loading gait file: " << gaitCommandFile << std::endl; GaitKeyboardPublisher gaitCommand(nodeHandle, gaitCommandFile, robotName, true); - while (ros::ok() && ros::master::check()) { + while (rclcpp::ok()) { gaitCommand.getKeyboardCommand(); } diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPoseCommandNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPoseCommandNode.cpp index 1e827ef3e..b4137e3db 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPoseCommandNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPoseCommandNode.cpp @@ -29,8 +29,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include -#include +#include #include #include @@ -94,15 +93,28 @@ TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTar return {timeTrajectory, stateTrajectory, inputTrajectory}; } +static auto LOGGER = rclcpp::get_logger("LeggedRobotPoseCommandNode"); + +auto declareAndGetStringParam = [] (rclcpp::Node::SharedPtr &node, const std::string ¶m, std::string ¶m_value) { + if (!node->has_parameter(param)) + { + node->declare_parameter(param, std::string("")); + } + rclcpp::Parameter parameter; + node->get_parameter(param, parameter); + param_value = parameter.as_string(); + RCLCPP_INFO_STREAM(LOGGER, "Retrieved parameter " << param << " with value " << param_value); +}; + int main(int argc, char* argv[]) { const std::string robotName = "legged_robot"; // Initialize ros node - ::ros::init(argc, argv, robotName + "_target"); - ::ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr nodeHandle = rclcpp::Node::make_shared(robotName + "_target"); // Get node parameters std::string referenceFile; - nodeHandle.getParam("/referenceFile", referenceFile); + declareAndGetStringParam(nodeHandle, "reference_file", referenceFile); loadData::loadCppDataType(referenceFile, "comHeight", comHeight); loadData::loadEigenMatrix(referenceFile, "defaultJointState", defaultJointState); diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp index 13953b284..26d17a606 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp @@ -27,8 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include -#include +#include #include #include @@ -40,17 +39,30 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. using namespace ocs2; using namespace legged_robot; +static auto LOGGER = rclcpp::get_logger("LeggedRobotSQPMPCNode"); + +auto declareAndGetStringParam = [] (rclcpp::Node::SharedPtr &node, const std::string ¶m, std::string ¶m_value) { + if (!node->has_parameter(param)) + { + node->declare_parameter(param, std::string("")); + } + rclcpp::Parameter parameter; + node->get_parameter(param, parameter); + param_value = parameter.as_string(); + RCLCPP_INFO_STREAM(LOGGER, "Retrieved parameter " << param << " with value " << param_value); +}; + int main(int argc, char** argv) { const std::string robotName = "legged_robot"; // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr nodeHandle = rclcpp::Node::make_shared(robotName + "_mpc"); // Get node parameters std::string taskFile, urdfFile, referenceFile; - nodeHandle.getParam("/taskFile", taskFile); - nodeHandle.getParam("/urdfFile", urdfFile); - nodeHandle.getParam("/referenceFile", referenceFile); + declareAndGetStringParam(nodeHandle, "task_file", taskFile); + declareAndGetStringParam(nodeHandle, "urdf_file", urdfFile); + declareAndGetStringParam(nodeHandle, "reference_file", referenceFile); // Robot interface LeggedRobotInterface interface(taskFile, urdfFile, referenceFile); diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitKeyboardPublisher.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitKeyboardPublisher.cpp index f381c062f..c9784c1d1 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitKeyboardPublisher.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitKeyboardPublisher.cpp @@ -33,7 +33,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include +#include #include "ocs2_legged_robot_ros/gait/ModeSequenceTemplateRos.h" @@ -43,18 +43,19 @@ namespace legged_robot { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -GaitKeyboardPublisher::GaitKeyboardPublisher(ros::NodeHandle nodeHandle, const std::string& gaitFile, const std::string& robotName, +GaitKeyboardPublisher::GaitKeyboardPublisher(rclcpp::Node::SharedPtr &nodeHandle, const std::string& gaitFile, const std::string& robotName, bool verbose) { - ROS_INFO_STREAM(robotName + "_mpc_mode_schedule node is setting up ..."); + RCLCPP_INFO_STREAM(rclcpp::get_logger("ocs2_legged_robot_ros"), robotName + "_mpc_mode_schedule node is setting up ..."); loadData::loadStdVector(gaitFile, "list", gaitList_, verbose); - modeSequenceTemplatePublisher_ = nodeHandle.advertise(robotName + "_mpc_mode_schedule", 1, true); + + modeSequenceTemplatePublisher_ = nodeHandle->create_publisher(robotName + "_mpc_mode_schedule", 1); gaitMap_.clear(); for (const auto& gaitName : gaitList_) { gaitMap_.insert({gaitName, loadModeSequenceTemplate(gaitFile, gaitName, verbose)}); } - ROS_INFO_STREAM(robotName + "_mpc_mode_schedule command node is ready."); + RCLCPP_INFO_STREAM(rclcpp::get_logger("ocs2_legged_robot_ros"), robotName + "_mpc_mode_schedule command node is ready."); } /******************************************************************************************************/ @@ -64,7 +65,7 @@ void GaitKeyboardPublisher::getKeyboardCommand() { const std::string commadMsg = "Enter the desired gait, for the list of available gait enter \"list\""; std::cout << commadMsg << ": "; - auto shouldTerminate = []() { return !ros::ok() || !ros::master::check(); }; + auto shouldTerminate = []() { return !rclcpp::ok(); }; const auto commandLine = stringToWords(getCommandLineString(shouldTerminate)); if (commandLine.empty()) { @@ -87,7 +88,7 @@ void GaitKeyboardPublisher::getKeyboardCommand() { try { ModeSequenceTemplate modeSequenceTemplate = gaitMap_.at(gaitCommand); - modeSequenceTemplatePublisher_.publish(createModeSequenceTemplateMsg(modeSequenceTemplate)); + modeSequenceTemplatePublisher_->publish(createModeSequenceTemplateMsg(modeSequenceTemplate)); } catch (const std::out_of_range& e) { std::cout << "Gait \"" << gaitCommand << "\" not found.\n"; printGaitList(gaitList_); diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitReceiver.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitReceiver.cpp index 82c452459..891943522 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitReceiver.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitReceiver.cpp @@ -37,10 +37,10 @@ namespace legged_robot { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -GaitReceiver::GaitReceiver(ros::NodeHandle nodeHandle, std::shared_ptr gaitSchedulePtr, const std::string& robotName) +GaitReceiver::GaitReceiver(rclcpp::Node::SharedPtr &nodeHandle, std::shared_ptr gaitSchedulePtr, const std::string& robotName) : gaitSchedulePtr_(std::move(gaitSchedulePtr)), receivedGait_({0.0, 1.0}, {ModeNumber::STANCE}), gaitUpdated_(false) { - mpcModeSequenceSubscriber_ = nodeHandle.subscribe(robotName + "_mpc_mode_schedule", 1, &GaitReceiver::mpcModeSequenceCallback, this, - ::ros::TransportHints().udp()); + mpcModeSequenceSubscriber_ = nodeHandle->create_subscription(robotName + "_mpc_mode_schedule", + 1, std::bind(&GaitReceiver::mpcModeSequenceCallback, this, std::placeholders::_1)); } /******************************************************************************************************/ @@ -61,7 +61,7 @@ void GaitReceiver::preSolverRun(scalar_t initTime, scalar_t finalTime, const vec /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void GaitReceiver::mpcModeSequenceCallback(const ocs2_msgs::mode_schedule::ConstPtr& msg) { +void GaitReceiver::mpcModeSequenceCallback(const ocs2_msgs::msg::ModeSchedule::SharedPtr msg) { std::lock_guard lock(receivedGaitMutex_); receivedGait_ = readModeSequenceTemplateMsg(*msg); gaitUpdated_ = true; diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/visualization/LeggedRobotVisualizer.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/visualization/LeggedRobotVisualizer.cpp index 60c6d0c05..67447c31c 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/visualization/LeggedRobotVisualizer.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/visualization/LeggedRobotVisualizer.cpp @@ -44,8 +44,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_legged_robot/gait/MotionPhaseDefinition.h" // Additional messages not in the helpers file -#include -#include +// #include // URDF related #include @@ -58,41 +57,37 @@ namespace legged_robot { /******************************************************************************************************/ /******************************************************************************************************/ LeggedRobotVisualizer::LeggedRobotVisualizer(PinocchioInterface pinocchioInterface, CentroidalModelInfo centroidalModelInfo, - const PinocchioEndEffectorKinematics& endEffectorKinematics, ros::NodeHandle& nodeHandle, - scalar_t maxUpdateFrequency) + const PinocchioEndEffectorKinematics& endEffectorKinematics, rclcpp::Node::SharedPtr& nodeHandle, + const std::string &urdf_xml, scalar_t maxUpdateFrequency) : pinocchioInterface_(std::move(pinocchioInterface)), centroidalModelInfo_(std::move(centroidalModelInfo)), endEffectorKinematicsPtr_(endEffectorKinematics.clone()), lastTime_(std::numeric_limits::lowest()), - minPublishTimeDifference_(1.0 / maxUpdateFrequency) { + minPublishTimeDifference_(1.0 / maxUpdateFrequency), + robot_state_publisher::RobotStatePublisher(nodeHandle->get_node_options()), + nodeHandle_(nodeHandle) { endEffectorKinematicsPtr_->setPinocchioInterface(pinocchioInterface_); - launchVisualizerNode(nodeHandle); + launchVisualizerNode(nodeHandle, urdf_xml); }; /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void LeggedRobotVisualizer::launchVisualizerNode(ros::NodeHandle& nodeHandle) { - costDesiredBasePositionPublisher_ = nodeHandle.advertise("/legged_robot/desiredBaseTrajectory", 1); +void LeggedRobotVisualizer::launchVisualizerNode(rclcpp::Node::SharedPtr& nodeHandle, const std::string &urdf_xml) { + tfBroadcaster_ = std::make_unique(nodeHandle); + + costDesiredBasePositionPublisher_ = nodeHandle->create_publisher("/legged_robot/desiredBaseTrajectory", 1); costDesiredFeetPositionPublishers_.resize(centroidalModelInfo_.numThreeDofContacts); - costDesiredFeetPositionPublishers_[0] = nodeHandle.advertise("/legged_robot/desiredFeetTrajectory/LF", 1); - costDesiredFeetPositionPublishers_[1] = nodeHandle.advertise("/legged_robot/desiredFeetTrajectory/RF", 1); - costDesiredFeetPositionPublishers_[2] = nodeHandle.advertise("/legged_robot/desiredFeetTrajectory/LH", 1); - costDesiredFeetPositionPublishers_[3] = nodeHandle.advertise("/legged_robot/desiredFeetTrajectory/RH", 1); - stateOptimizedPublisher_ = nodeHandle.advertise("/legged_robot/optimizedStateTrajectory", 1); - currentStatePublisher_ = nodeHandle.advertise("/legged_robot/currentState", 1); + costDesiredFeetPositionPublishers_[0] = nodeHandle->create_publisher("/legged_robot/desiredFeetTrajectory/LF", 1); + costDesiredFeetPositionPublishers_[1] = nodeHandle->create_publisher("/legged_robot/desiredFeetTrajectory/RF", 1); + costDesiredFeetPositionPublishers_[2] = nodeHandle->create_publisher("/legged_robot/desiredFeetTrajectory/LH", 1); + costDesiredFeetPositionPublishers_[3] = nodeHandle->create_publisher("/legged_robot/desiredFeetTrajectory/RH", 1); + stateOptimizedPublisher_ = nodeHandle->create_publisher("/legged_robot/optimizedStateTrajectory", 1); + currentStatePublisher_ = nodeHandle->create_publisher("/legged_robot/currentState", 1); // Load URDF model - urdf::Model urdfModel; - if (!urdfModel.initParam("legged_robot_description")) { - std::cerr << "[LeggedRobotVisualizer] Could not read URDF from: \"legged_robot_description\"" << std::endl; - } else { - KDL::Tree kdlTree; - kdl_parser::treeFromUrdfModel(urdfModel, kdlTree); - - robotStatePublisherPtr_.reset(new robot_state_publisher::RobotStatePublisher(kdlTree)); - robotStatePublisherPtr_->publishFixedTransforms(true); - } + this->setupURDF(urdf_xml); + this->publishFixedTransforms(); } /******************************************************************************************************/ @@ -105,7 +100,7 @@ void LeggedRobotVisualizer::update(const SystemObservation& observation, const P pinocchio::forwardKinematics(model, data, centroidal_model::getGeneralizedCoordinates(observation.state, centroidalModelInfo_)); pinocchio::updateFramePlacements(model, data); - const auto timeStamp = ros::Time::now(); + const auto timeStamp = nodeHandle_->now(); publishObservation(timeStamp, observation); publishDesiredTrajectory(timeStamp, command.mpcTargetTrajectories_); publishOptimizedStateTrajectory(timeStamp, primalSolution.timeTrajectory_, primalSolution.stateTrajectory_, @@ -117,7 +112,7 @@ void LeggedRobotVisualizer::update(const SystemObservation& observation, const P /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void LeggedRobotVisualizer::publishObservation(ros::Time timeStamp, const SystemObservation& observation) { +void LeggedRobotVisualizer::publishObservation(rclcpp::Time timeStamp, const SystemObservation& observation) { // Extract components from state const auto basePose = centroidal_model::getBasePose(observation.state, centroidalModelInfo_); const auto qJoints = centroidal_model::getJointAngles(observation.state, centroidalModelInfo_); @@ -138,30 +133,26 @@ void LeggedRobotVisualizer::publishObservation(ros::Time timeStamp, const System /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void LeggedRobotVisualizer::publishJointTransforms(ros::Time timeStamp, const vector_t& jointAngles) const { - if (robotStatePublisherPtr_ != nullptr) { - std::map jointPositions{{"LF_HAA", jointAngles[0]}, {"LF_HFE", jointAngles[1]}, {"LF_KFE", jointAngles[2]}, - {"LH_HAA", jointAngles[3]}, {"LH_HFE", jointAngles[4]}, {"LH_KFE", jointAngles[5]}, - {"RF_HAA", jointAngles[6]}, {"RF_HFE", jointAngles[7]}, {"RF_KFE", jointAngles[8]}, - {"RH_HAA", jointAngles[9]}, {"RH_HFE", jointAngles[10]}, {"RH_KFE", jointAngles[11]}}; - robotStatePublisherPtr_->publishTransforms(jointPositions, timeStamp); - } +void LeggedRobotVisualizer::publishJointTransforms(rclcpp::Time timeStamp, const vector_t& jointAngles) { + std::map jointPositions{{"LF_HAA", jointAngles[0]}, {"LF_HFE", jointAngles[1]}, {"LF_KFE", jointAngles[2]}, + {"LH_HAA", jointAngles[3]}, {"LH_HFE", jointAngles[4]}, {"LH_KFE", jointAngles[5]}, + {"RF_HAA", jointAngles[6]}, {"RF_HFE", jointAngles[7]}, {"RF_KFE", jointAngles[8]}, + {"RH_HAA", jointAngles[9]}, {"RH_HFE", jointAngles[10]}, {"RH_KFE", jointAngles[11]}}; + this->publishTransforms(jointPositions, timeStamp); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void LeggedRobotVisualizer::publishBaseTransform(ros::Time timeStamp, const vector_t& basePose) { - if (robotStatePublisherPtr_ != nullptr) { - geometry_msgs::TransformStamped baseToWorldTransform; - baseToWorldTransform.header = getHeaderMsg(frameId_, timeStamp); - baseToWorldTransform.child_frame_id = "base"; - - const Eigen::Quaternion q_world_base = getQuaternionFromEulerAnglesZyx(vector3_t(basePose.tail<3>())); - baseToWorldTransform.transform.rotation = getOrientationMsg(q_world_base); - baseToWorldTransform.transform.translation = getVectorMsg(basePose.head<3>()); - tfBroadcaster_.sendTransform(baseToWorldTransform); - } +void LeggedRobotVisualizer::publishBaseTransform(rclcpp::Time timeStamp, const vector_t& basePose) { + geometry_msgs::msg::TransformStamped baseToWorldTransform; + baseToWorldTransform.header = getHeaderMsg(frameId_, timeStamp); + baseToWorldTransform.child_frame_id = "base"; + + const Eigen::Quaternion q_world_base = getQuaternionFromEulerAnglesZyx(vector3_t(basePose.tail<3>())); + baseToWorldTransform.transform.rotation = getOrientationMsg(q_world_base); + baseToWorldTransform.transform.translation = getVectorMsg(basePose.head<3>()); + tfBroadcaster_->sendTransform(baseToWorldTransform); } /******************************************************************************************************/ @@ -170,9 +161,10 @@ void LeggedRobotVisualizer::publishBaseTransform(ros::Time timeStamp, const vect void LeggedRobotVisualizer::publishTrajectory(const std::vector& system_observation_array, scalar_t speed) { for (size_t k = 0; k < system_observation_array.size() - 1; k++) { scalar_t frameDuration = speed * (system_observation_array[k + 1].time - system_observation_array[k].time); - scalar_t publishDuration = timedExecutionInSeconds([&]() { publishObservation(ros::Time::now(), system_observation_array[k]); }); + scalar_t publishDuration = timedExecutionInSeconds([&]() { publishObservation(nodeHandle_->now(), system_observation_array[k]); }); if (frameDuration > publishDuration) { - ros::WallDuration(frameDuration - publishDuration).sleep(); + rclcpp::Duration dur(frameDuration - publishDuration); + rclcpp::sleep_for(std::chrono::nanoseconds(dur.nanoseconds())); } } } @@ -180,12 +172,12 @@ void LeggedRobotVisualizer::publishTrajectory(const std::vector& feetPositions, const std::vector& feetForces) const { // Reserve message const size_t numberOfCartesianMarkers = 10; - visualization_msgs::MarkerArray markerArray; + visualization_msgs::msg::MarkerArray markerArray; markerArray.markers.reserve(numberOfCartesianMarkers); // Feet positions and Forces @@ -208,22 +200,22 @@ void LeggedRobotVisualizer::publishCartesianMarkers(ros::Time timeStamp, const c assignIncreasingId(markerArray.markers.begin(), markerArray.markers.end()); // Publish cartesian markers (minus the CoM Pose) - currentStatePublisher_.publish(markerArray); + currentStatePublisher_->publish(markerArray); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void LeggedRobotVisualizer::publishDesiredTrajectory(ros::Time timeStamp, const TargetTrajectories& targetTrajectories) { +void LeggedRobotVisualizer::publishDesiredTrajectory(rclcpp::Time timeStamp, const TargetTrajectories& targetTrajectories) { const auto& stateTrajectory = targetTrajectories.stateTrajectory; const auto& inputTrajectory = targetTrajectories.inputTrajectory; // Reserve com messages - std::vector desiredBasePositionMsg; + std::vector desiredBasePositionMsg; desiredBasePositionMsg.reserve(stateTrajectory.size()); // Reserve feet messages - feet_array_t> desiredFeetPositionMsgs; + feet_array_t> desiredFeetPositionMsgs; for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) { desiredFeetPositionMsgs[i].reserve(stateTrajectory.size()); } @@ -239,7 +231,7 @@ void LeggedRobotVisualizer::publishDesiredTrajectory(ros::Time timeStamp, const // Construct base pose msg const auto basePose = centroidal_model::getBasePose(state, centroidalModelInfo_); - geometry_msgs::Pose pose; + geometry_msgs::msg::Pose pose; pose.position = getPointMsg(basePose.head<3>()); // Fill message containers @@ -253,7 +245,7 @@ void LeggedRobotVisualizer::publishDesiredTrajectory(ros::Time timeStamp, const const auto feetPositions = endEffectorKinematicsPtr_->getPosition(state); for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) { - geometry_msgs::Pose footPose; + geometry_msgs::msg::Pose footPose; footPose.position = getPointMsg(feetPositions[i]); desiredFeetPositionMsgs[i].push_back(footPose.position); } @@ -265,30 +257,30 @@ void LeggedRobotVisualizer::publishDesiredTrajectory(ros::Time timeStamp, const comLineMsg.id = 0; // Publish - costDesiredBasePositionPublisher_.publish(comLineMsg); + costDesiredBasePositionPublisher_->publish(comLineMsg); for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) { auto footLineMsg = getLineMsg(std::move(desiredFeetPositionMsgs[i]), feetColorMap_[i], trajectoryLineWidth_); footLineMsg.header = getHeaderMsg(frameId_, timeStamp); footLineMsg.id = 0; - costDesiredFeetPositionPublishers_[i].publish(footLineMsg); + costDesiredFeetPositionPublishers_[i]->publish(footLineMsg); } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void LeggedRobotVisualizer::publishOptimizedStateTrajectory(ros::Time timeStamp, const scalar_array_t& mpcTimeTrajectory, +void LeggedRobotVisualizer::publishOptimizedStateTrajectory(rclcpp::Time timeStamp, const scalar_array_t& mpcTimeTrajectory, const vector_array_t& mpcStateTrajectory, const ModeSchedule& modeSchedule) { if (mpcTimeTrajectory.empty() || mpcStateTrajectory.empty()) { return; // Nothing to publish } // Reserve Feet msg - feet_array_t> feetMsgs; - std::for_each(feetMsgs.begin(), feetMsgs.end(), [&](std::vector& v) { v.reserve(mpcStateTrajectory.size()); }); + feet_array_t> feetMsgs; + std::for_each(feetMsgs.begin(), feetMsgs.end(), [&](std::vector& v) { v.reserve(mpcStateTrajectory.size()); }); // Reserve Com Msg - std::vector mpcComPositionMsgs; + std::vector mpcComPositionMsgs; mpcComPositionMsgs.reserve(mpcStateTrajectory.size()); // Extract Com and Feet from state @@ -296,7 +288,7 @@ void LeggedRobotVisualizer::publishOptimizedStateTrajectory(ros::Time timeStamp, const auto basePose = centroidal_model::getBasePose(state, centroidalModelInfo_); // Fill com position and pose msgs - geometry_msgs::Pose pose; + geometry_msgs::msg::Pose pose; pose.position = getPointMsg(basePose.head<3>()); mpcComPositionMsgs.push_back(pose.position); @@ -314,7 +306,7 @@ void LeggedRobotVisualizer::publishOptimizedStateTrajectory(ros::Time timeStamp, }); // Convert feet msgs to Array message - visualization_msgs::MarkerArray markerArray; + visualization_msgs::msg::MarkerArray markerArray; markerArray.markers.reserve(centroidalModelInfo_.numThreeDofContacts + 2); // 1 trajectory per foot + 1 for the future footholds + 1 for the com trajectory for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) { @@ -325,8 +317,8 @@ void LeggedRobotVisualizer::publishOptimizedStateTrajectory(ros::Time timeStamp, markerArray.markers.back().ns = "CoM Trajectory"; // Future footholds - visualization_msgs::Marker sphereList; - sphereList.type = visualization_msgs::Marker::SPHERE_LIST; + visualization_msgs::msg::Marker sphereList; + sphereList.type = visualization_msgs::msg::Marker::SPHERE_LIST; sphereList.scale.x = footMarkerDiameter_; sphereList.scale.y = footMarkerDiameter_; sphereList.scale.z = footMarkerDiameter_; @@ -362,7 +354,7 @@ void LeggedRobotVisualizer::publishOptimizedStateTrajectory(ros::Time timeStamp, assignHeader(markerArray.markers.begin(), markerArray.markers.end(), getHeaderMsg(frameId_, timeStamp)); assignIncreasingId(markerArray.markers.begin(), markerArray.markers.end()); - stateOptimizedPublisher_.publish(markerArray); + stateOptimizedPublisher_->publish(markerArray); } } // namespace legged_robot diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/CMakeLists.txt b/ocs2_robotic_examples/ocs2_mobile_manipulator/CMakeLists.txt index 9d010712d..96aacdce9 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_mobile_manipulator) # Generate compile_commands.json for clang tools @@ -118,12 +118,12 @@ endif (cmake_clang_tools_FOUND) ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_mobile_manipulator/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/package.xml b/ocs2_robotic_examples/ocs2_mobile_manipulator/package.xml index 0d1ff99af..45a294f09 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/package.xml +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/package.xml @@ -10,7 +10,7 @@ TODO - catkin + ament_cmake cmake_clang_tools ocs2_core diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/CMakeLists.txt index 6831c0606..b65bd307b 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_mobile_manipulator_ros) # Generate compile_commands.json for clang tools @@ -146,8 +146,8 @@ endif (cmake_clang_tools_FOUND) ## Install ## ############# -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) install( TARGETS @@ -155,7 +155,7 @@ install( mobile_manipulator_distance_visualization mobile_manipulator_dummy_mrt_node mobile_manipulator_target - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + RUNTIME DESTINATION bin ) install(DIRECTORY launch rviz DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/package.xml b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/package.xml index 3a68339d1..2a8142b58 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/package.xml @@ -11,7 +11,7 @@ BSD3 - catkin + ament_cmake cmake_clang_tools roslib diff --git a/ocs2_robotic_examples/ocs2_quadrotor/CMakeLists.txt b/ocs2_robotic_examples/ocs2_quadrotor/CMakeLists.txt index dece8bae4..21e78f307 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_quadrotor/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_quadrotor) # Generate compile_commands.json for clang tools @@ -111,12 +111,12 @@ endif(cmake_clang_tools_FOUND) ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} diff --git a/ocs2_robotic_examples/ocs2_quadrotor/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_quadrotor/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_quadrotor/package.xml b/ocs2_robotic_examples/ocs2_quadrotor/package.xml index ac89ffc34..e0e015bfc 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor/package.xml +++ b/ocs2_robotic_examples/ocs2_quadrotor/package.xml @@ -10,7 +10,7 @@ TODO - catkin + ament_cmake roslib pybind11_catkin diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_quadrotor_ros/CMakeLists.txt index d85fb3d68..b77c0642b 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_quadrotor_ros) # Generate compile_commands.json for clang tools @@ -107,15 +107,15 @@ endif(cmake_clang_tools_FOUND) ## Install ## ############# -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) install( TARGETS quadrotor_mpc quadrotor_dummy_test quadrotor_target - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + RUNTIME DESTINATION bin ) install(DIRECTORY launch rviz DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_quadrotor_ros/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/package.xml b/ocs2_robotic_examples/ocs2_quadrotor_ros/package.xml index 498084aa3..a2d38e1fd 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/package.xml @@ -10,7 +10,7 @@ TODO - catkin + ament_cmake roslib tf diff --git a/ocs2_robotic_examples/ocs2_robotic_examples/CMakeLists.txt b/ocs2_robotic_examples/ocs2_robotic_examples/CMakeLists.txt index b0bbf2530..5a3c4b5cc 100644 --- a/ocs2_robotic_examples/ocs2_robotic_examples/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_robotic_examples/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_robotic_examples) -find_package(catkin REQUIRED) +find_package(ament_cmake REQUIRED) catkin_metapackage() diff --git a/ocs2_robotic_examples/ocs2_robotic_examples/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_robotic_examples/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_robotic_examples/package.xml b/ocs2_robotic_examples/ocs2_robotic_examples/package.xml index 8aa431308..beb285b06 100644 --- a/ocs2_robotic_examples/ocs2_robotic_examples/package.xml +++ b/ocs2_robotic_examples/ocs2_robotic_examples/package.xml @@ -10,7 +10,7 @@ TODO - catkin + ament_cmake ocs2_ballbot ocs2_ballbot_ros diff --git a/ocs2_robotic_tools/CMakeLists.txt b/ocs2_robotic_tools/CMakeLists.txt index 6c560fda9..835c97bcf 100644 --- a/ocs2_robotic_tools/CMakeLists.txt +++ b/ocs2_robotic_tools/CMakeLists.txt @@ -1,30 +1,20 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_robotic_tools) # Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -find_package(catkin REQUIRED COMPONENTS - ocs2_core - ocs2_oc -) +find_package(ament_cmake REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(OpenMP REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ocs2_core - ocs2_oc - DEPENDS +find_package(Boost REQUIRED COMPONENTS + system + filesystem + log + log_setup ) @@ -32,25 +22,38 @@ catkin_package( ## Build ## ########### -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - add_library(${PROJECT_NAME} src/common/RotationTransforms.cpp src/common/LoopshapingRobotInterface.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) + +ament_target_dependencies(${PROJECT_NAME} + ocs2_oc + ocs2_core + ocs2_thirdparty ) + target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) add_executable(${PROJECT_NAME}_lintTarget src/lintTarget.cpp ) +target_include_directories(${PROJECT_NAME}_lintTarget PUBLIC + $ + $ +) +ament_target_dependencies(${PROJECT_NAME}_lintTarget + ocs2_oc + ocs2_core + ocs2_thirdparty +) + ######################### ### CLANG TOOLING ### @@ -70,35 +73,43 @@ endif(cmake_clang_tools_FOUND) ## Install ## ############# -install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY - include/${PROJECT_NAME}/ - DESTINATION - ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install(DIRECTORY include/ + DESTINATION include +) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_export_dependencies(ocs2_thirdparty) +ament_export_dependencies(ocs2_oc) +ament_export_dependencies(ocs2_core) ############# ## Testing ## ############# -## Info ============================== -## to run tests, cd package folder and run -## $ catkin build -DCMAKE_BUILD_TYPE=RelWithDebInfo --this -## $ catkin run_tests --no-deps --this -## to see the summary of unit test results run -## $ catkin_test_results ../../../build/ocs2_robotic_tools - -catkin_add_gtest(rotation_transform_tests +find_package(ament_lint_auto REQUIRED) +find_package(ament_cmake_gtest REQUIRED) +ament_lint_auto_find_test_dependencies() + +ament_add_gtest(rotation_transform_tests test/common/TestRotationTransforms.cpp test/common/TestRotationDerivativesTransforms.cpp ) target_link_libraries(rotation_transform_tests ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) target_compile_options(rotation_transform_tests PRIVATE ${OCS2_CXX_FLAGS}) + +ament_package() + diff --git a/ocs2_robotic_tools/package.xml b/ocs2_robotic_tools/package.xml index b73cabd70..e7b38ef17 100644 --- a/ocs2_robotic_tools/package.xml +++ b/ocs2_robotic_tools/package.xml @@ -10,7 +10,7 @@ TODO - catkin + ament_cmake cmake_clang_tools ocs2_core @@ -18,4 +18,7 @@ ocs2_core ocs2_oc + + ament_cmake + diff --git a/ocs2_ros_interfaces/CMakeLists.txt b/ocs2_ros_interfaces/CMakeLists.txt index 1537aefa3..af26e3339 100644 --- a/ocs2_ros_interfaces/CMakeLists.txt +++ b/ocs2_ros_interfaces/CMakeLists.txt @@ -1,56 +1,43 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_ros_interfaces) -set(CATKIN_PACKAGE_DEPENDENCIES - roscpp - ocs2_msgs - ocs2_core - ocs2_ddp - ocs2_mpc - std_msgs - visualization_msgs - geometry_msgs - interactive_markers -) - -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ${CATKIN_PACKAGE_DEPENDENCIES} -) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(ocs2_msgs REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(std_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(interactive_markers REQUIRED) +find_package(OpenMP REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) -find_package(Boost REQUIRED COMPONENTS filesystem) - -# Generate compile_commands.json for clang tools -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost +find_package(Boost REQUIRED COMPONENTS + system + filesystem + log + log_setup ) ########### ## Build ## ########### -## Specify additional locations of header files -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} +set(DEPS + rclcpp + ocs2_msgs + ocs2_core + ocs2_ddp + ocs2_oc + ocs2_mpc + std_msgs + visualization_msgs + geometry_msgs + interactive_markers ) add_library(${PROJECT_NAME} @@ -67,43 +54,52 @@ add_library(${PROJECT_NAME} src/visualization/VisualizationHelpers.cpp src/visualization/VisualizationColors.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} -) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -add_executable(test_custom_callback_queue - test/test_custom_callback_queue.cpp +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ ) -add_dependencies(test_custom_callback_queue - ${catkin_EXPORTED_TARGETS} + +target_link_libraries(${PROJECT_NAME} + ${Boost_LIBRARIES} + Eigen3::Eigen ) -target_link_libraries(test_custom_callback_queue - ${catkin_LIBRARIES} + +ament_target_dependencies(${PROJECT_NAME} + ${DEPS} ) -target_compile_options(test_custom_callback_queue PRIVATE ${OCS2_CXX_FLAGS}) # multiplot remap node add_executable(multiplot_remap src/multiplot/MultiplotRemap.cpp ) -add_dependencies(multiplot_remap - ${catkin_EXPORTED_TARGETS} + +target_include_directories(multiplot_remap PUBLIC + $ + $ ) + +ament_target_dependencies(multiplot_remap + ${DEPS} +) + target_link_libraries(multiplot_remap ${PROJECT_NAME} - ${catkin_LIBRARIES} ) target_compile_options(multiplot_remap PRIVATE ${OCS2_CXX_FLAGS}) add_executable(${PROJECT_NAME}_lintTarget src/lintTarget.cpp ) -add_dependencies(${PROJECT_NAME}_lintTarget - ${catkin_EXPORTED_TARGETS} + +target_include_directories(${PROJECT_NAME}_lintTarget PUBLIC + $ + $ ) -target_link_libraries(${PROJECT_NAME}_lintTarget - ${catkin_LIBRARIES} + +ament_target_dependencies(${PROJECT_NAME}_lintTarget + ${DEPS} ) ######################### @@ -125,27 +121,35 @@ endif(cmake_clang_tools_FOUND) ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) -install(TARGETS multiplot_remap - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(DIRECTORY multiplot DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY launch multiplot DESTINATION share/${PROJECT_NAME}) + +install(DIRECTORY include/ + DESTINATION include ) -install(DIRECTORY launch multiplot - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + +ament_export_include_directories(include) + +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets( + export_${PROJECT_NAME} ) -############# -## Testing ## -############# -## Info ============================== -## to run tests, cd package folder and run -## $ catkin build -DCMAKE_BUILD_TYPE=RelWithDebInfo --this -## $ catkin run_tests --no-deps --this -## to see the summary of unit test results run -## $ catkin_test_results ../../../build/ocs2_ros_interfaces +ament_export_dependencies(rclcpp) +ament_export_dependencies(ocs2_msgs) +ament_export_dependencies(ocs2_core) +ament_export_dependencies(ocs2_ddp) +ament_export_dependencies(ocs2_oc) +ament_export_dependencies(ocs2_mpc) +ament_export_dependencies(std_msgs) +ament_export_dependencies(visualization_msgs) +ament_export_dependencies(geometry_msgs) +ament_export_dependencies(interactive_markers) + +ament_package() diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesInteractiveMarker.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesInteractiveMarker.h index db286a144..03b8db42a 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesInteractiveMarker.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesInteractiveMarker.h @@ -33,11 +33,12 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include -#include +#include +#include #include #include +#include namespace ocs2 { @@ -57,17 +58,17 @@ class TargetTrajectoriesInteractiveMarker final { * observation is be expected on "topicPrefix_mpc_observation" topic. * @param [in] gaolPoseToTargetTrajectories: A function which transforms the commanded pose to TargetTrajectories. */ - TargetTrajectoriesInteractiveMarker(::ros::NodeHandle& nodeHandle, const std::string& topicPrefix, + TargetTrajectoriesInteractiveMarker(::rclcpp::Node::SharedPtr& node, const std::string& topicPrefix, GaolPoseToTargetTrajectories gaolPoseToTargetTrajectories); /** * Spins ROS to update the interactive markers. */ - void publishInteractiveMarker() { ::ros::spin(); } + void publishInteractiveMarker() { ::rclcpp::spin(node_); } private: - visualization_msgs::InteractiveMarker createInteractiveMarker() const; - void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); + visualization_msgs::msg::InteractiveMarker createInteractiveMarker() const; + void processFeedback(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr feedback); interactive_markers::MenuHandler menuHandler_; interactive_markers::InteractiveMarkerServer server_; @@ -76,9 +77,10 @@ class TargetTrajectoriesInteractiveMarker final { std::unique_ptr targetTrajectoriesPublisherPtr_; - ::ros::Subscriber observationSubscriber_; + ::rclcpp::Subscription::SharedPtr observationSubscriber_; mutable std::mutex latestObservationMutex_; SystemObservation latestObservation_; + ::rclcpp::Node::SharedPtr node_; }; } // namespace ocs2 diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesKeyboardPublisher.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesKeyboardPublisher.h index 731975c06..496a7b29b 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesKeyboardPublisher.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesKeyboardPublisher.h @@ -33,7 +33,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include +#include #include #include @@ -57,7 +57,7 @@ class TargetTrajectoriesKeyboardPublisher final { * @param [in] targetCommandLimits: The limits of the loaded command from command-line (for safety purposes). * @param [in] commandLineToTargetTrajectoriesFun: A function which transforms the command line input to TargetTrajectories. */ - TargetTrajectoriesKeyboardPublisher(::ros::NodeHandle& nodeHandle, const std::string& topicPrefix, + TargetTrajectoriesKeyboardPublisher(::rclcpp::Node::SharedPtr& nodeHandle, const std::string& topicPrefix, const scalar_array_t& targetCommandLimits, CommandLineToTargetTrajectories commandLineToTargetTrajectoriesFun); @@ -81,9 +81,10 @@ class TargetTrajectoriesKeyboardPublisher final { std::unique_ptr targetTrajectoriesPublisherPtr_; - ::ros::Subscriber observationSubscriber_; + ::rclcpp::Subscription::SharedPtr observationSubscriber_; mutable std::mutex latestObservationMutex_; SystemObservation latestObservation_; + ::rclcpp::Node::SharedPtr node_; }; } // namespace ocs2 diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesRosPublisher.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesRosPublisher.h index ffa79f70b..150cc58fd 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesRosPublisher.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesRosPublisher.h @@ -32,7 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include +#include #include #include @@ -51,7 +51,7 @@ class TargetTrajectoriesRosPublisher final { * @param [in] nodeHandle: ROS node handle. * @param [in] topicPrefix: The TargetTrajectories will be published on "topicPrefix_mpc_target" topic. */ - TargetTrajectoriesRosPublisher(::ros::NodeHandle& nodeHandle, const std::string& topicPrefix = "anonymousRobot"); + TargetTrajectoriesRosPublisher(::rclcpp::Node::SharedPtr& nodeHandle, const std::string& topicPrefix = "anonymousRobot"); /** Destructor. */ ~TargetTrajectoriesRosPublisher(); @@ -60,7 +60,7 @@ class TargetTrajectoriesRosPublisher final { void publishTargetTrajectories(const TargetTrajectories& targetTrajectories); private: - ::ros::Publisher targetTrajectoriesPublisher_; + ::rclcpp::Publisher::SharedPtr targetTrajectoriesPublisher_; }; } // namespace ocs2 diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h index 7de6e7bce..d86e0bb88 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h @@ -36,31 +36,31 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include // MPC messages -#include -#include -#include -#include +#include +#include +#include +#include namespace ocs2 { namespace ros_msg_conversions { /** Creates the observation message. */ -ocs2_msgs::mpc_observation createObservationMsg(const SystemObservation& observation); +ocs2_msgs::msg::MPCObservation createObservationMsg(const SystemObservation& observation); /** Reads the observation message. */ -SystemObservation readObservationMsg(const ocs2_msgs::mpc_observation& observationMsg); +SystemObservation readObservationMsg(const ocs2_msgs::msg::MPCObservation& observationMsg); /** Creates the mode sequence message. */ -ocs2_msgs::mode_schedule createModeScheduleMsg(const ModeSchedule& modeSchedule); +ocs2_msgs::msg::ModeSchedule createModeScheduleMsg(const ModeSchedule& modeSchedule); /** Reads the mode sequence message. */ -ModeSchedule readModeScheduleMsg(const ocs2_msgs::mode_schedule& modeScheduleMsg); +ModeSchedule readModeScheduleMsg(const ocs2_msgs::msg::ModeSchedule& modeScheduleMsg); /** Creates the target trajectories message. */ -ocs2_msgs::mpc_target_trajectories createTargetTrajectoriesMsg(const TargetTrajectories& targetTrajectories); +ocs2_msgs::msg::MPCTargetTrajectories createTargetTrajectoriesMsg(const TargetTrajectories& targetTrajectories); /** Returns the TargetTrajectories message. */ -TargetTrajectories readTargetTrajectoriesMsg(const ocs2_msgs::mpc_target_trajectories& targetTrajectoriesMsg); +TargetTrajectories readTargetTrajectoriesMsg(const ocs2_msgs::msg::MPCTargetTrajectories& targetTrajectoriesMsg); /** * Creates the performance indices message. @@ -69,10 +69,10 @@ TargetTrajectories readTargetTrajectoriesMsg(const ocs2_msgs::mpc_target_traject * @param [in] performanceIndices: The performance indices of the solver. * @return The performance indices ROS message. */ -ocs2_msgs::mpc_performance_indices createPerformanceIndicesMsg(scalar_t initTime, const PerformanceIndex& performanceIndices); +ocs2_msgs::msg::MPCPerformanceIndices createPerformanceIndicesMsg(scalar_t initTime, const PerformanceIndex& performanceIndices); /** Reads the performance indices message. */ -PerformanceIndex readPerformanceIndicesMsg(const ocs2_msgs::mpc_performance_indices& performanceIndicesMsg); +PerformanceIndex readPerformanceIndicesMsg(const ocs2_msgs::msg::MPCPerformanceIndices& performanceIndicesMsg); } // namespace ros_msg_conversions } // namespace ocs2 diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgHelpers.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgHelpers.h index 8bdd89e2a..0f9ed2b5b 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgHelpers.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgHelpers.h @@ -34,25 +34,26 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include -#include -#include -#include +#include +#include +#include +#include +#include namespace ocs2 { namespace ros_msg_helpers { -geometry_msgs::Vector3 getVectorMsg(const Eigen::Vector3d& vec); +geometry_msgs::msg::Vector3 getVectorMsg(const Eigen::Vector3d& vec); -geometry_msgs::Point getPointMsg(const Eigen::Vector3d& point); +geometry_msgs::msg::Point getPointMsg(const Eigen::Vector3d& point); -geometry_msgs::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientation); +geometry_msgs::msg::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientation); -std_msgs::Header getHeaderMsg(const std::string& frame_id, const ros::Time& timeStamp); +std_msgs::msg::Header getHeaderMsg(const std::string& frame_id, const rclcpp::Time& timeStamp); -visualization_msgs::Marker getLineMsg(std::vector&& points, std::array color, double lineWidth); +visualization_msgs::msg::Marker getLineMsg(std::vector&& points, std::array color, double lineWidth); -std_msgs::ColorRGBA getColor(std::array rgb, double alpha = 1.0); +std_msgs::msg::ColorRGBA getColor(std::array rgb, double alpha = 1.0); } // namespace ros_msg_helpers } // namespace ocs2 diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h index fe0611e6e..316b20aa9 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h @@ -38,15 +38,13 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include -#include -#include +#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -56,7 +54,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#define PUBLISH_THREAD +// #define PUBLISH_THREAD namespace ocs2 { @@ -90,17 +88,12 @@ class MPC_ROS_Interface { */ void shutdownNode(); - /** - * Spins ROS. - */ - void spin(); - /** * This is the main routine which launches all the nodes required for MPC to run which includes: * (1) The MPC policy publisher (either feedback or feedforward policy). * (2) The observation subscriber which gets the current measured state to invoke the MPC run routine. */ - void launchNodes(ros::NodeHandle& nodeHandle); + void launchNodes(rclcpp::Node::SharedPtr& nodeHandle); protected: /** @@ -109,7 +102,8 @@ class MPC_ROS_Interface { * @param req: Service request. * @param res: Service response. */ - bool resetMpcCallback(ocs2_msgs::reset::Request& req, ocs2_msgs::reset::Response& res); + bool resetMpcCallback(const std::shared_ptr req, + std::shared_ptr res); /** * Creates MPC Policy message. @@ -119,7 +113,7 @@ class MPC_ROS_Interface { * @param [in] performanceIndices: The performance indices data of the solver. * @return MPC policy message. */ - static ocs2_msgs::mpc_flattened_controller createMpcPolicyMsg(const PrimalSolution& primalSolution, const CommandData& commandData, + static ocs2_msgs::msg::MPCFlattenedController createMpcPolicyMsg(const PrimalSolution& primalSolution, const CommandData& commandData, const PerformanceIndex& performanceIndices); /** @@ -140,7 +134,7 @@ class MPC_ROS_Interface { * * @param [in] msg: The observation message. */ - void mpcObservationCallback(const ocs2_msgs::mpc_observation::ConstPtr& msg); + void mpcObservationCallback(const ocs2_msgs::msg::MPCObservation::SharedPtr msg); protected: /* @@ -150,13 +144,13 @@ class MPC_ROS_Interface { std::string topicPrefix_; - std::shared_ptr nodeHandlerPtr_; + rclcpp::Node::SharedPtr node_; // Publishers and subscribers - ::ros::Subscriber mpcObservationSubscriber_; - ::ros::Subscriber mpcTargetTrajectoriesSubscriber_; - ::ros::Publisher mpcPolicyPublisher_; - ::ros::ServiceServer mpcResetServiceServer_; + ::rclcpp::Subscription::SharedPtr mpcObservationSubscriber_; + ::rclcpp::Subscription::SharedPtr mpcTargetTrajectoriesSubscriber_; + ::rclcpp::Publisher::SharedPtr mpcPolicyPublisher_; + ::rclcpp::Service::SharedPtr mpcResetServiceServer_; std::unique_ptr bufferCommandPtr_; std::unique_ptr publisherCommandPtr_; diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h index 797ba6f2f..82ef63201 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h @@ -37,19 +37,17 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include -#include -#include +#include // MPC messages -#include -#include +#include +#include #include #include "ocs2_ros_interfaces/common/RosMsgConversions.h" -#define PUBLISH_THREAD +// #define PUBLISH_THREAD namespace ocs2 { @@ -65,8 +63,7 @@ class MRT_ROS_Interface : public MRT_BASE { * policy's receiving topic "topicPrefix_mpc_policy", and MPC reset service "topicPrefix_mpc_reset". * @param [in] mrtTransportHints: ROS transmission protocol. */ - explicit MRT_ROS_Interface(std::string topicPrefix = "anonymousRobot", - ::ros::TransportHints mrtTransportHints = ::ros::TransportHints().tcpNoDelay()); + explicit MRT_ROS_Interface(std::string topicPrefix = "anonymousRobot"); /** * Destructor @@ -94,7 +91,7 @@ class MRT_ROS_Interface : public MRT_BASE { * Launches the ROS publishers and subscribers to communicate with the MPC node. * @param nodeHandle */ - void launchNodes(::ros::NodeHandle& nodeHandle); + void launchNodes(rclcpp::Node::SharedPtr& node); void setCurrentObservation(const SystemObservation& currentObservation) override; @@ -105,7 +102,7 @@ class MRT_ROS_Interface : public MRT_BASE { * * @param [in] msg: A constant pointer to the message */ - void mpcPolicyCallback(const ocs2_msgs::mpc_flattened_controller::ConstPtr& msg); + void mpcPolicyCallback(const ocs2_msgs::msg::MPCFlattenedController::SharedPtr msg); /** * Helper function to read a MPC policy message. @@ -115,7 +112,7 @@ class MRT_ROS_Interface : public MRT_BASE { * @param [out] primalSolution: The MPC policy data * @param [out] performanceIndices: The MPC performance indices data */ - static void readPolicyMsg(const ocs2_msgs::mpc_flattened_controller& msg, CommandData& commandData, PrimalSolution& primalSolution, + static void readPolicyMsg(const ocs2_msgs::msg::MPCFlattenedController& msg, CommandData& commandData, PrimalSolution& primalSolution, PerformanceIndex& performanceIndices); /** @@ -127,20 +124,21 @@ class MRT_ROS_Interface : public MRT_BASE { std::string topicPrefix_; // Publishers and subscribers - ::ros::Publisher mpcObservationPublisher_; - ::ros::Subscriber mpcPolicySubscriber_; - ::ros::ServiceClient mpcResetServiceClient_; + ::rclcpp::Publisher::SharedPtr mpcObservationPublisher_; + ::rclcpp::Subscription::SharedPtr mpcPolicySubscriber_; + ::rclcpp::Client::SharedPtr mpcResetServiceClient_; // ROS messages - ocs2_msgs::mpc_observation mpcObservationMsg_; - ocs2_msgs::mpc_observation mpcObservationMsgBuffer_; + ocs2_msgs::msg::MPCObservation mpcObservationMsg_; + ocs2_msgs::msg::MPCObservation mpcObservationMsgBuffer_; - ::ros::CallbackQueue mrtCallbackQueue_; - ::ros::TransportHints mrtTransportHints_; + // ::ros::CallbackQueue mrtCallbackQueue_; + // ::ros::TransportHints mrtTransportHints_; // Multi-threading for publishers bool terminateThread_; bool readyToPublish_; + rclcpp::Node::SharedPtr node_; std::thread publisherWorker_; std::mutex publisherMutex_; std::condition_variable msgReady_; diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h index 9d46ea51f..a1b0dbb13 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h @@ -33,9 +33,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include +#include #include +#include +#include namespace ocs2 { @@ -71,13 +73,13 @@ class RosReferenceManager final : public ReferenceManagerDecorator { * (1) ModeSchedule : The predefined mode schedule for time-triggered hybrid systems. * (2) TargetTrajectories : The commanded TargetTrajectories. */ - void subscribe(ros::NodeHandle& nodeHandle); + void subscribe(rclcpp::Node::SharedPtr& nodeHandle); private: const std::string topicPrefix_; - ::ros::Subscriber modeScheduleSubscriber_; - ::ros::Subscriber targetTrajectoriesSubscriber_; + ::rclcpp::Subscription::SharedPtr modeScheduleSubscriber_; + ::rclcpp::Subscription::SharedPtr targetTrajectoriesSubscriber_; }; /******************************************************************************************************/ diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/visualization/VisualizationHelpers.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/visualization/VisualizationHelpers.h index 445d52dac..49fc603f2 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/visualization/VisualizationHelpers.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/visualization/VisualizationHelpers.h @@ -36,25 +36,27 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + // ROS messages -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include namespace ocs2 { -std_msgs::ColorRGBA getColor(Color color, double alpha = 1.0); +std_msgs::msg::ColorRGBA getColor(Color color, double alpha = 1.0); -void setVisible(visualization_msgs::Marker& marker); +void setVisible(visualization_msgs::msg::Marker& marker); -void setInvisible(visualization_msgs::Marker& marker); +void setInvisible(visualization_msgs::msg::Marker& marker); -std_msgs::Header getHeaderMsg(const std::string& frame_id, const ros::Time& timeStamp); +std_msgs::msg::Header getHeaderMsg(const std::string& frame_id, const rclcpp::Time& timeStamp); template -void assignHeader(It firstIt, It lastIt, const std_msgs::Header& header) { +void assignHeader(It firstIt, It lastIt, const std_msgs::msg::Header& header) { for (; firstIt != lastIt; ++firstIt) { firstIt->header = header; } @@ -67,33 +69,33 @@ void assignIncreasingId(It firstIt, It lastIt, int startId = 0) { } } -visualization_msgs::Marker getLineMsg(std::vector&& points, Color color, double lineWidth); +visualization_msgs::msg::Marker getLineMsg(std::vector&& points, Color color, double lineWidth); -geometry_msgs::Point getPointMsg(const Eigen::Vector3d& point); +geometry_msgs::msg::Point getPointMsg(const Eigen::Vector3d& point); -geometry_msgs::Vector3 getVectorMsg(const Eigen::Vector3d& vec); +geometry_msgs::msg::Vector3 getVectorMsg(const Eigen::Vector3d& vec); -geometry_msgs::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientation); +geometry_msgs::msg::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientation); -visualization_msgs::Marker getSphereMsg(const Eigen::Vector3d& point, Color color, double diameter); +visualization_msgs::msg::Marker getSphereMsg(const Eigen::Vector3d& point, Color color, double diameter); -visualization_msgs::Marker getPlaneMsg(const Eigen::Vector3d& point, const Eigen::Quaterniond& orientation, Color color, double width, +visualization_msgs::msg::Marker getPlaneMsg(const Eigen::Vector3d& point, const Eigen::Quaterniond& orientation, Color color, double width, double length, double thickness); -visualization_msgs::Marker getArrowToPointMsg(const Eigen::Vector3d& vec, const Eigen::Vector3d& point, Color color); +visualization_msgs::msg::Marker getArrowToPointMsg(const Eigen::Vector3d& vec, const Eigen::Vector3d& point, Color color); -visualization_msgs::Marker getArrowAtPointMsg(const Eigen::Vector3d& vec, const Eigen::Vector3d& point, Color color); +visualization_msgs::msg::Marker getArrowAtPointMsg(const Eigen::Vector3d& vec, const Eigen::Vector3d& point, Color color); -visualization_msgs::Marker getArrowBetweenPointsMsg(const Eigen::Vector3d& start, const Eigen::Vector3d& end, Color color); +visualization_msgs::msg::Marker getArrowBetweenPointsMsg(const Eigen::Vector3d& start, const Eigen::Vector3d& end, Color color); -visualization_msgs::Marker getFootMarker(const Eigen::Vector3d& position, bool contactFlag, Color color, double diameter, +visualization_msgs::msg::Marker getFootMarker(const Eigen::Vector3d& position, bool contactFlag, Color color, double diameter, double liftedAlpha); -visualization_msgs::Marker getForceMarker(const Eigen::Vector3d& force, const Eigen::Vector3d& position, bool contactFlag, Color color, +visualization_msgs::msg::Marker getForceMarker(const Eigen::Vector3d& force, const Eigen::Vector3d& position, bool contactFlag, Color color, double forceScale); template -visualization_msgs::Marker getCenterOfPressureMarker(ForceIt firstForce, ForceIt lastForce, PositionIt positionIt, ContactIt contactIt, +visualization_msgs::msg::Marker getCenterOfPressureMarker(ForceIt firstForce, ForceIt lastForce, PositionIt positionIt, ContactIt contactIt, Color color, double diameter) { // Compute center of pressure Eigen::Vector3d centerOfPressure = Eigen::Vector3d::Zero(); @@ -109,7 +111,7 @@ visualization_msgs::Marker getCenterOfPressureMarker(ForceIt firstForce, ForceIt } // Construct marker - visualization_msgs::Marker copMarker = getSphereMsg(centerOfPressure, color, diameter); + visualization_msgs::msg::Marker copMarker = getSphereMsg(centerOfPressure, color, diameter); if (numContacts == 0) { setInvisible(copMarker); } @@ -119,10 +121,10 @@ visualization_msgs::Marker getCenterOfPressureMarker(ForceIt firstForce, ForceIt } template -visualization_msgs::Marker getSupportPolygonMarker(PositionIt firstPos, PositionIt lastPos, ContactIt contactIt, Color color, +visualization_msgs::msg::Marker getSupportPolygonMarker(PositionIt firstPos, PositionIt lastPos, ContactIt contactIt, Color color, double lineWidth) { - visualization_msgs::Marker lineList; - lineList.type = visualization_msgs::Marker::LINE_LIST; + visualization_msgs::msg::Marker lineList; + lineList.type = visualization_msgs::msg::Marker::LINE_LIST; auto numElements = std::distance(firstPos, lastPos); lineList.points.reserve(numElements * (numElements - 1) / 2); // Upper bound on the number of lines diff --git a/ocs2_ros_interfaces/package.xml b/ocs2_ros_interfaces/package.xml index a18ee22b9..49fc1727c 100644 --- a/ocs2_ros_interfaces/package.xml +++ b/ocs2_ros_interfaces/package.xml @@ -1,6 +1,5 @@ - - + ocs2_ros_interfaces 0.0.0 @@ -11,20 +10,19 @@ Jan Carius Ruben Grandia - catkin - - cmake_modules - cmake_clang_tools + ament_cmake - roscpp + rclcpp ocs2_msgs ocs2_core ocs2_ddp + ocs2_oc ocs2_mpc std_msgs visualization_msgs geometry_msgs interactive_markers - rqt_multiplot - + + ament_cmake + diff --git a/ocs2_ros_interfaces/src/command/TargetTrajectoriesInteractiveMarker.cpp b/ocs2_ros_interfaces/src/command/TargetTrajectoriesInteractiveMarker.cpp index 264a59f0f..837d0c965 100644 --- a/ocs2_ros_interfaces/src/command/TargetTrajectoriesInteractiveMarker.cpp +++ b/ocs2_ros_interfaces/src/command/TargetTrajectoriesInteractiveMarker.cpp @@ -29,7 +29,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ros_interfaces/command/TargetTrajectoriesInteractiveMarker.h" -#include +#include #include namespace ocs2 { @@ -37,21 +37,21 @@ namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -TargetTrajectoriesInteractiveMarker::TargetTrajectoriesInteractiveMarker(::ros::NodeHandle& nodeHandle, const std::string& topicPrefix, +TargetTrajectoriesInteractiveMarker::TargetTrajectoriesInteractiveMarker(::rclcpp::Node::SharedPtr& node, const std::string& topicPrefix, GaolPoseToTargetTrajectories gaolPoseToTargetTrajectories) - : server_("simple_marker"), gaolPoseToTargetTrajectories_(std::move(gaolPoseToTargetTrajectories)) { + : server_("simple_marker", node), gaolPoseToTargetTrajectories_(std::move(gaolPoseToTargetTrajectories)), node_(node) { // observation subscriber - auto observationCallback = [this](const ocs2_msgs::mpc_observation::ConstPtr& msg) { + auto observationCallback = [this](const ocs2_msgs::msg::MPCObservation::SharedPtr msg) { std::lock_guard lock(latestObservationMutex_); latestObservation_ = ros_msg_conversions::readObservationMsg(*msg); }; - observationSubscriber_ = nodeHandle.subscribe(topicPrefix + "_mpc_observation", 1, observationCallback); + observationSubscriber_ = node->create_subscription(topicPrefix + "_mpc_observation", 1, observationCallback); // Trajectories publisher - targetTrajectoriesPublisherPtr_.reset(new TargetTrajectoriesRosPublisher(nodeHandle, topicPrefix)); + targetTrajectoriesPublisherPtr_.reset(new TargetTrajectoriesRosPublisher(node, topicPrefix)); // create an interactive marker for our server - menuHandler_.insert("Send target pose", boost::bind(&TargetTrajectoriesInteractiveMarker::processFeedback, this, _1)); + menuHandler_.insert("Send target pose", std::bind(&TargetTrajectoriesInteractiveMarker::processFeedback, this, std::placeholders::_1)); // create an interactive marker for our server auto interactiveMarker = createInteractiveMarker(); @@ -68,10 +68,10 @@ TargetTrajectoriesInteractiveMarker::TargetTrajectoriesInteractiveMarker(::ros:: /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -visualization_msgs::InteractiveMarker TargetTrajectoriesInteractiveMarker::createInteractiveMarker() const { - visualization_msgs::InteractiveMarker interactiveMarker; +visualization_msgs::msg::InteractiveMarker TargetTrajectoriesInteractiveMarker::createInteractiveMarker() const { + visualization_msgs::msg::InteractiveMarker interactiveMarker; interactiveMarker.header.frame_id = "world"; - interactiveMarker.header.stamp = ros::Time::now(); + interactiveMarker.header.stamp = node_->get_clock()->now(); interactiveMarker.name = "Goal"; interactiveMarker.scale = 0.2; interactiveMarker.description = "Right click to send command"; @@ -79,8 +79,8 @@ visualization_msgs::InteractiveMarker TargetTrajectoriesInteractiveMarker::creat // create a grey box marker const auto boxMarker = []() { - visualization_msgs::Marker marker; - marker.type = visualization_msgs::Marker::CUBE; + visualization_msgs::msg::Marker marker; + marker.type = visualization_msgs::msg::Marker::CUBE; marker.scale.x = 0.1; marker.scale.y = 0.1; marker.scale.z = 0.1; @@ -92,10 +92,10 @@ visualization_msgs::InteractiveMarker TargetTrajectoriesInteractiveMarker::creat }(); // create a non-interactive control which contains the box - visualization_msgs::InteractiveMarkerControl boxControl; + visualization_msgs::msg::InteractiveMarkerControl boxControl; boxControl.always_visible = 1; boxControl.markers.push_back(boxMarker); - boxControl.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D; + boxControl.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_ROTATE_3D; // add the control to the interactive marker interactiveMarker.controls.push_back(boxControl); @@ -103,17 +103,17 @@ visualization_msgs::InteractiveMarker TargetTrajectoriesInteractiveMarker::creat // create a control which will move the box // this control does not contain any markers, // which will cause RViz to insert two arrows - visualization_msgs::InteractiveMarkerControl control; + visualization_msgs::msg::InteractiveMarkerControl control; control.orientation.w = 1; control.orientation.x = 1; control.orientation.y = 0; control.orientation.z = 0; control.name = "rotate_x"; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; + control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS; interactiveMarker.controls.push_back(control); control.name = "move_x"; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; + control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS; interactiveMarker.controls.push_back(control); control.orientation.w = 1; @@ -121,10 +121,10 @@ visualization_msgs::InteractiveMarker TargetTrajectoriesInteractiveMarker::creat control.orientation.y = 1; control.orientation.z = 0; control.name = "rotate_z"; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; + control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS; interactiveMarker.controls.push_back(control); control.name = "move_z"; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; + control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS; interactiveMarker.controls.push_back(control); control.orientation.w = 1; @@ -132,10 +132,10 @@ visualization_msgs::InteractiveMarker TargetTrajectoriesInteractiveMarker::creat control.orientation.y = 0; control.orientation.z = 1; control.name = "rotate_y"; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; + control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS; interactiveMarker.controls.push_back(control); control.name = "move_y"; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; + control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS; interactiveMarker.controls.push_back(control); return interactiveMarker; @@ -144,7 +144,7 @@ visualization_msgs::InteractiveMarker TargetTrajectoriesInteractiveMarker::creat /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void TargetTrajectoriesInteractiveMarker::processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) { +void TargetTrajectoriesInteractiveMarker::processFeedback(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr feedback) { // Desired state trajectory const Eigen::Vector3d position(feedback->pose.position.x, feedback->pose.position.y, feedback->pose.position.z); const Eigen::Quaterniond orientation(feedback->pose.orientation.w, feedback->pose.orientation.x, feedback->pose.orientation.y, diff --git a/ocs2_ros_interfaces/src/command/TargetTrajectoriesKeyboardPublisher.cpp b/ocs2_ros_interfaces/src/command/TargetTrajectoriesKeyboardPublisher.cpp index df30f83d9..a5dfd490c 100644 --- a/ocs2_ros_interfaces/src/command/TargetTrajectoriesKeyboardPublisher.cpp +++ b/ocs2_ros_interfaces/src/command/TargetTrajectoriesKeyboardPublisher.cpp @@ -31,7 +31,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include +#include #include namespace ocs2 { @@ -39,17 +39,18 @@ namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -TargetTrajectoriesKeyboardPublisher::TargetTrajectoriesKeyboardPublisher(::ros::NodeHandle& nodeHandle, const std::string& topicPrefix, +TargetTrajectoriesKeyboardPublisher::TargetTrajectoriesKeyboardPublisher(::rclcpp::Node::SharedPtr& nodeHandle, const std::string& topicPrefix, const scalar_array_t& targetCommandLimits, CommandLineToTargetTrajectories commandLineToTargetTrajectoriesFun) : targetCommandLimits_(Eigen::Map(targetCommandLimits.data(), targetCommandLimits.size())), - commandLineToTargetTrajectoriesFun_(std::move(commandLineToTargetTrajectoriesFun)) { + commandLineToTargetTrajectoriesFun_(std::move(commandLineToTargetTrajectoriesFun)), + node_(nodeHandle) { // observation subscriber - auto observationCallback = [this](const ocs2_msgs::mpc_observation::ConstPtr& msg) { + auto observationCallback = [this](const ocs2_msgs::msg::MPCObservation::SharedPtr msg) { std::lock_guard lock(latestObservationMutex_); latestObservation_ = ros_msg_conversions::readObservationMsg(*msg); }; - observationSubscriber_ = nodeHandle.subscribe(topicPrefix + "_mpc_observation", 1, observationCallback); + observationSubscriber_ = nodeHandle->create_subscription(topicPrefix + "_mpc_observation", 1, observationCallback); // Trajectories publisher targetTrajectoriesPublisherPtr_.reset(new TargetTrajectoriesRosPublisher(nodeHandle, topicPrefix)); @@ -59,7 +60,7 @@ TargetTrajectoriesKeyboardPublisher::TargetTrajectoriesKeyboardPublisher(::ros:: /******************************************************************************************************/ /******************************************************************************************************/ void TargetTrajectoriesKeyboardPublisher::publishKeyboardCommand(const std::string& commadMsg) { - while (ros::ok() && ros::master::check()) { + while (rclcpp::ok()) { // get command line std::cout << commadMsg << ": "; const vector_t commandLineInput = getCommandLine().cwiseMin(targetCommandLimits_).cwiseMax(-targetCommandLimits_); @@ -68,7 +69,7 @@ void TargetTrajectoriesKeyboardPublisher::publishKeyboardCommand(const std::stri std::cout << "The following command is published: [" << toDelimitedString(commandLineInput) << "]\n\n"; // get the latest observation - ::ros::spinOnce(); + ::rclcpp::spin_some(node_); SystemObservation observation; { std::lock_guard lock(latestObservationMutex_); @@ -88,7 +89,7 @@ void TargetTrajectoriesKeyboardPublisher::publishKeyboardCommand(const std::stri /******************************************************************************************************/ vector_t TargetTrajectoriesKeyboardPublisher::getCommandLine() { // get command line as one long string - auto shouldTerminate = []() { return !ros::ok() || !ros::master::check(); }; + auto shouldTerminate = []() { return !rclcpp::ok(); }; const std::string line = getCommandLineString(shouldTerminate); // a line to words diff --git a/ocs2_ros_interfaces/src/command/TargetTrajectoriesRosPublisher.cpp b/ocs2_ros_interfaces/src/command/TargetTrajectoriesRosPublisher.cpp index 832bac294..c0f6a0b80 100644 --- a/ocs2_ros_interfaces/src/command/TargetTrajectoriesRosPublisher.cpp +++ b/ocs2_ros_interfaces/src/command/TargetTrajectoriesRosPublisher.cpp @@ -30,24 +30,23 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ros_interfaces/command/TargetTrajectoriesRosPublisher.h" // MPC messages -#include +#include namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -TargetTrajectoriesRosPublisher::TargetTrajectoriesRosPublisher(::ros::NodeHandle& nodeHandle, const std::string& topicPrefix) { - targetTrajectoriesPublisher_ = nodeHandle.advertise(topicPrefix + "_mpc_target", 1, false); - ros::spinOnce(); - ROS_INFO_STREAM("The TargetTrajectories is publishing on " + topicPrefix + "_mpc_target topic."); +TargetTrajectoriesRosPublisher::TargetTrajectoriesRosPublisher(::rclcpp::Node::SharedPtr& nodeHandle, const std::string& topicPrefix) { + targetTrajectoriesPublisher_ = nodeHandle->create_publisher(topicPrefix + "_mpc_target", 1); + rclcpp::spin_some(nodeHandle); + RCLCPP_INFO_STREAM(nodeHandle->get_logger(), "The TargetTrajectories is publishing on " + topicPrefix + "_mpc_target topic."); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ TargetTrajectoriesRosPublisher::~TargetTrajectoriesRosPublisher() { - targetTrajectoriesPublisher_.shutdown(); } /******************************************************************************************************/ @@ -55,7 +54,7 @@ TargetTrajectoriesRosPublisher::~TargetTrajectoriesRosPublisher() { /******************************************************************************************************/ void TargetTrajectoriesRosPublisher::publishTargetTrajectories(const TargetTrajectories& targetTrajectories) { const auto mpcTargetTrajectoriesMsg = ros_msg_conversions::createTargetTrajectoriesMsg(targetTrajectories); - targetTrajectoriesPublisher_.publish(mpcTargetTrajectoriesMsg); + targetTrajectoriesPublisher_->publish(mpcTargetTrajectoriesMsg); } } // namespace ocs2 diff --git a/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp b/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp index 4f203ec75..8af34dbbb 100644 --- a/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp +++ b/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp @@ -35,8 +35,8 @@ namespace ros_msg_conversions { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::mpc_observation createObservationMsg(const SystemObservation& observation) { - ocs2_msgs::mpc_observation observationMsg; +ocs2_msgs::msg::MPCObservation createObservationMsg(const SystemObservation& observation) { + ocs2_msgs::msg::MPCObservation observationMsg; observationMsg.time = observation.time; @@ -58,7 +58,7 @@ ocs2_msgs::mpc_observation createObservationMsg(const SystemObservation& observa /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -SystemObservation readObservationMsg(const ocs2_msgs::mpc_observation& observationMsg) { +SystemObservation readObservationMsg(const ocs2_msgs::msg::MPCObservation& observationMsg) { SystemObservation observation; observation.time = observationMsg.time; @@ -77,20 +77,20 @@ SystemObservation readObservationMsg(const ocs2_msgs::mpc_observation& observati /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::mode_schedule createModeScheduleMsg(const ModeSchedule& modeSchedule) { - ocs2_msgs::mode_schedule modeScheduleMsg; +ocs2_msgs::msg::ModeSchedule createModeScheduleMsg(const ModeSchedule& modeSchedule) { + ocs2_msgs::msg::ModeSchedule modeScheduleMsg; // event times - modeScheduleMsg.eventTimes.clear(); - modeScheduleMsg.eventTimes.reserve(modeSchedule.eventTimes.size()); + modeScheduleMsg.event_times.clear(); + modeScheduleMsg.event_times.reserve(modeSchedule.eventTimes.size()); for (const auto& ti : modeSchedule.eventTimes) { - modeScheduleMsg.eventTimes.push_back(ti); + modeScheduleMsg.event_times.push_back(ti); } // mode sequence - modeScheduleMsg.modeSequence.clear(); - modeScheduleMsg.modeSequence.reserve(modeSchedule.modeSequence.size()); + modeScheduleMsg.mode_sequence.clear(); + modeScheduleMsg.mode_sequence.reserve(modeSchedule.modeSequence.size()); for (const auto& si : modeSchedule.modeSequence) { - modeScheduleMsg.modeSequence.push_back(si); + modeScheduleMsg.mode_sequence.push_back(si); } return modeScheduleMsg; @@ -99,18 +99,18 @@ ocs2_msgs::mode_schedule createModeScheduleMsg(const ModeSchedule& modeSchedule) /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ModeSchedule readModeScheduleMsg(const ocs2_msgs::mode_schedule& modeScheduleMsg) { +ModeSchedule readModeScheduleMsg(const ocs2_msgs::msg::ModeSchedule& modeScheduleMsg) { // event times scalar_array_t eventTimes; - eventTimes.reserve(modeScheduleMsg.eventTimes.size()); - for (const auto& ti : modeScheduleMsg.eventTimes) { + eventTimes.reserve(modeScheduleMsg.event_times.size()); + for (const auto& ti : modeScheduleMsg.event_times) { eventTimes.push_back(ti); } // mode sequence size_array_t modeSequence; - modeSequence.reserve(modeScheduleMsg.modeSequence.size()); - for (const auto& si : modeScheduleMsg.modeSequence) { + modeSequence.reserve(modeScheduleMsg.mode_sequence.size()); + for (const auto& si : modeScheduleMsg.mode_sequence) { modeSequence.push_back(si); } @@ -120,16 +120,16 @@ ModeSchedule readModeScheduleMsg(const ocs2_msgs::mode_schedule& modeScheduleMsg /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::mpc_performance_indices createPerformanceIndicesMsg(scalar_t initTime, const PerformanceIndex& performanceIndices) { - ocs2_msgs::mpc_performance_indices performanceIndicesMsg; +ocs2_msgs::msg::MPCPerformanceIndices createPerformanceIndicesMsg(scalar_t initTime, const PerformanceIndex& performanceIndices) { + ocs2_msgs::msg::MPCPerformanceIndices performanceIndicesMsg; - performanceIndicesMsg.initTime = initTime; + performanceIndicesMsg.init_time = initTime; performanceIndicesMsg.merit = performanceIndices.merit; performanceIndicesMsg.cost = performanceIndices.cost; - performanceIndicesMsg.dynamicsViolationSSE = performanceIndices.dynamicsViolationSSE; - performanceIndicesMsg.equalityConstraintsSSE = performanceIndices.equalityConstraintsSSE; - performanceIndicesMsg.equalityLagrangian = performanceIndices.equalityLagrangian; - performanceIndicesMsg.inequalityLagrangian = performanceIndices.inequalityLagrangian; + performanceIndicesMsg.dynamics_violation_sse = performanceIndices.dynamicsViolationSSE; + performanceIndicesMsg.equality_constraints_sse = performanceIndices.equalityConstraintsSSE; + performanceIndicesMsg.equality_lagrangian = performanceIndices.equalityLagrangian; + performanceIndicesMsg.inequality_lagrangian = performanceIndices.inequalityLagrangian; return performanceIndicesMsg; } @@ -137,15 +137,15 @@ ocs2_msgs::mpc_performance_indices createPerformanceIndicesMsg(scalar_t initTime /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -PerformanceIndex readPerformanceIndicesMsg(const ocs2_msgs::mpc_performance_indices& performanceIndicesMsg) { +PerformanceIndex readPerformanceIndicesMsg(const ocs2_msgs::msg::MPCPerformanceIndices& performanceIndicesMsg) { PerformanceIndex performanceIndices; performanceIndices.merit = performanceIndicesMsg.merit; performanceIndices.cost = performanceIndicesMsg.cost; - performanceIndices.dynamicsViolationSSE = performanceIndicesMsg.dynamicsViolationSSE; - performanceIndices.equalityConstraintsSSE = performanceIndicesMsg.equalityConstraintsSSE; - performanceIndices.equalityLagrangian = performanceIndicesMsg.equalityLagrangian; - performanceIndices.inequalityLagrangian = performanceIndicesMsg.inequalityLagrangian; + performanceIndices.dynamicsViolationSSE = performanceIndicesMsg.dynamics_violation_sse; + performanceIndices.equalityConstraintsSSE = performanceIndicesMsg.equality_constraints_sse; + performanceIndices.equalityLagrangian = performanceIndicesMsg.equality_lagrangian; + performanceIndices.inequalityLagrangian = performanceIndicesMsg.inequality_lagrangian; return performanceIndices; } @@ -153,28 +153,28 @@ PerformanceIndex readPerformanceIndicesMsg(const ocs2_msgs::mpc_performance_indi /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::mpc_target_trajectories createTargetTrajectoriesMsg(const TargetTrajectories& targetTrajectories) { - ocs2_msgs::mpc_target_trajectories targetTrajectoriesMsg; +ocs2_msgs::msg::MPCTargetTrajectories createTargetTrajectoriesMsg(const TargetTrajectories& targetTrajectories) { + ocs2_msgs::msg::MPCTargetTrajectories targetTrajectoriesMsg; const auto& timeTrajectory = targetTrajectories.timeTrajectory; const auto& stateTrajectory = targetTrajectories.stateTrajectory; const auto& inputTrajectory = targetTrajectories.inputTrajectory; // time and state size_t N = stateTrajectory.size(); - targetTrajectoriesMsg.timeTrajectory.resize(N); - targetTrajectoriesMsg.stateTrajectory.resize(N); + targetTrajectoriesMsg.time_trajectory.resize(N); + targetTrajectoriesMsg.state_trajectory.resize(N); for (size_t i = 0; i < N; i++) { - targetTrajectoriesMsg.timeTrajectory[i] = timeTrajectory[i]; + targetTrajectoriesMsg.time_trajectory[i] = timeTrajectory[i]; - targetTrajectoriesMsg.stateTrajectory[i].value = + targetTrajectoriesMsg.state_trajectory[i].value = std::vector(stateTrajectory[i].data(), stateTrajectory[i].data() + stateTrajectory[i].size()); } // end of i loop // input N = inputTrajectory.size(); - targetTrajectoriesMsg.inputTrajectory.resize(N); + targetTrajectoriesMsg.input_trajectory.resize(N); for (size_t i = 0; i < N; i++) { - targetTrajectoriesMsg.inputTrajectory[i].value = + targetTrajectoriesMsg.input_trajectory[i].value = std::vector(inputTrajectory[i].data(), inputTrajectory[i].data() + inputTrajectory[i].size()); } // end of i loop @@ -184,8 +184,8 @@ ocs2_msgs::mpc_target_trajectories createTargetTrajectoriesMsg(const TargetTraje /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -TargetTrajectories readTargetTrajectoriesMsg(const ocs2_msgs::mpc_target_trajectories& targetTrajectoriesMsg) { - size_t N = targetTrajectoriesMsg.stateTrajectory.size(); +TargetTrajectories readTargetTrajectoriesMsg(const ocs2_msgs::msg::MPCTargetTrajectories& targetTrajectoriesMsg) { + size_t N = targetTrajectoriesMsg.state_trajectory.size(); if (N == 0) { throw std::runtime_error("An empty target trajectories message is received."); } @@ -194,19 +194,19 @@ TargetTrajectories readTargetTrajectoriesMsg(const ocs2_msgs::mpc_target_traject scalar_array_t desiredTimeTrajectory(N); vector_array_t desiredStateTrajectory(N); for (size_t i = 0; i < N; i++) { - desiredTimeTrajectory[i] = targetTrajectoriesMsg.timeTrajectory[i]; + desiredTimeTrajectory[i] = targetTrajectoriesMsg.time_trajectory[i]; - desiredStateTrajectory[i] = Eigen::Map(targetTrajectoriesMsg.stateTrajectory[i].value.data(), - targetTrajectoriesMsg.stateTrajectory[i].value.size()) + desiredStateTrajectory[i] = Eigen::Map(targetTrajectoriesMsg.state_trajectory[i].value.data(), + targetTrajectoriesMsg.state_trajectory[i].value.size()) .cast(); } // end of i loop // input - N = targetTrajectoriesMsg.inputTrajectory.size(); + N = targetTrajectoriesMsg.input_trajectory.size(); vector_array_t desiredInputTrajectory(N); for (size_t i = 0; i < N; i++) { - desiredInputTrajectory[i] = Eigen::Map(targetTrajectoriesMsg.inputTrajectory[i].value.data(), - targetTrajectoriesMsg.inputTrajectory[i].value.size()) + desiredInputTrajectory[i] = Eigen::Map(targetTrajectoriesMsg.input_trajectory[i].value.data(), + targetTrajectoriesMsg.input_trajectory[i].value.size()) .cast(); } // end of i loop diff --git a/ocs2_ros_interfaces/src/common/RosMsgHelpers.cpp b/ocs2_ros_interfaces/src/common/RosMsgHelpers.cpp index 2d716dbd8..7831d399f 100644 --- a/ocs2_ros_interfaces/src/common/RosMsgHelpers.cpp +++ b/ocs2_ros_interfaces/src/common/RosMsgHelpers.cpp @@ -32,24 +32,24 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace ros_msg_helpers { -geometry_msgs::Point getPointMsg(const Eigen::Vector3d& point) { - geometry_msgs::Point pointMsg; +geometry_msgs::msg::Point getPointMsg(const Eigen::Vector3d& point) { + geometry_msgs::msg::Point pointMsg; pointMsg.x = point.x(); pointMsg.y = point.y(); pointMsg.z = point.z(); return pointMsg; } -geometry_msgs::Vector3 getVectorMsg(const Eigen::Vector3d& vec) { - geometry_msgs::Vector3 vecMsg; +geometry_msgs::msg::Vector3 getVectorMsg(const Eigen::Vector3d& vec) { + geometry_msgs::msg::Vector3 vecMsg; vecMsg.x = vec.x(); vecMsg.y = vec.y(); vecMsg.z = vec.z(); return vecMsg; } -geometry_msgs::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientation) { - geometry_msgs::Quaternion orientationMsg; +geometry_msgs::msg::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientation) { + geometry_msgs::msg::Quaternion orientationMsg; orientationMsg.x = orientation.x(); orientationMsg.y = orientation.y(); orientationMsg.z = orientation.z(); @@ -57,16 +57,16 @@ geometry_msgs::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientatio return orientationMsg; } -std_msgs::Header getHeaderMsg(const std::string& frame_id, const ros::Time& timeStamp) { - std_msgs::Header header; +std_msgs::msg::Header getHeaderMsg(const std::string& frame_id, const rclcpp::Time& timeStamp) { + std_msgs::msg::Header header; header.frame_id = frame_id; header.stamp = timeStamp; return header; } -visualization_msgs::Marker getLineMsg(std::vector&& points, std::array color, double lineWidth) { - visualization_msgs::Marker line; - line.type = visualization_msgs::Marker::LINE_STRIP; +visualization_msgs::msg::Marker getLineMsg(std::vector&& points, std::array color, double lineWidth) { + visualization_msgs::msg::Marker line; + line.type = visualization_msgs::msg::Marker::LINE_STRIP; line.scale.x = lineWidth; line.color = getColor(color); line.points = std::move(points); @@ -74,8 +74,8 @@ visualization_msgs::Marker getLineMsg(std::vector&& points return line; } -std_msgs::ColorRGBA getColor(std::array rgb, double alpha /* = 1.0*/) { - std_msgs::ColorRGBA colorMsg; +std_msgs::msg::ColorRGBA getColor(std::array rgb, double alpha /* = 1.0*/) { + std_msgs::msg::ColorRGBA colorMsg; colorMsg.r = rgb[0]; colorMsg.g = rgb[1]; colorMsg.b = rgb[2]; diff --git a/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp b/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp index 2872fd8d8..f866a9434 100644 --- a/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp +++ b/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp @@ -33,6 +33,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { +static auto LOGGER = rclcpp::get_logger("MPC_ROS_Interface"); + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -74,11 +76,12 @@ void MPC_ROS_Interface::resetMpcNode(TargetTrajectories&& initTargetTrajectories /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -bool MPC_ROS_Interface::resetMpcCallback(ocs2_msgs::reset::Request& req, ocs2_msgs::reset::Response& res) { - if (static_cast(req.reset)) { - auto targetTrajectories = ros_msg_conversions::readTargetTrajectoriesMsg(req.targetTrajectories); +bool MPC_ROS_Interface::resetMpcCallback(const std::shared_ptr req, + std::shared_ptr res) { + if (static_cast(req->reset)) { + auto targetTrajectories = ros_msg_conversions::readTargetTrajectoriesMsg(req->target_trajectories); resetMpcNode(std::move(targetTrajectories)); - res.done = static_cast(true); + res->done = static_cast(true); std::cerr << "\n#####################################################" << "\n#####################################################" @@ -88,7 +91,7 @@ bool MPC_ROS_Interface::resetMpcCallback(ocs2_msgs::reset::Request& req, ocs2_ms return true; } else { - ROS_WARN_STREAM("[MPC_ROS_Interface] Reset request failed!"); + RCLCPP_WARN_STREAM(LOGGER, "[MPC_ROS_Interface] Reset request failed!"); return false; } } @@ -96,23 +99,23 @@ bool MPC_ROS_Interface::resetMpcCallback(ocs2_msgs::reset::Request& req, ocs2_ms /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::mpc_flattened_controller MPC_ROS_Interface::createMpcPolicyMsg(const PrimalSolution& primalSolution, +ocs2_msgs::msg::MPCFlattenedController MPC_ROS_Interface::createMpcPolicyMsg(const PrimalSolution& primalSolution, const CommandData& commandData, const PerformanceIndex& performanceIndices) { - ocs2_msgs::mpc_flattened_controller mpcPolicyMsg; + ocs2_msgs::msg::MPCFlattenedController mpcPolicyMsg; - mpcPolicyMsg.initObservation = ros_msg_conversions::createObservationMsg(commandData.mpcInitObservation_); - mpcPolicyMsg.planTargetTrajectories = ros_msg_conversions::createTargetTrajectoriesMsg(commandData.mpcTargetTrajectories_); - mpcPolicyMsg.modeSchedule = ros_msg_conversions::createModeScheduleMsg(primalSolution.modeSchedule_); - mpcPolicyMsg.performanceIndices = + mpcPolicyMsg.init_observation = ros_msg_conversions::createObservationMsg(commandData.mpcInitObservation_); + mpcPolicyMsg.plan_target_trajectories = ros_msg_conversions::createTargetTrajectoriesMsg(commandData.mpcTargetTrajectories_); + mpcPolicyMsg.mode_schedule = ros_msg_conversions::createModeScheduleMsg(primalSolution.modeSchedule_); + mpcPolicyMsg.performance_indices = ros_msg_conversions::createPerformanceIndicesMsg(commandData.mpcInitObservation_.time, performanceIndices); switch (primalSolution.controllerPtr_->getType()) { case ControllerType::FEEDFORWARD: - mpcPolicyMsg.controllerType = ocs2_msgs::mpc_flattened_controller::CONTROLLER_FEEDFORWARD; + mpcPolicyMsg.controller_type = ocs2_msgs::msg::MPCFlattenedController::CONTROLLER_FEEDFORWARD; break; case ControllerType::LINEAR: - mpcPolicyMsg.controllerType = ocs2_msgs::mpc_flattened_controller::CONTROLLER_LINEAR; + mpcPolicyMsg.controller_type = ocs2_msgs::msg::MPCFlattenedController::CONTROLLER_LINEAR; break; default: throw std::runtime_error("MPC_ROS_Interface::createMpcPolicyMsg: Unknown ControllerType"); @@ -121,43 +124,43 @@ ocs2_msgs::mpc_flattened_controller MPC_ROS_Interface::createMpcPolicyMsg(const // maximum length of the message const size_t N = primalSolution.timeTrajectory_.size(); - mpcPolicyMsg.timeTrajectory.clear(); - mpcPolicyMsg.timeTrajectory.reserve(N); - mpcPolicyMsg.stateTrajectory.clear(); - mpcPolicyMsg.stateTrajectory.reserve(N); + mpcPolicyMsg.time_trajectory.clear(); + mpcPolicyMsg.time_trajectory.reserve(N); + mpcPolicyMsg.state_trajectory.clear(); + mpcPolicyMsg.state_trajectory.reserve(N); mpcPolicyMsg.data.clear(); mpcPolicyMsg.data.reserve(N); - mpcPolicyMsg.postEventIndices.clear(); - mpcPolicyMsg.postEventIndices.reserve(primalSolution.postEventIndices_.size()); + mpcPolicyMsg.post_event_indices.clear(); + mpcPolicyMsg.post_event_indices.reserve(primalSolution.postEventIndices_.size()); // time for (auto t : primalSolution.timeTrajectory_) { - mpcPolicyMsg.timeTrajectory.emplace_back(t); + mpcPolicyMsg.time_trajectory.emplace_back(t); } // post-event indices for (auto ind : primalSolution.postEventIndices_) { - mpcPolicyMsg.postEventIndices.emplace_back(static_cast(ind)); + mpcPolicyMsg.post_event_indices.emplace_back(static_cast(ind)); } // state for (size_t k = 0; k < N; k++) { - ocs2_msgs::mpc_state mpcState; + ocs2_msgs::msg::MPCState mpcState; mpcState.value.resize(primalSolution.stateTrajectory_[k].rows()); for (size_t j = 0; j < primalSolution.stateTrajectory_[k].rows(); j++) { mpcState.value[j] = primalSolution.stateTrajectory_[k](j); } - mpcPolicyMsg.stateTrajectory.emplace_back(mpcState); + mpcPolicyMsg.state_trajectory.emplace_back(mpcState); } // end of k loop // input for (size_t k = 0; k < N; k++) { - ocs2_msgs::mpc_input mpcInput; + ocs2_msgs::msg::MPCInput mpcInput; mpcInput.value.resize(primalSolution.inputTrajectory_[k].rows()); for (size_t j = 0; j < primalSolution.inputTrajectory_[k].rows(); j++) { mpcInput.value[j] = primalSolution.inputTrajectory_[k](j); } - mpcPolicyMsg.inputTrajectory.emplace_back(mpcInput); + mpcPolicyMsg.input_trajectory.emplace_back(mpcInput); } // end of k loop // controller @@ -165,7 +168,7 @@ ocs2_msgs::mpc_flattened_controller MPC_ROS_Interface::createMpcPolicyMsg(const std::vector*> policyMsgDataPointers; policyMsgDataPointers.reserve(N); for (auto t : primalSolution.timeTrajectory_) { - mpcPolicyMsg.data.emplace_back(ocs2_msgs::controller_data()); + mpcPolicyMsg.data.emplace_back(ocs2_msgs::msg::ControllerData()); policyMsgDataPointers.push_back(&mpcPolicyMsg.data.back().data); timeTrajectoryTruncated.push_back(t); @@ -197,11 +200,11 @@ void MPC_ROS_Interface::publisherWorker() { publisherPerformanceIndicesPtr_.swap(bufferPerformanceIndicesPtr_); } - ocs2_msgs::mpc_flattened_controller mpcPolicyMsg = + ocs2_msgs::msg::MPCFlattenedController mpcPolicyMsg = createMpcPolicyMsg(*publisherPrimalSolutionPtr_, *publisherCommandPtr_, *publisherPerformanceIndicesPtr_); // publish the message - mpcPolicyPublisher_.publish(mpcPolicyMsg); + mpcPolicyPublisher_->publish(mpcPolicyMsg); readyToPublish_ = false; lk.unlock(); @@ -234,11 +237,11 @@ void MPC_ROS_Interface::copyToBuffer(const SystemObservation& mpcInitObservation /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation::ConstPtr& msg) { +void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::msg::MPCObservation::SharedPtr msg) { std::lock_guard resetLock(resetMutex_); if (!resetRequestedEver_.load()) { - ROS_WARN_STREAM("MPC should be reset first. Either call MPC_ROS_Interface::reset() or use the reset service."); + RCLCPP_WARN_STREAM(LOGGER, "MPC should be reset first. Either call MPC_ROS_Interface::reset() or use the reset service."); return; } @@ -283,9 +286,9 @@ void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation: msgReady_.notify_one(); #else - ocs2_msgs::mpc_flattened_controller mpcPolicyMsg = + ocs2_msgs::msg::MPCFlattenedController mpcPolicyMsg = createMpcPolicyMsg(*bufferPrimalSolutionPtr_, *bufferCommandPtr_, *bufferPerformanceIndicesPtr_); - mpcPolicyPublisher_.publish(mpcPolicyMsg); + mpcPolicyPublisher_->publish(mpcPolicyMsg); #endif } @@ -294,7 +297,7 @@ void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation: /******************************************************************************************************/ void MPC_ROS_Interface::shutdownNode() { #ifdef PUBLISH_THREAD - ROS_INFO_STREAM("Shutting down workers ..."); + RCLCPP_INFO_STREAM(LOGGER, "Shutting down workers ..."); std::unique_lock lk(publisherMutex_); terminateThread_ = true; @@ -306,49 +309,44 @@ void MPC_ROS_Interface::shutdownNode() { publisherWorker_.join(); } - ROS_INFO_STREAM("All workers are shut down."); + RCLCPP_INFO_STREAM(LOGGER, "All workers are shut down."); #endif // shutdown publishers - mpcPolicyPublisher_.shutdown(); + mpcPolicyPublisher_.reset(); } -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void MPC_ROS_Interface::spin() { - ROS_INFO_STREAM("Start spinning now ..."); - // Equivalent to ros::spin() + check if master is alive - while (::ros::ok() && ::ros::master::check()) { - ::ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0.1)); - } -} /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MPC_ROS_Interface::launchNodes(ros::NodeHandle& nodeHandle) { - ROS_INFO_STREAM("MPC node is setting up ..."); +void MPC_ROS_Interface::launchNodes(rclcpp::Node::SharedPtr& nodeHandle) { + RCLCPP_INFO_STREAM(LOGGER, "MPC node is setting up ..."); + + using namespace std::placeholders; // Observation subscriber - mpcObservationSubscriber_ = nodeHandle.subscribe(topicPrefix_ + "_mpc_observation", 1, &MPC_ROS_Interface::mpcObservationCallback, this, - ::ros::TransportHints().tcpNoDelay()); + mpcObservationSubscriber_ = nodeHandle->create_subscription( + topicPrefix_ + "_mpc_observation", 1, + std::bind(&MPC_ROS_Interface::mpcObservationCallback, this, _1)); // MPC publisher - mpcPolicyPublisher_ = nodeHandle.advertise(topicPrefix_ + "_mpc_policy", 1, true); + mpcPolicyPublisher_ = nodeHandle->create_publisher( + topicPrefix_ + "_mpc_policy", 1); // MPC reset service server - mpcResetServiceServer_ = nodeHandle.advertiseService(topicPrefix_ + "_mpc_reset", &MPC_ROS_Interface::resetMpcCallback, this); + mpcResetServiceServer_ = nodeHandle->create_service( + topicPrefix_ + "_mpc_reset", std::bind(&MPC_ROS_Interface::resetMpcCallback, this, _1, _2)); // display #ifdef PUBLISH_THREAD - ROS_INFO_STREAM("Publishing SLQ-MPC messages on a separate thread."); + RCLCPP_INFO_STREAM(LOGGER, "Publishing SLQ-MPC messages on a separate thread."); #endif - ROS_INFO_STREAM("MPC node is ready."); + RCLCPP_INFO_STREAM(LOGGER, "MPC node is ready."); - // spin - spin(); + RCLCPP_INFO_STREAM(LOGGER, "Start spinning now ..."); + rclcpp::spin(nodeHandle); } } // namespace ocs2 diff --git a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp index d41995ae5..9ddb1cc68 100644 --- a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp +++ b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp @@ -31,6 +31,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { +static auto LOGGER = rclcpp::get_logger("MRT_ROS_Dummy_Loop"); + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -41,7 +43,7 @@ MRT_ROS_Dummy_Loop::MRT_ROS_Dummy_Loop(MRT_ROS_Interface& mrt, scalar_t mrtDesir } if (mpcDesiredFrequency_ > 0) { - ROS_WARN_STREAM("MPC loop is not realtime! For realtime setting, set mpcDesiredFrequency to any negative number."); + RCLCPP_WARN_STREAM(LOGGER, "MPC loop is not realtime! For realtime setting, set mpcDesiredFrequency to any negative number."); } } @@ -49,18 +51,20 @@ MRT_ROS_Dummy_Loop::MRT_ROS_Dummy_Loop(MRT_ROS_Interface& mrt, scalar_t mrtDesir /******************************************************************************************************/ /******************************************************************************************************/ void MRT_ROS_Dummy_Loop::run(const SystemObservation& initObservation, const TargetTrajectories& initTargetTrajectories) { - ROS_INFO_STREAM("Waiting for the initial policy ..."); - + RCLCPP_INFO_STREAM(LOGGER, "Waiting for the initial policy ..."); + const scalar_t timeStep = (1.0 / mrtDesiredFrequency_); // Reset MPC node mrt_.resetMpcNode(initTargetTrajectories); // Wait for the initial policy - while (!mrt_.initialPolicyReceived() && ros::ok() && ros::master::check()) { + while (!mrt_.initialPolicyReceived() && ::rclcpp::ok()) { mrt_.spinMRT(); mrt_.setCurrentObservation(initObservation); - ros::Rate(mrtDesiredFrequency_).sleep(); + + int16_t sleeptime = rclcpp::Duration(timeStep).nanoseconds(); + rclcpp::sleep_for(std::chrono::nanoseconds(sleeptime)); } - ROS_INFO_STREAM("Initial policy has been received."); + RCLCPP_INFO_STREAM(LOGGER, "Initial policy has been received."); // Pick simulation loop mode if (mpcDesiredFrequency_ > 0.0) { @@ -88,8 +92,8 @@ void MRT_ROS_Dummy_Loop::synchronizedDummyLoop(const SystemObservation& initObse return mrt_.updatePolicy() && std::abs(mrt_.getPolicy().timeTrajectory_.front() - time) < (tol / mpcDesiredFrequency_); }; - ros::Rate simRate(mrtDesiredFrequency_); - while (ros::ok() && ros::master::check()) { + rclcpp::Rate simRate(mrtDesiredFrequency_); + while (rclcpp::ok()) { std::cout << "### Current time " << currentObservation.time << "\n"; // Trigger MRT callbacks @@ -98,7 +102,7 @@ void MRT_ROS_Dummy_Loop::synchronizedDummyLoop(const SystemObservation& initObse // Update the MPC policy if it is time to do so if (loopCounter % mpcUpdateRatio == 0) { // Wait for the policy to be updated - while (!policyUpdatedForTime(currentObservation.time) && ros::ok() && ros::master::check()) { + while (!policyUpdatedForTime(currentObservation.time) && rclcpp::ok()) { mrt_.spinMRT(); } std::cout << "<<< New MPC policy starting at " << mrt_.getPolicy().timeTrajectory_.front() << "\n"; @@ -122,7 +126,7 @@ void MRT_ROS_Dummy_Loop::synchronizedDummyLoop(const SystemObservation& initObse } ++loopCounter; - ros::spinOnce(); + // REQUIRED on NOT? rclcpp::spin_some(); simRate.sleep(); } } @@ -134,8 +138,8 @@ void MRT_ROS_Dummy_Loop::realtimeDummyLoop(const SystemObservation& initObservat // Loop variables SystemObservation currentObservation = initObservation; - ros::Rate simRate(mrtDesiredFrequency_); - while (ros::ok() && ros::master::check()) { + rclcpp::Rate simRate(mrtDesiredFrequency_); + while (rclcpp::ok()) { std::cout << "### Current time " << currentObservation.time << "\n"; // Trigger MRT callbacks @@ -160,7 +164,7 @@ void MRT_ROS_Dummy_Loop::realtimeDummyLoop(const SystemObservation& initObservat observer->update(currentObservation, mrt_.getPolicy(), mrt_.getCommand()); } - ros::spinOnce(); + // REQUIRED on NOT? rclcpp::spin_some(); simRate.sleep(); } } diff --git a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp index 60397deff..5fd1fcba0 100644 --- a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp +++ b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp @@ -2,278 +2,281 @@ Copyright (c) 2020, Farbod Farshidian. All rights reserved. Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: + modification, are permitted provided that the following conditions are met: -* Redistributions of source code must retain the above copyright notice, this + * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. -* Redistributions in binary form must reproduce the above copyright notice, + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -******************************************************************************/ + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ #include "ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h" #include #include -namespace ocs2 { + namespace ocs2 { -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -MRT_ROS_Interface::MRT_ROS_Interface(std::string topicPrefix, ros::TransportHints mrtTransportHints) - : topicPrefix_(std::move(topicPrefix)), mrtTransportHints_(mrtTransportHints) { + static auto LOGGER = rclcpp::get_logger("MRT_ROS_Interface"); + + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + MRT_ROS_Interface::MRT_ROS_Interface(std::string topicPrefix) + : topicPrefix_(std::move(topicPrefix)) { // Start thread for publishing #ifdef PUBLISH_THREAD - // Close old thread if it is already running - shutdownPublisher(); - terminateThread_ = false; - readyToPublish_ = false; - publisherWorker_ = std::thread(&MRT_ROS_Interface::publisherWorkerThread, this); + // Close old thread if it is already running + shutdownPublisher(); + terminateThread_ = false; + readyToPublish_ = false; + publisherWorker_ = std::thread(&MRT_ROS_Interface::publisherWorkerThread, this); #endif -} - -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -MRT_ROS_Interface::~MRT_ROS_Interface() { - shutdownNodes(); -} - -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void MRT_ROS_Interface::resetMpcNode(const TargetTrajectories& initTargetTrajectories) { - this->reset(); - - ocs2_msgs::reset resetSrv; - resetSrv.request.reset = static_cast(true); - resetSrv.request.targetTrajectories = ros_msg_conversions::createTargetTrajectoriesMsg(initTargetTrajectories); - - while (!mpcResetServiceClient_.waitForExistence(ros::Duration(5.0)) && ::ros::ok() && ::ros::master::check()) { - ROS_ERROR_STREAM("Failed to call service to reset MPC, retrying..."); } - mpcResetServiceClient_.call(resetSrv); - ROS_INFO_STREAM("MPC node has been reset."); -} + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + MRT_ROS_Interface::~MRT_ROS_Interface() { + shutdownNodes(); + } + + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + void MRT_ROS_Interface::resetMpcNode(const TargetTrajectories& initTargetTrajectories) { + this->reset(); + + auto request = std::make_shared(); + + request->reset = static_cast(true); + request->target_trajectories = ros_msg_conversions::createTargetTrajectoriesMsg(initTargetTrajectories); -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void MRT_ROS_Interface::setCurrentObservation(const SystemObservation& currentObservation) { + while (!mpcResetServiceClient_->wait_for_service(std::chrono::seconds(5)) && ::rclcpp::ok()) { + RCLCPP_ERROR_STREAM(LOGGER, "Failed to call service to reset MPC, retrying..."); + } + + mpcResetServiceClient_->async_send_request(request); + RCLCPP_INFO_STREAM(LOGGER, "MPC node has been reset."); + } + + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + void MRT_ROS_Interface::setCurrentObservation(const SystemObservation& currentObservation) { #ifdef PUBLISH_THREAD - std::unique_lock lk(publisherMutex_); + std::unique_lock lk(publisherMutex_); #endif - // create the message - mpcObservationMsg_ = ros_msg_conversions::createObservationMsg(currentObservation); + // create the message + mpcObservationMsg_ = ros_msg_conversions::createObservationMsg(currentObservation); - // publish the current observation + // publish the current observation #ifdef PUBLISH_THREAD - readyToPublish_ = true; - lk.unlock(); - msgReady_.notify_one(); + readyToPublish_ = true; + lk.unlock(); + msgReady_.notify_one(); #else - mpcObservationPublisher_.publish(mpcObservationMsg_); + mpcObservationPublisher_->publish(mpcObservationMsg_); #endif -} + } -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void MRT_ROS_Interface::publisherWorkerThread() { - while (!terminateThread_) { - std::unique_lock lk(publisherMutex_); + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + void MRT_ROS_Interface::publisherWorkerThread() { + while (!terminateThread_) { + std::unique_lock lk(publisherMutex_); - msgReady_.wait(lk, [&] { return (readyToPublish_ || terminateThread_); }); + msgReady_.wait(lk, [&] { return (readyToPublish_ || terminateThread_); }); - if (terminateThread_) { - break; - } + if (terminateThread_) { + break; + } - mpcObservationMsgBuffer_ = std::move(mpcObservationMsg_); + mpcObservationMsgBuffer_ = std::move(mpcObservationMsg_); - readyToPublish_ = false; + readyToPublish_ = false; - lk.unlock(); - msgReady_.notify_one(); + lk.unlock(); + msgReady_.notify_one(); - mpcObservationPublisher_.publish(mpcObservationMsgBuffer_); - } -} - -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void MRT_ROS_Interface::readPolicyMsg(const ocs2_msgs::mpc_flattened_controller& msg, CommandData& commandData, - PrimalSolution& primalSolution, PerformanceIndex& performanceIndices) { - commandData.mpcInitObservation_ = ros_msg_conversions::readObservationMsg(msg.initObservation); - commandData.mpcTargetTrajectories_ = ros_msg_conversions::readTargetTrajectoriesMsg(msg.planTargetTrajectories); - performanceIndices = ros_msg_conversions::readPerformanceIndicesMsg(msg.performanceIndices); - - const size_t N = msg.timeTrajectory.size(); - if (N == 0) { - throw std::runtime_error("[MRT_ROS_Interface::readPolicyMsg] controller message is empty!"); - } - if (msg.stateTrajectory.size() != N && msg.inputTrajectory.size() != N) { - throw std::runtime_error("[MRT_ROS_Interface::readPolicyMsg] state and input trajectories must have same length!"); - } - if (msg.data.size() != N) { - throw std::runtime_error("[MRT_ROS_Interface::readPolicyMsg] Data has the wrong length!"); + mpcObservationPublisher_->publish(mpcObservationMsgBuffer_); + } } - primalSolution.clear(); - - primalSolution.modeSchedule_ = ros_msg_conversions::readModeScheduleMsg(msg.modeSchedule); - - size_array_t stateDim(N); - size_array_t inputDim(N); - primalSolution.timeTrajectory_.reserve(N); - primalSolution.stateTrajectory_.reserve(N); - primalSolution.inputTrajectory_.reserve(N); - for (size_t i = 0; i < N; i++) { - stateDim[i] = msg.stateTrajectory[i].value.size(); - inputDim[i] = msg.inputTrajectory[i].value.size(); - primalSolution.timeTrajectory_.emplace_back(msg.timeTrajectory[i]); - primalSolution.stateTrajectory_.emplace_back( - Eigen::Map(msg.stateTrajectory[i].value.data(), stateDim[i]).cast()); - primalSolution.inputTrajectory_.emplace_back( - Eigen::Map(msg.inputTrajectory[i].value.data(), inputDim[i]).cast()); - } + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + void MRT_ROS_Interface::readPolicyMsg(const ocs2_msgs::msg::MPCFlattenedController& msg, CommandData& commandData, + PrimalSolution& primalSolution, PerformanceIndex& performanceIndices) { - primalSolution.postEventIndices_.reserve(msg.postEventIndices.size()); - for (auto ind : msg.postEventIndices) { - primalSolution.postEventIndices_.emplace_back(static_cast(ind)); - } + commandData.mpcInitObservation_ = ros_msg_conversions::readObservationMsg(msg.init_observation); + commandData.mpcTargetTrajectories_ = ros_msg_conversions::readTargetTrajectoriesMsg(msg.plan_target_trajectories); + performanceIndices = ros_msg_conversions::readPerformanceIndicesMsg(msg.performance_indices); - std::vector const*> controllerDataPtrArray(N, nullptr); - for (int i = 0; i < N; i++) { - controllerDataPtrArray[i] = &(msg.data[i].data); - } + const size_t N = msg.time_trajectory.size(); + if (N == 0) { + throw std::runtime_error("[MRT_ROS_Interface::readPolicyMsg] controller message is empty!"); + } + if (msg.state_trajectory.size() != N && msg.input_trajectory.size() != N) { + throw std::runtime_error("[MRT_ROS_Interface::readPolicyMsg] state and input trajectories must have same length!"); + } + if (msg.data.size() != N) { + throw std::runtime_error("[MRT_ROS_Interface::readPolicyMsg] Data has the wrong length!"); + } - // instantiate the correct controller - switch (msg.controllerType) { - case ocs2_msgs::mpc_flattened_controller::CONTROLLER_FEEDFORWARD: { - auto controller = FeedforwardController::unFlatten(primalSolution.timeTrajectory_, controllerDataPtrArray); - primalSolution.controllerPtr_.reset(new FeedforwardController(std::move(controller))); - break; + primalSolution.clear(); + + primalSolution.modeSchedule_ = ros_msg_conversions::readModeScheduleMsg(msg.mode_schedule); + + size_array_t stateDim(N); + size_array_t inputDim(N); + primalSolution.timeTrajectory_.reserve(N); + primalSolution.stateTrajectory_.reserve(N); + primalSolution.inputTrajectory_.reserve(N); + for (size_t i = 0; i < N; i++) { + stateDim[i] = msg.state_trajectory[i].value.size(); + inputDim[i] = msg.input_trajectory[i].value.size(); + primalSolution.timeTrajectory_.emplace_back(msg.time_trajectory[i]); + primalSolution.stateTrajectory_.emplace_back( + Eigen::Map(msg.state_trajectory[i].value.data(), stateDim[i]).cast()); + primalSolution.inputTrajectory_.emplace_back( + Eigen::Map(msg.input_trajectory[i].value.data(), inputDim[i]).cast()); } - case ocs2_msgs::mpc_flattened_controller::CONTROLLER_LINEAR: { - auto controller = LinearController::unFlatten(stateDim, inputDim, primalSolution.timeTrajectory_, controllerDataPtrArray); - primalSolution.controllerPtr_.reset(new LinearController(std::move(controller))); - break; + + primalSolution.postEventIndices_.reserve(msg.post_event_indices.size()); + for (auto ind : msg.post_event_indices) { + primalSolution.postEventIndices_.emplace_back(static_cast(ind)); } - default: - throw std::runtime_error("[MRT_ROS_Interface::readPolicyMsg] Unknown controllerType!"); + + std::vector const*> controllerDataPtrArray(N, nullptr); + for (int i = 0; i < N; i++) { + controllerDataPtrArray[i] = &(msg.data[i].data); + } + + // instantiate the correct controller + switch (msg.controller_type) { + case ocs2_msgs::msg::MPCFlattenedController::CONTROLLER_FEEDFORWARD: { + auto controller = FeedforwardController::unFlatten(primalSolution.timeTrajectory_, controllerDataPtrArray); + primalSolution.controllerPtr_.reset(new FeedforwardController(std::move(controller))); + break; + } + case ocs2_msgs::msg::MPCFlattenedController::CONTROLLER_LINEAR: { + auto controller = LinearController::unFlatten(stateDim, inputDim, primalSolution.timeTrajectory_, controllerDataPtrArray); + primalSolution.controllerPtr_.reset(new LinearController(std::move(controller))); + break; + } + default: + throw std::runtime_error("[MRT_ROS_Interface::readPolicyMsg] Unknown controllerType!"); + } + } + + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + void MRT_ROS_Interface::mpcPolicyCallback( + const ocs2_msgs::msg::MPCFlattenedController::SharedPtr msg) { + // read new policy and command from msg + std::unique_ptr commandPtr(new CommandData); + std::unique_ptr primalSolutionPtr(new PrimalSolution); + std::unique_ptr performanceIndicesPtr(new PerformanceIndex); + readPolicyMsg(*msg, *commandPtr, *primalSolutionPtr, *performanceIndicesPtr); + + this->moveToBuffer(std::move(commandPtr), std::move(primalSolutionPtr), std::move(performanceIndicesPtr)); } -} - -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void MRT_ROS_Interface::mpcPolicyCallback(const ocs2_msgs::mpc_flattened_controller::ConstPtr& msg) { - // read new policy and command from msg - std::unique_ptr commandPtr(new CommandData); - std::unique_ptr primalSolutionPtr(new PrimalSolution); - std::unique_ptr performanceIndicesPtr(new PerformanceIndex); - readPolicyMsg(*msg, *commandPtr, *primalSolutionPtr, *performanceIndicesPtr); - - this->moveToBuffer(std::move(commandPtr), std::move(primalSolutionPtr), std::move(performanceIndicesPtr)); -} - -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void MRT_ROS_Interface::shutdownNodes() { + + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + void MRT_ROS_Interface::shutdownNodes() { #ifdef PUBLISH_THREAD - ROS_INFO_STREAM("Shutting down workers ..."); + RCLCPP_INFO_STREAM(LOGGER, "Shutting down workers ..."); - shutdownPublisher(); + shutdownPublisher(); - ROS_INFO_STREAM("All workers are shut down."); + RCLCPP_INFO_STREAM(LOGGER, "All workers are shut down."); #endif - // clean up callback queue - mrtCallbackQueue_.clear(); - mpcPolicySubscriber_.shutdown(); + // clean up callback queue + // mrtCallbackQueue_.clear(); + // mpcPolicySubscriber_.shutdown(); - // shutdown publishers - mpcObservationPublisher_.shutdown(); -} + // shutdown publishers + // mpcObservationPublisher_.shutdown(); + } -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void MRT_ROS_Interface::shutdownPublisher() { - std::unique_lock lk(publisherMutex_); - terminateThread_ = true; - lk.unlock(); + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + void MRT_ROS_Interface::shutdownPublisher() { + std::unique_lock lk(publisherMutex_); + terminateThread_ = true; + lk.unlock(); - msgReady_.notify_all(); + msgReady_.notify_all(); - if (publisherWorker_.joinable()) { - publisherWorker_.join(); + if (publisherWorker_.joinable()) { + publisherWorker_.join(); + } } -} - -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void MRT_ROS_Interface::spinMRT() { - mrtCallbackQueue_.callOne(); -}; - -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void MRT_ROS_Interface::launchNodes(ros::NodeHandle& nodeHandle) { - this->reset(); - - // display - ROS_INFO_STREAM("MRT node is setting up ..."); - - // observation publisher - mpcObservationPublisher_ = nodeHandle.advertise(topicPrefix_ + "_mpc_observation", 1); - - // policy subscriber - auto ops = ros::SubscribeOptions::create( - topicPrefix_ + "_mpc_policy", // topic name - 1, // queue length - boost::bind(&MRT_ROS_Interface::mpcPolicyCallback, this, boost::placeholders::_1), // callback - ros::VoidConstPtr(), // tracked object - &mrtCallbackQueue_ // pointer to callback queue object - ); - ops.transport_hints = mrtTransportHints_; - mpcPolicySubscriber_ = nodeHandle.subscribe(ops); - - // MPC reset service client - mpcResetServiceClient_ = nodeHandle.serviceClient(topicPrefix_ + "_mpc_reset"); - - // display + + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + void MRT_ROS_Interface::spinMRT() { + rclcpp::spin_some(node_); + // mrtCallbackQueue_.callOne(); + }; + + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + void MRT_ROS_Interface::launchNodes(rclcpp::Node::SharedPtr& nodeHandle) { + this->reset(); + + node_ = nodeHandle; + + // display + RCLCPP_INFO_STREAM(LOGGER, "MRT node is setting up ..."); + + // observation publisher + mpcObservationPublisher_ = nodeHandle->create_publisher( + topicPrefix_ + "_mpc_observation", 1); + + // policy subscriber + mpcPolicySubscriber_ = nodeHandle->create_subscription( + topicPrefix_ + "_mpc_policy", 1, + std::bind(&MRT_ROS_Interface::mpcPolicyCallback, this, + std::placeholders::_1)); + + // MPC reset service client + mpcResetServiceClient_ = nodeHandle->create_client(topicPrefix_ + "_mpc_reset"); + // display #ifdef PUBLISH_THREAD - ROS_INFO_STREAM("Publishing MRT messages on a separate thread."); + RCLCPP_INFO_STREAM(LOGGER, "Publishing MRT messages on a separate thread."); #endif - ROS_INFO_STREAM("MRT node is ready."); + RCLCPP_INFO_STREAM(LOGGER, "MRT node is ready."); - spinMRT(); -} + spinMRT(); + } } // namespace ocs2 diff --git a/ocs2_ros_interfaces/src/multiplot/MultiplotRemap.cpp b/ocs2_ros_interfaces/src/multiplot/MultiplotRemap.cpp index eca6fda8d..43e70c112 100644 --- a/ocs2_ros_interfaces/src/multiplot/MultiplotRemap.cpp +++ b/ocs2_ros_interfaces/src/multiplot/MultiplotRemap.cpp @@ -28,37 +28,40 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ #include - -#include -#include +#include #include -#include -#include +#include +#include namespace { class MultiplotRemap { public: /** Constructor */ - explicit MultiplotRemap(const std::string& mpcPolicyTopicName, ::ros::NodeHandle& nodeHandle) { + explicit MultiplotRemap(const std::string& mpcPolicyTopicName, ::rclcpp::Node::SharedPtr& nodeHandle) { + + // rclcpp::QoS qos(rmw_qos_profile_sensor_data); mpcPolicySubscriber_ = - nodeHandle.subscribe(mpcPolicyTopicName, 1, &MultiplotRemap::mpcPoicyCallback, this, ::ros::TransportHints().tcpNoDelay()); - mpcPerformanceIndicesPublisher_ = nodeHandle.advertise("mpc_performance_indices", 1); + nodeHandle->create_subscription( + mpcPolicyTopicName, 1, std::bind(&MultiplotRemap::mpcPoicyCallback, this, std::placeholders::_1)); + + mpcPerformanceIndicesPublisher_ = nodeHandle->create_publisher( + "mpc_performance_indices", 1); } /** Default deconstructor */ ~MultiplotRemap() = default; - private: - void mpcPoicyCallback(const ocs2_msgs::mpc_flattened_controller::ConstPtr& policyMsg) { - mpcPerformanceIndicesPublisher_.publish(policyMsg->performanceIndices); +private: + void mpcPoicyCallback(const ocs2_msgs::msg::MPCFlattenedController::SharedPtr policyMsg) { + mpcPerformanceIndicesPublisher_->publish(policyMsg->performance_indices); } // publishers and subscribers - ::ros::Subscriber mpcPolicySubscriber_; - ::ros::Publisher mpcPerformanceIndicesPublisher_; + ::rclcpp::Subscription::SharedPtr mpcPolicySubscriber_; + ::rclcpp::Publisher::SharedPtr mpcPerformanceIndicesPublisher_; }; } // unnamed namespace @@ -66,17 +69,17 @@ class MultiplotRemap { int main(int argc, char** argv) { // mpc policy topic name std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + programArgs = ::rclcpp::remove_ros_arguments(argc, argv); if (programArgs.size() <= 1) { throw std::runtime_error("MPC policy topic name is not specified!"); } const std::string mpcPolicyTopicName = std::string(programArgs[1]); - ::ros::init(argc, argv, "multiplot_remap"); - ::ros::NodeHandle nodeHandle; + ::rclcpp::init(argc, argv); + auto nodeHandle = std::make_shared<::rclcpp::Node>("multiplot_remap"); MultiplotRemap multiplotRemap(mpcPolicyTopicName, nodeHandle); - ::ros::spin(); + ::rclcpp::spin(nodeHandle); return 0; } diff --git a/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp b/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp index c7e880031..6ff374723 100644 --- a/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp +++ b/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp @@ -31,11 +31,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ros_interfaces/common/RosMsgConversions.h" -#include +#include // MPC messages -#include -#include +#include +#include namespace ocs2 { @@ -48,21 +48,21 @@ RosReferenceManager::RosReferenceManager(std::string topicPrefix, std::shared_pt /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void RosReferenceManager::subscribe(ros::NodeHandle& nodeHandle) { +void RosReferenceManager::subscribe(rclcpp::Node::SharedPtr& nodeHandle) { // ModeSchedule - auto modeScheduleCallback = [this](const ocs2_msgs::mode_schedule::ConstPtr& msg) { + auto modeScheduleCallback = [this](const ocs2_msgs::msg::ModeSchedule::SharedPtr msg) { auto modeSchedule = ros_msg_conversions::readModeScheduleMsg(*msg); referenceManagerPtr_->setModeSchedule(std::move(modeSchedule)); }; - modeScheduleSubscriber_ = nodeHandle.subscribe(topicPrefix_ + "_mode_schedule", 1, modeScheduleCallback); + modeScheduleSubscriber_ = nodeHandle->create_subscription(topicPrefix_ + "_mode_schedule", 1, modeScheduleCallback); // TargetTrajectories - auto targetTrajectoriesCallback = [this](const ocs2_msgs::mpc_target_trajectories::ConstPtr& msg) { + auto targetTrajectoriesCallback = [this](const ocs2_msgs::msg::MPCTargetTrajectories::SharedPtr msg) { auto targetTrajectories = ros_msg_conversions::readTargetTrajectoriesMsg(*msg); referenceManagerPtr_->setTargetTrajectories(std::move(targetTrajectories)); }; targetTrajectoriesSubscriber_ = - nodeHandle.subscribe(topicPrefix_ + "_mpc_target", 1, targetTrajectoriesCallback); + nodeHandle->create_subscription(topicPrefix_ + "_mpc_target", 1, targetTrajectoriesCallback); } } // namespace ocs2 diff --git a/ocs2_ros_interfaces/src/visualization/VisualizationHelpers.cpp b/ocs2_ros_interfaces/src/visualization/VisualizationHelpers.cpp index d95726ac5..81bc61bfe 100644 --- a/ocs2_ros_interfaces/src/visualization/VisualizationHelpers.cpp +++ b/ocs2_ros_interfaces/src/visualization/VisualizationHelpers.cpp @@ -31,9 +31,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { -std_msgs::ColorRGBA getColor(Color color, double alpha) { +std_msgs::msg::ColorRGBA getColor(Color color, double alpha) { const auto rgb = getRGB(color); - std_msgs::ColorRGBA colorMsg; + std_msgs::msg::ColorRGBA colorMsg; colorMsg.r = rgb[0]; colorMsg.g = rgb[1]; colorMsg.b = rgb[2]; @@ -41,24 +41,24 @@ std_msgs::ColorRGBA getColor(Color color, double alpha) { return colorMsg; } -void setVisible(visualization_msgs::Marker& marker) { +void setVisible(visualization_msgs::msg::Marker& marker) { marker.color.a = 1.0; } -void setInvisible(visualization_msgs::Marker& marker) { +void setInvisible(visualization_msgs::msg::Marker& marker) { marker.color.a = 0.001; // Rviz creates a warning when a is set to 0 } -std_msgs::Header getHeaderMsg(const std::string& frame_id, const ros::Time& timeStamp) { - std_msgs::Header header; +std_msgs::msg::Header getHeaderMsg(const std::string& frame_id, const rclcpp::Time& timeStamp) { + std_msgs::msg::Header header; header.frame_id = frame_id; header.stamp = timeStamp; return header; } -visualization_msgs::Marker getLineMsg(std::vector&& points, Color color, double lineWidth) { - visualization_msgs::Marker line; - line.type = visualization_msgs::Marker::LINE_STRIP; +visualization_msgs::msg::Marker getLineMsg(std::vector&& points, Color color, double lineWidth) { + visualization_msgs::msg::Marker line; + line.type = visualization_msgs::msg::Marker::LINE_STRIP; line.scale.x = lineWidth; line.color = getColor(color); line.points = std::move(points); @@ -66,24 +66,24 @@ visualization_msgs::Marker getLineMsg(std::vector&& points return line; } -geometry_msgs::Point getPointMsg(const Eigen::Vector3d& point) { - geometry_msgs::Point pointMsg; +geometry_msgs::msg::Point getPointMsg(const Eigen::Vector3d& point) { + geometry_msgs::msg::Point pointMsg; pointMsg.x = point.x(); pointMsg.y = point.y(); pointMsg.z = point.z(); return pointMsg; } -geometry_msgs::Vector3 getVectorMsg(const Eigen::Vector3d& vec) { - geometry_msgs::Vector3 vecMsg; +geometry_msgs::msg::Vector3 getVectorMsg(const Eigen::Vector3d& vec) { + geometry_msgs::msg::Vector3 vecMsg; vecMsg.x = vec.x(); vecMsg.y = vec.y(); vecMsg.z = vec.z(); return vecMsg; } -geometry_msgs::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientation) { - geometry_msgs::Quaternion orientationMsg; +geometry_msgs::msg::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientation) { + geometry_msgs::msg::Quaternion orientationMsg; orientationMsg.x = orientation.x(); orientationMsg.y = orientation.y(); orientationMsg.z = orientation.z(); @@ -91,9 +91,9 @@ geometry_msgs::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientatio return orientationMsg; } -visualization_msgs::Marker getSphereMsg(const Eigen::Vector3d& point, Color color, double diameter) { - visualization_msgs::Marker sphere; - sphere.type = visualization_msgs::Marker::SPHERE; +visualization_msgs::msg::Marker getSphereMsg(const Eigen::Vector3d& point, Color color, double diameter) { + visualization_msgs::msg::Marker sphere; + sphere.type = visualization_msgs::msg::Marker::SPHERE; sphere.pose.position = getPointMsg(point); sphere.pose.orientation = getOrientationMsg({1., 0., 0., 0.}); sphere.scale.x = diameter; @@ -103,10 +103,10 @@ visualization_msgs::Marker getSphereMsg(const Eigen::Vector3d& point, Color colo return sphere; } -visualization_msgs::Marker getPlaneMsg(const Eigen::Vector3d& point, const Eigen::Quaterniond& orientation, Color color, double width, +visualization_msgs::msg::Marker getPlaneMsg(const Eigen::Vector3d& point, const Eigen::Quaterniond& orientation, Color color, double width, double length, double thickness) { - visualization_msgs::Marker plane; - plane.type = visualization_msgs::Marker::CUBE; + visualization_msgs::msg::Marker plane; + plane.type = visualization_msgs::msg::Marker::CUBE; plane.pose.position = getPointMsg(point); plane.pose.orientation = getOrientationMsg(orientation); plane.scale.x = length; @@ -116,17 +116,17 @@ visualization_msgs::Marker getPlaneMsg(const Eigen::Vector3d& point, const Eigen return plane; } -visualization_msgs::Marker getArrowToPointMsg(const Eigen::Vector3d& vec, const Eigen::Vector3d& point, Color color) { +visualization_msgs::msg::Marker getArrowToPointMsg(const Eigen::Vector3d& vec, const Eigen::Vector3d& point, Color color) { return getArrowBetweenPointsMsg(point - vec, point, color); } -visualization_msgs::Marker getArrowAtPointMsg(const Eigen::Vector3d& vec, const Eigen::Vector3d& point, Color color) { +visualization_msgs::msg::Marker getArrowAtPointMsg(const Eigen::Vector3d& vec, const Eigen::Vector3d& point, Color color) { return getArrowBetweenPointsMsg(point, point + vec, color); } -visualization_msgs::Marker getArrowBetweenPointsMsg(const Eigen::Vector3d& start, const Eigen::Vector3d& end, Color color) { - visualization_msgs::Marker arrow; - arrow.type = visualization_msgs::Marker::ARROW; +visualization_msgs::msg::Marker getArrowBetweenPointsMsg(const Eigen::Vector3d& start, const Eigen::Vector3d& end, Color color) { + visualization_msgs::msg::Marker arrow; + arrow.type = visualization_msgs::msg::Marker::ARROW; arrow.scale.x = 0.01; // shaft diameter arrow.scale.y = 0.02; // arrow-head diameter arrow.scale.z = 0.06; // arrow-head length @@ -138,7 +138,7 @@ visualization_msgs::Marker getArrowBetweenPointsMsg(const Eigen::Vector3d& start return arrow; } -visualization_msgs::Marker getFootMarker(const Eigen::Vector3d& position, bool contactFlag, Color color, double diameter, +visualization_msgs::msg::Marker getFootMarker(const Eigen::Vector3d& position, bool contactFlag, Color color, double diameter, double liftedAlpha) { auto footMarker = getSphereMsg(position, color, diameter); if (!contactFlag) { @@ -148,7 +148,7 @@ visualization_msgs::Marker getFootMarker(const Eigen::Vector3d& position, bool c return footMarker; } -visualization_msgs::Marker getForceMarker(const Eigen::Vector3d& force, const Eigen::Vector3d& position, bool contactFlag, Color color, +visualization_msgs::msg::Marker getForceMarker(const Eigen::Vector3d& force, const Eigen::Vector3d& position, bool contactFlag, Color color, double forceScale) { auto forceMarker = getArrowToPointMsg(force / forceScale, position, color); forceMarker.ns = "EE Forces"; diff --git a/ocs2_ros_interfaces/test/test_custom_callback_queue.cpp b/ocs2_ros_interfaces/test/test_custom_callback_queue.cpp index e7446261c..a2d36ea94 100644 --- a/ocs2_ros_interfaces/test/test_custom_callback_queue.cpp +++ b/ocs2_ros_interfaces/test/test_custom_callback_queue.cpp @@ -53,12 +53,12 @@ int main(int argc, char* argv[]) { ros::init(argc, argv, "my_node"); // Publisher - ros::NodeHandle nh_pub; + rclcpp::Node::SharedPtr nh_pub; size_t publish_queue_size = 1000; ros::Publisher chatter_pub = nh_pub.advertise("chatter", publish_queue_size); // Subscriber - ros::NodeHandle nh_sub; + rclcpp::Node::SharedPtr nh_sub; nh_sub.setCallbackQueue(&my_queue); size_t subscribe_queue_size = 1000; diff --git a/ocs2_sqp/blasfeo_catkin/CMakeLists.txt b/ocs2_sqp/blasfeo_catkin/CMakeLists.txt index d6f371272..c811e5205 100644 --- a/ocs2_sqp/blasfeo_catkin/CMakeLists.txt +++ b/ocs2_sqp/blasfeo_catkin/CMakeLists.txt @@ -1,20 +1,18 @@ cmake_minimum_required(VERSION 3.14) project(blasfeo_catkin) -find_package(catkin REQUIRED) +set(CMAKE_ASM_COMPILER "as") + +find_package(ament_cmake REQUIRED) include(FetchContent) # Define directories -set(BLASFEO_DEVEL_PREFIX ${CATKIN_DEVEL_PREFIX} CACHE STRING "Blasfeo install path") -set(BLASFEO_INCLUDE_DIR ${BLASFEO_DEVEL_PREFIX}/include) -set(BLASFEO_LIB_DIR ${BLASFEO_DEVEL_PREFIX}/lib) + set(BLASFEO_DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/download) set(BLASFEO_BUILD_DIR ${CMAKE_CURRENT_BINARY_DIR}/build) # Create directories if they do not exist -file(MAKE_DIRECTORY ${BLASFEO_INCLUDE_DIR}) -file(MAKE_DIRECTORY ${BLASFEO_LIB_DIR}) file(MAKE_DIRECTORY ${BLASFEO_DOWNLOAD_DIR}) file(MAKE_DIRECTORY ${BLASFEO_BUILD_DIR}) @@ -35,19 +33,8 @@ FetchContent_Declare(blasfeoDownload ) FetchContent_MakeAvailable(blasfeoDownload) -# Copy header to where catkin expects them -file(GLOB_RECURSE HEADERS "${BLASFEO_DOWNLOAD_DIR}/include/*.h") -foreach(HEADER_FILE ${HEADERS}) - message(STATUS "FOUND HEADER: " ${HEADER_FILE}) - file(COPY ${HEADER_FILE} DESTINATION ${BLASFEO_INCLUDE_DIR}) -endforeach() +# Propagate dependencies -# Install the library where catkin expects them -set_target_properties(blasfeo PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${BLASFEO_LIB_DIR}) +ament_export_libraries(blasfeo) -# Propagate dependencies -catkin_package( - INCLUDE_DIRS ${BLASFEO_INCLUDE_DIR} - LIBRARIES blasfeo - CFG_EXTRAS blasfeo-extras.cmake -) +ament_package() diff --git a/ocs2_sqp/blasfeo_catkin/package.xml b/ocs2_sqp/blasfeo_catkin/package.xml index b7059c424..e4becc162 100644 --- a/ocs2_sqp/blasfeo_catkin/package.xml +++ b/ocs2_sqp/blasfeo_catkin/package.xml @@ -7,5 +7,5 @@ See package - catkin + ament_cmake diff --git a/ocs2_sqp/hpipm_catkin/CMakeLists.txt b/ocs2_sqp/hpipm_catkin/CMakeLists.txt index ed6443e6e..b35f75ecb 100644 --- a/ocs2_sqp/hpipm_catkin/CMakeLists.txt +++ b/ocs2_sqp/hpipm_catkin/CMakeLists.txt @@ -2,26 +2,28 @@ cmake_minimum_required(VERSION 3.14) project(hpipm_catkin) ## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - blasfeo_catkin - ocs2_core - ocs2_qp_solver -) +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_qp_solver REQUIRED) +find_package(blasfeo REQUIRED) +find_package(OpenMP REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS + system + filesystem + log + log_setup +) + include(FetchContent) # Define directories -set(HPIPM_DEVEL_PREFIX ${CATKIN_DEVEL_PREFIX} CACHE STRING "HPIPM install path") -set(HPIPM_INCLUDE_DIR ${HPIPM_DEVEL_PREFIX}/include) -set(HPIPM_LIB_DIR ${HPIPM_DEVEL_PREFIX}/lib) set(HPIPM_DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/download) set(HPIPM_BUILD_DIR ${CMAKE_CURRENT_BINARY_DIR}/build) # Create directories if they do not exist -file(MAKE_DIRECTORY ${HPIPM_INCLUDE_DIR}) -file(MAKE_DIRECTORY ${HPIPM_LIB_DIR}) file(MAKE_DIRECTORY ${HPIPM_DOWNLOAD_DIR}) file(MAKE_DIRECTORY ${HPIPM_BUILD_DIR}) @@ -45,25 +47,6 @@ FetchContent_Declare(hpipmDownload ) FetchContent_MakeAvailable(hpipmDownload) -# Copy header to where catkin expects them -file(GLOB_RECURSE HEADERS "${HPIPM_DOWNLOAD_DIR}/include/*.h") -foreach(HEADER_FILE ${HEADERS}) - message(STATUS "FOUND HEADER: " ${HEADER_FILE}) - file(COPY ${HEADER_FILE} DESTINATION ${HPIPM_INCLUDE_DIR}) -endforeach() - -# Install the library where catkin expects them -set_target_properties(hpipm PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${HPIPM_LIB_DIR}) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS include ${HPIPM_INCLUDE_DIR} - CATKIN_DEPENDS blasfeo_catkin - LIBRARIES ${PROJECT_NAME} hpipm -) ########### ## Build ## @@ -71,8 +54,7 @@ catkin_package( include_directories( include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} + blasfeo ) # Hpipm interface @@ -81,11 +63,13 @@ add_library(${PROJECT_NAME} src/HpipmInterfaceSettings.cpp src/OcpSize.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} + +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + blasfeo ) + target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} hpipm ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) @@ -110,25 +94,32 @@ endif(cmake_clang_tools_FOUND) ############# install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +ament_export_include_directories(include) +ament_export_libraries(hpipm) +ament_export_libraries(${PROJECT_NAME}) + +ament_package() ############# ## Testing ## ############# -catkin_add_gtest(test_${PROJECT_NAME} - test/testHpipmInterface.cpp -) -add_dependencies(test_${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(test_${PROJECT_NAME} - ${PROJECT_NAME} - ${catkin_LIBRARIES} - hpipm - gtest_main -) +#catkin_add_gtest(test_${PROJECT_NAME} +# test/testHpipmInterface.cpp +#) +#add_dependencies(test_${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +#target_link_libraries(test_${PROJECT_NAME} +# ${PROJECT_NAME} +# ${catkin_LIBRARIES} +# hpipm +# gtest_main +#) diff --git a/ocs2_sqp/hpipm_catkin/package.xml b/ocs2_sqp/hpipm_catkin/package.xml index 33f411ebe..7cca3a1dc 100644 --- a/ocs2_sqp/hpipm_catkin/package.xml +++ b/ocs2_sqp/hpipm_catkin/package.xml @@ -7,7 +7,7 @@ See package - catkin + ament_cmake ocs2_core ocs2_qp_solver blasfeo_catkin diff --git a/ocs2_sqp/ocs2_sqp/CMakeLists.txt b/ocs2_sqp/ocs2_sqp/CMakeLists.txt index 7af8598c8..476b362c0 100644 --- a/ocs2_sqp/ocs2_sqp/CMakeLists.txt +++ b/ocs2_sqp/ocs2_sqp/CMakeLists.txt @@ -1,7 +1,16 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_sqp) -set(CATKIN_PACKAGE_DEPENDENCIES +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_qp_solver REQUIRED) +find_package(blasfeo_catkin REQUIRED) +find_package(hpipm_catkin REQUIRED) +find_package(OpenMP REQUIRED) + +set(AMENT_DEPENDENCIES ocs2_core ocs2_ddp ocs2_mpc @@ -11,44 +20,20 @@ set(CATKIN_PACKAGE_DEPENDENCIES hpipm_catkin ) -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) find_package(Boost REQUIRED COMPONENTS system filesystem + log + log_setup ) find_package(Eigen3 3.3 REQUIRED NO_MODULE) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - LIBRARIES - ${PROJECT_NAME} - DEPENDS - Boost -) - ########### ## Build ## ########### -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} -) - # Multiple shooting solver library add_library(${PROJECT_NAME} src/ConstraintProjection.cpp @@ -59,13 +44,16 @@ add_library(${PROJECT_NAME} src/MultipleShootingTranscription.cpp src/TimeDiscretization.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} + +ament_target_dependencies(${PROJECT_NAME} + ${AMENT_DEPENDENCIES} ) + target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) ######################### @@ -86,21 +74,39 @@ endif(cmake_clang_tools_FOUND) ############# ## Install ## ############# -install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install(DIRECTORY include/ + DESTINATION include +) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_export_dependencies(ocs2_core) +ament_export_dependencies(ocs2_oc) +ament_export_dependencies(ocs2_mpc) +ament_export_dependencies(ocs2_ddp) +ament_export_dependencies(ocs2_qp_solver) +ament_export_dependencies(blasfeo_catkin) +ament_export_dependencies(hpipm_catkin) ############# ## Testing ## ############# +find_package(ament_lint_auto REQUIRED) +find_package(ament_cmake_gtest REQUIRED) +ament_lint_auto_find_test_dependencies() -catkin_add_gtest(test_${PROJECT_NAME} +ament_add_gtest(test_${PROJECT_NAME} test/testCircularKinematics.cpp test/testDiscretization.cpp test/testProjection.cpp @@ -109,9 +115,12 @@ catkin_add_gtest(test_${PROJECT_NAME} test/testUnconstrained.cpp test/testValuefunction.cpp ) -add_dependencies(test_${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main -) \ No newline at end of file +) + + +ament_package() + diff --git a/ocs2_sqp/ocs2_sqp/package.xml b/ocs2_sqp/ocs2_sqp/package.xml index 132839f88..31ea2bc1d 100644 --- a/ocs2_sqp/ocs2_sqp/package.xml +++ b/ocs2_sqp/ocs2_sqp/package.xml @@ -8,7 +8,7 @@ TODO - catkin + ament_cmake ocs2_core ocs2_ddp ocs2_mpc @@ -17,4 +17,8 @@ blasfeo_catkin hpipm_catkin + + + ament_cmake + diff --git a/ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt b/ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt index 800dfc775..b82b8c968 100644 --- a/ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt +++ b/ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt @@ -1,38 +1,19 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_qp_solver) -find_package(catkin REQUIRED COMPONENTS - ocs2_core - ocs2_oc -) -find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(OpenMP REQUIRED) -# Generate compile_commands.json for clang tools -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - test/include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ocs2_core - ocs2_oc - DEPENDS -) +find_package(Eigen3 3.3 REQUIRED NO_MODULE) -include_directories( - include - test/include - ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} +find_package(Boost REQUIRED COMPONENTS + filesystem + system + log + log_setup ) # Declare a C++ library @@ -42,8 +23,13 @@ add_library(${PROJECT_NAME} src/QpSolver.cpp src/QpTrajectories.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_oc ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) @@ -66,30 +52,53 @@ endif(cmake_clang_tools_FOUND) ## Install ## ############# -install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) -install(DIRECTORY test/include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_export_include_directories(include) +ament_export_dependencies(ocs2_core) +ament_export_dependencies(ocs2_oc) + ############# ## Testing ## ############# -catkin_add_gtest(test_${PROJECT_NAME} +if(BUILD_TESTING) + +# Include linting tests +find_package(ament_lint_auto REQUIRED) +find_package(ament_cmake_gtest REQUIRED) +ament_lint_auto_find_test_dependencies() + +ament_add_gtest(test_${PROJECT_NAME} test/testDiscreteTranscription.cpp test/testQpSolver.cpp test/testOcs2QpSolver.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} + ${Boost_LIBRARIES} gtest_main ) +target_include_directories(test_${PROJECT_NAME} PUBLIC + $ + $ +) +endif(BUILD_TESTING) + +ament_package() + diff --git a/ocs2_test_tools/ocs2_qp_solver/package.xml b/ocs2_test_tools/ocs2_qp_solver/package.xml index 440c5ce43..caa0007af 100644 --- a/ocs2_test_tools/ocs2_qp_solver/package.xml +++ b/ocs2_test_tools/ocs2_qp_solver/package.xml @@ -10,9 +10,12 @@ BSD - catkin + ament_cmake cmake_clang_tools ocs2_core ocs2_oc + + ament_cmake + diff --git a/ocs2_thirdparty/CMakeLists.txt b/ocs2_thirdparty/CMakeLists.txt index 9217b8724..9f346465e 100644 --- a/ocs2_thirdparty/CMakeLists.txt +++ b/ocs2_thirdparty/CMakeLists.txt @@ -1,12 +1,12 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.10.2) project(ocs2_thirdparty) -find_package(catkin) +find_package(ament_cmake REQUIRED) -catkin_package( - INCLUDE_DIRS - include -) install(DIRECTORY include/ - DESTINATION "${CATKIN_PACKAGE_INCLUDE_DESTINATION}/../") + DESTINATION include +) +ament_export_include_directories(include) + +ament_package() diff --git a/ocs2_thirdparty/package.xml b/ocs2_thirdparty/package.xml index 01135f0ec..1578ae8e3 100644 --- a/ocs2_thirdparty/package.xml +++ b/ocs2_thirdparty/package.xml @@ -10,8 +10,9 @@ Check individual license files for each library - catkin - cmake_modules - + ament_cmake + + ament_cmake +