diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 0f3ac9433..40f687991 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -1,4 +1,4 @@ -name: Build and Test OCS2 +name: Build and Test OCS2 (ROS2 Jazzy) on: push: @@ -8,71 +8,106 @@ on: jobs: build: - - # build both Debug and Release mode strategy: fail-fast: false matrix: - build_type: [ Debug, Release ] + include: + - build_type: Release + build_testing: "ON" + run_tests: true + packages_up_to: "" + # Full Debug + tests is too slow for GitHub-hosted runners; still build Debug + # (without tests) to catch compilation issues. Also only build up to core ROS + # interfaces to keep disk usage below GitHub runner limits. + - build_type: Debug + build_testing: "OFF" + run_tests: false + packages_up_to: "ocs2_ros_interfaces" - # environment: regular Ubuntu with a vanilla ROS container runs-on: ubuntu-latest container: - image: ros:noetic + image: ros:jazzy-ros-base + + env: + DEBIAN_FRONTEND: noninteractive steps: - name: Environment Info run: | pwd uname -r - lsb_release -a + cat /etc/os-release - name: System deps run: | - apt-get update - apt-get install -y git ninja-build liburdfdom-dev liboctomap-dev libassimp-dev checkinstall wget rsync + apt-get update && apt-get install -y --no-install-recommends \ + build-essential cmake git ninja-build \ + curl ca-certificates gnupg lsb-release \ + python3-colcon-common-extensions python3-rosdep python3-pip \ + python3-dev pybind11-dev \ + libeigen3-dev libboost-all-dev libglpk-dev \ + libgmp-dev libmpfr-dev libcgal-dev libopencv-dev libpcl-dev \ + liburdfdom-dev \ + ros-jazzy-eigen3-cmake-module \ + ros-jazzy-hpp-fcl \ + ros-jazzy-grid-map \ + ros-jazzy-xacro ros-jazzy-robot-state-publisher ros-jazzy-rviz2 \ + && rm -rf /var/lib/apt/lists/* - - uses: actions/checkout@v2 - with: - path: src/ocs2 + - name: Install Pinocchio (robotpkg) + run: | + install -d -m 0755 /etc/apt/keyrings + curl -fsSL http://robotpkg.openrobots.org/packages/debian/robotpkg.asc | tee /etc/apt/keyrings/robotpkg.asc >/dev/null + echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(. /etc/os-release && echo $VERSION_CODENAME) robotpkg" > /etc/apt/sources.list.d/robotpkg.list + apt-get update && apt-get install -y --no-install-recommends \ + robotpkg-pinocchio robotpkg-coal \ + && rm -rf /var/lib/apt/lists/* - - name: Rosdep + - name: Set OpenRobots env run: | - rosdep update - rosdep install --from-paths src --ignore-src -r -y + echo "CMAKE_PREFIX_PATH=/opt/openrobots:${CMAKE_PREFIX_PATH}" >> $GITHUB_ENV + echo "LD_LIBRARY_PATH=/opt/openrobots/lib:${LD_LIBRARY_PATH}" >> $GITHUB_ENV - - name: Checkout dependencies + - name: Pin setuptools < 80 run: | - git clone --recurse-submodules https://github.com/leggedrobotics/pinocchio.git src/pinocchio - git clone --recurse-submodules https://github.com/leggedrobotics/hpp-fcl.git src/hpp-fcl - git clone https://github.com/leggedrobotics/ocs2_robotic_assets.git src/ocs2_robotic_assets - - - name: Install RaiSim + pip3 install --no-cache-dir --break-system-packages 'setuptools<80' + + - name: Checkout + uses: actions/checkout@v4 + with: + path: src/ocs2 + + - name: Checkout dependencies run: | - git clone --depth 1 https://github.com/raisimTech/raisimLib.git -b v1.1.01 src/raisim_tech - cd src/raisim_tech - mkdir build - cd build - cmake .. -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)") - make -j4 && checkinstall - - - name: Install ONNX Runtime + git clone --depth 1 --branch ros2 --single-branch https://github.com/leggedrobotics/ocs2_robotic_assets.git src/ocs2_robotic_assets + git clone --depth 1 --filter=blob:none --sparse --branch ros2 --single-branch https://github.com/leggedrobotics/elevation_mapping_cupy.git src/elevation_mapping_cupy + cd src/elevation_mapping_cupy + git sparse-checkout set plane_segmentation + + - name: Rosdep + shell: bash run: | - wget https://github.com/microsoft/onnxruntime/releases/download/v1.7.0/onnxruntime-linux-x64-1.7.0.tgz -P tmp/microsoft - tar xf tmp/microsoft/onnxruntime-linux-x64-1.7.0.tgz -C tmp/microsoft - rsync -a tmp/microsoft/onnxruntime-linux-x64-1.7.0/include/ /usr/local/include/onnxruntime - rsync -a tmp/microsoft/onnxruntime-linux-x64-1.7.0/lib/ /usr/local/lib - rsync -a src/ocs2/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/ /usr/local/share/onnxruntime + if [ ! -f /etc/ros/rosdep/sources.list.d/20-default.list ]; then rosdep init; fi + rosdep update + source /opt/ros/jazzy/setup.bash + rosdep install --from-paths src --ignore-src -r -y --skip-keys pinocchio - name: Build (${{ matrix.build_type }}) shell: bash run: | - source /opt/ros/noetic/setup.bash - catkin_make_isolated --use-ninja --merge --only-pkg-with-deps ocs2 --cmake-args -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} + source /opt/ros/jazzy/setup.bash + if [ -n "${{ matrix.packages_up_to }}" ]; then + colcon build --packages-up-to ${{ matrix.packages_up_to }} --event-handlers console_direct+ \ + --cmake-args -GNinja -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} -DBUILD_TESTING=${{ matrix.build_testing }} + else + colcon build --event-handlers console_direct+ \ + --cmake-args -GNinja -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} -DBUILD_TESTING=${{ matrix.build_testing }} + fi - name: Test + if: ${{ matrix.run_tests }} shell: bash run: | - source devel_isolated/setup.bash - catkin_make_isolated --use-ninja --merge --only-pkg-with-deps ocs2 --catkin-make-args run_tests - catkin_test_results + source install/setup.bash + colcon test --event-handlers console_direct+ + colcon test-result --verbose diff --git a/README.md b/README.md index c7ea94d1b..b48eaa070 100644 --- a/README.md +++ b/README.md @@ -1,15 +1,66 @@ -# OCS2 Toolbox +# OCS2 Toolbox (ROS 2) -## Summary -OCS2 is a C++ toolbox tailored for Optimal Control for Switched Systems (OCS2). The toolbox provides an efficient implementation of the following algorith - -* SLQ: Continuous-time domin DDP -* iLQR: Discrete-time domain DDP -* SQP: Multiple-shooting algorithm based on HPIPM -* PISOC: Path integral stochatic optimal control +This branch (`ros2`) ports OCS2 from ROS 1/catkin to **ROS 2 (colcon/ament)** and currently targets **Ubuntu 24.04 + ROS 2 Jazzy**. +For the ROS 1 version, see branch `main`. ![legged-robot](https://leggedrobotics.github.io/ocs2/_static/gif/legged_robot.gif) -OCS2 handles general path constraints through Augmented Lagrangian or relaxed barrier methods. To facilitate the application of OCS2 in robotic tasks, it provides the user with additional tools to set up the system dynamics (such as kinematic or dynamic models) and cost/constraints (such as self-collision avoidance and end-effector tracking) from a URDF model. The library also provides an automatic differentiation tool to calculate derivatives of the system dynamics, constraints, and cost. To facilitate its deployment on robotic platforms, the OCS2 provides tools for ROS interfaces. The toolbox’s efficient and numerically stable implementations in conjunction with its user-friendly interface have paved the way for employing it on numerous robotic applications with limited onboard computation power. +OCS2 (**O**ptimal **C**ontrol for **S**witched **S**ystems) is a C++ toolbox for formulating and solving nonlinear optimal control problems, with an emphasis on real-time **Model Predictive Control (MPC)** for robotics. + +OCS2 handles general path constraints through Augmented Lagrangian or relaxed barrier methods. To facilitate the application of OCS2 in robotic tasks, it provides tools to set up the system dynamics (such as kinematic or dynamic models) and cost/constraints (such as self-collision avoidance and end-effector tracking) from a URDF model. The library also provides an automatic differentiation tool to calculate derivatives of the system dynamics, constraints, and cost. To facilitate deployment on robotic platforms, OCS2 provides tools for ROS interfaces. + +## What’s included + +- **Optimal control solvers** + - **SLQ**: continuous-time constrained DDP + - **iLQR**: discrete-time constrained DDP + - **SQP**: multiple-shooting SQP (QP subproblems via HPIPM/BLASFEO) + - **SLP**: sequential linear programming (PIPG) + - **IPM**: multiple-shooting nonlinear interior-point method +- **Robotics tooling**: URDF/Pinocchio integration, kinematics, centroidal models, and self-collision constraints (HPP-FCL). +- **ROS integration**: messages, nodes, and visualization/plotting tools for deploying MPC on robots. +- **Robotic examples**: end-to-end MPC examples (and ROS wrappers) for double integrator, cartpole, ballbot, quadrotor, mobile manipulator, legged robot, and more. + +## Port status (ROS 2 Jazzy) + +- This branch replaces catkin with ament/colcon across the workspace. +- `blasfeo_catkin` / `hpipm_catkin` keep their names but are ROS 2 `ament_cmake` packages. +- `ocs2_mpcnet` and `ocs2_raisim` are currently not ported and are ignored via `COLCON_IGNORE`. +- `rqt_multiplot` is not released for Jazzy on Ubuntu 24.04; multiplot launch files are optional. + +## Installation + +Follow the branch-specific instructions in `installation.md` (dependencies, Pinocchio on Jazzy, and colcon build): +- [`installation.md`](installation.md) + +Optional Docker environment: +- [`docker/README.md`](docker/README.md) + +## Run an example + +After building and sourcing your workspace: + +```bash +ros2 launch ocs2_ballbot_ros ballbot_mpc_mrt.launch.py +``` + +## Documentation + +Project documentation is hosted at: +- https://leggedrobotics.github.io/ocs2/ + +Note: the online docs still describe the ROS 1/catkin workflow; the solver concepts and APIs are largely identical, but build/launch instructions differ on this branch. + +## Citing OCS2 + +```latex +@misc{OCS2, + title = {{OCS2}: An open source library for Optimal Control of Switched Systems}, + note = {[Online]. Available: \url{https://github.com/leggedrobotics/ocs2}}, + author = {Farbod Farshidian and others} +} +``` + +## License -For more information refer to the project's [Documentation Page](https://leggedrobotics.github.io/ocs2/) +BSD 3-Clause, see `LICENCE.txt`. diff --git a/docker/Dockerfile.jazzy b/docker/Dockerfile.jazzy new file mode 100644 index 000000000..fb9c42848 --- /dev/null +++ b/docker/Dockerfile.jazzy @@ -0,0 +1,61 @@ +FROM ros:jazzy-ros-base + +SHELL ["/bin/bash", "-c"] +ENV DEBIAN_FRONTEND=noninteractive + +# System deps needed by OCS2 (core + pinocchio + plane segmentation + examples). +RUN apt-get update && apt-get install -y --no-install-recommends \ + build-essential cmake git \ + curl ca-certificates gnupg lsb-release \ + python3-colcon-common-extensions python3-rosdep \ + python3-dev python3-pip pybind11-dev \ + libeigen3-dev libboost-all-dev libglpk-dev \ + libgmp-dev libmpfr-dev libcgal-dev libopencv-dev libpcl-dev \ + liburdfdom-dev \ + ros-jazzy-eigen3-cmake-module \ + ros-jazzy-hpp-fcl \ + ros-jazzy-grid-map \ + ros-jazzy-xacro ros-jazzy-robot-state-publisher ros-jazzy-rviz2 \ + && rm -rf /var/lib/apt/lists/* + +# Pinocchio on Jazzy: install from OpenRobots robotpkg (rosdep maps it to a non-existent ros-jazzy-pinocchio). +RUN install -d -m 0755 /etc/apt/keyrings && \ + curl -fsSL http://robotpkg.openrobots.org/packages/debian/robotpkg.asc | tee /etc/apt/keyrings/robotpkg.asc >/dev/null && \ + echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(. /etc/os-release && echo $VERSION_CODENAME) robotpkg" > /etc/apt/sources.list.d/robotpkg.list && \ + apt-get update && apt-get install -y --no-install-recommends \ + robotpkg-pinocchio robotpkg-coal \ + && rm -rf /var/lib/apt/lists/* + +ENV CMAKE_PREFIX_PATH=/opt/openrobots:${CMAKE_PREFIX_PATH} +ENV LD_LIBRARY_PATH=/opt/openrobots/lib:${LD_LIBRARY_PATH} + +# Pin setuptools < 80 for colcon --symlink-install compatibility +# See: https://github.com/colcon/colcon-core/issues/696 +RUN pip3 install --no-cache-dir --break-system-packages 'setuptools<80' + +# rosdep (optional but convenient). +RUN if [ ! -f /etc/ros/rosdep/sources.list.d/20-default.list ]; then rosdep init; fi && rosdep update + +WORKDIR /ws +RUN mkdir -p /ws/src + +# Copy OCS2 repo (build context should be the root of this repository). +COPY . /ws/src/ocs2 + +# Assets used by many examples/tests. +RUN git clone --depth 1 --branch ros2 --single-branch https://github.com/leggedrobotics/ocs2_robotic_assets.git /ws/src/ocs2_robotic_assets + +# Plane segmentation packages used by perceptive examples (convex_plane_decomposition*, grid_map_filters_rsl). +RUN git clone --depth 1 --filter=blob:none --sparse --branch ros2 --single-branch https://github.com/leggedrobotics/elevation_mapping_cupy.git /ws/src/elevation_mapping_cupy && \ + cd /ws/src/elevation_mapping_cupy && \ + git sparse-checkout set plane_segmentation + +# Install ROS deps from package.xml, then build. +RUN source /opt/ros/jazzy/setup.bash && \ + rosdep install --from-paths src --ignore-src -r -y --skip-keys pinocchio && \ + colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo + +RUN echo "source /opt/ros/jazzy/setup.bash" >> /root/.bashrc && \ + echo "source /ws/install/setup.bash" >> /root/.bashrc + +CMD ["bash"] diff --git a/docker/README.md b/docker/README.md new file mode 100644 index 000000000..eda4e6650 --- /dev/null +++ b/docker/README.md @@ -0,0 +1,16 @@ +## OCS2 Jazzy Docker + +Build: + +```bash +docker build -f docker/Dockerfile.jazzy -t ocs2:jazzy . +``` + +Run: + +```bash +docker run --rm -it --net=host ocs2:jazzy +``` + +Note: the Dockerfile clones `ocs2_robotic_assets` from its `ros2` branch. +It also sparse-checkouts the `plane_segmentation/` packages from `elevation_mapping_cupy` (branch `ros2`) for the perceptive examples. diff --git a/installation.md b/installation.md new file mode 100644 index 000000000..56dc6824e --- /dev/null +++ b/installation.md @@ -0,0 +1,111 @@ +# OCS2 on ROS 2 Jazzy (branch `ros2`) + +This branch ports OCS2 from ROS 1/catkin to ROS 2 Jazzy/colcon. + +## Prerequisites + +- Ubuntu 24.04 + ROS 2 Jazzy installed. +- C++17 compiler. +- `colcon` build tooling. + +## System Dependencies (Ubuntu apt) + +```bash +sudo apt update +sudo apt install -y \ + build-essential cmake git \ + python3-colcon-common-extensions python3-rosdep \ + python3-dev pybind11-dev \ + libeigen3-dev libboost-all-dev libglpk-dev \ + libgmp-dev libmpfr-dev libcgal-dev libopencv-dev libpcl-dev \ + liburdfdom-dev \ + ros-jazzy-eigen3-cmake-module \ + ros-jazzy-hpp-fcl \ + ros-jazzy-grid-map \ + ros-jazzy-xacro ros-jazzy-robot-state-publisher ros-jazzy-rviz2 +``` + +### Pinocchio (robotpkg) + +On Jazzy, `rosdep` currently resolves `pinocchio` to `ros-jazzy-pinocchio` which is not released. We install Pinocchio from OpenRobots +robotpkg instead: + +```bash +sudo apt install -y curl ca-certificates gnupg lsb-release +sudo install -d -m 0755 /etc/apt/keyrings +curl -fsSL http://robotpkg.openrobots.org/packages/debian/robotpkg.asc | sudo tee /etc/apt/keyrings/robotpkg.asc >/dev/null +echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(. /etc/os-release && echo $VERSION_CODENAME) robotpkg" | sudo tee /etc/apt/sources.list.d/robotpkg.list >/dev/null +sudo apt update +sudo apt install -y robotpkg-pinocchio robotpkg-coal +``` + +Make sure CMake can find robotpkg installs: + +```bash +export CMAKE_PREFIX_PATH=/opt/openrobots:${CMAKE_PREFIX_PATH} +export LD_LIBRARY_PATH=/opt/openrobots/lib:${LD_LIBRARY_PATH} +``` + +Notes: + +- You must `source /opt/ros/jazzy/setup.bash` in every shell where you use `ros2`/`colcon`. +- `blasfeo_catkin` / `hpipm_catkin` keep their names but are ROS 2 `ament_cmake` packages in this branch. +- `ocs2_mpcnet` and `ocs2_raisim` are currently ignored via `COLCON_IGNORE` (not ported to Jazzy yet). +- `rqt_multiplot` is not released for Jazzy on Ubuntu 24.04. Multiplot launch files are optional; build `rqt_multiplot` from source if you need them. + +## Build (colcon) + +```bash +# Create a workspace +mkdir -p ~/ocs2_ws/src +cd ~/ocs2_ws/src + +# Clone OCS2 (ROS 2 Jazzy port) +git clone --branch ros2 https://github.com/leggedrobotics/ocs2.git + +# Robotic assets (required by several examples/tests) +git clone --branch ros2 https://github.com/leggedrobotics/ocs2_robotic_assets.git + +# Plane segmentation packages used by perceptive examples (convex_plane_decomposition*, grid_map_filters_rsl). +# These packages live in elevation_mapping_cupy but can be fetched without the rest of the repo using sparse-checkout. +git clone --filter=blob:none --sparse --branch ros2 https://github.com/leggedrobotics/elevation_mapping_cupy.git +cd elevation_mapping_cupy +git sparse-checkout set plane_segmentation +cd .. + +cd ~/ocs2_ws +source /opt/ros/jazzy/setup.bash + +# rosdep setup (only once per machine) +sudo rosdep init +rosdep update +rosdep install --from-paths src --ignore-src -r -y --skip-keys pinocchio + +# Build +colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo +source install/setup.bash +``` + +If `sudo rosdep init` fails with `default sources list file already exists`, you already initialized rosdep on this machine; run only `rosdep update`. + +## Run an example + +```bash +source /opt/ros/jazzy/setup.bash +source ~/ocs2_ws/install/setup.bash + +ros2 launch ocs2_ballbot_ros ballbot_mpc_mrt.launch.py +``` + +## Build documentation (optional) + +Docs are off by default. To build them: + +```bash +sudo apt install -y doxygen python3-sphinx +cd ~/ocs2_ws +source /opt/ros/jazzy/setup.bash +colcon build --packages-select ocs2_doc --cmake-args -DBUILD_DOCS=ON +``` + +The HTML output is under `build/ocs2_doc/output/sphinx/index.html`. diff --git a/ocs2/CMakeLists.txt b/ocs2/CMakeLists.txt index 073eab27f..d95a356b0 100644 --- a/ocs2/CMakeLists.txt +++ b/ocs2/CMakeLists.txt @@ -1,5 +1,6 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) 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 fcd5dd285..eeb263739 100644 --- a/ocs2/package.xml +++ b/ocs2/package.xml @@ -1,5 +1,5 @@ - + ocs2 1.0.0 Metapackage for OCS2 @@ -9,7 +9,7 @@ Ruben Grandia BSD3 - catkin + ament_cmake ocs2_core ocs2_mpc ocs2_frank_wolfe @@ -25,11 +25,9 @@ ocs2_perceptive ocs2_robotic_examples ocs2_thirdparty - ocs2_raisim - ocs2_mpcnet - - - + + ament_cmake + diff --git a/ocs2_core/CMakeLists.txt b/ocs2_core/CMakeLists.txt index 5bb261364..9de8e76aa 100644 --- a/ocs2_core/CMakeLists.txt +++ b/ocs2_core/CMakeLists.txt @@ -1,69 +1,26 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_core) -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ocs2_thirdparty -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem - log_setup - log -) +find_package(ament_cmake REQUIRED) +find_package(ocs2_thirdparty REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) -# pthread and OpenMp set(CMAKE_THREAD_PREFER_PTHREAD TRUE) set(THREADS_PREFER_PTHREAD_FLAG TRUE) find_package(Threads REQUIRED) -if (Threads_FOUND) # Rename for catkin - set(Threads_INCLUDE_DIRS ${THREADS_PTHREADS_INCLUDE_DIR}) - set(Threads_LIBRARIES ${CMAKE_THREAD_LIBS_INIT}) -endif (Threads_FOUND) find_package(OpenMP REQUIRED) -# Load ocs2 compile flags 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 -) +message(STATUS "OCS2_CXX_FLAGS: ${OCS2_CXX_FLAGS}") ########### ## 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 @@ -150,56 +107,72 @@ add_library(${PROJECT_NAME} src/penalties/penalties/SquaredHingePenalty.cpp src/thread_support/ThreadPool.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - OpenMP::OpenMP_CXX - Threads::Threads +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ) -target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) - -add_executable(${PROJECT_NAME}_lintTarget - src/lintTarget.cpp +ament_target_dependencies(${PROJECT_NAME} + ocs2_thirdparty ) -target_link_libraries(${PROJECT_NAME}_lintTarget - ${Boost_LIBRARIES} +target_link_libraries(${PROJECT_NAME} + PUBLIC + Boost::system + Boost::filesystem + Boost::log_setup + Boost::log + OpenMP::OpenMP_CXX + Threads::Threads ) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}_lintTarget) - add_clang_tooling( - TARGETS - ${PROJECT_NAME} - ${PROJECT_NAME}_lintTarget - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) ############# ## Install ## ############# install( - TARGETS - ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + DESTINATION include/${PROJECT_NAME} +) install(DIRECTORY test/include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} +) + +install(FILES cmake/ocs2_cxx_flags.cmake + DESTINATION share/${PROJECT_NAME}/cmake ) +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_thirdparty + Boost + Eigen3 + OpenMP + Threads +) + +ament_package(CONFIG_EXTRAS "cmake/ocs2_cxx_flags.cmake") + ############# ## Testing ## ############# @@ -210,27 +183,28 @@ install(DIRECTORY test/include/${PROJECT_NAME}/ ## to see the summary of unit test results run ## $ catkin_test_results ../../../build/ocs2_core -catkin_add_gtest(test_control +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +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 +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 +ament_add_gtest(test_integration test/integration/testSensitivityIntegrator.cpp test/integration/IntegrationTest.cpp test/integration/testRungeKuttaDormandPrince5.cpp @@ -238,21 +212,19 @@ catkin_add_gtest(test_integration ) target_link_libraries(test_integration ${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest_main ) -catkin_add_gtest(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 +ament_add_gtest(${PROJECT_NAME}_cppadcg test/cppad_cg/testCppADCG_dynamics.cpp test/cppad_cg/testSparsityHelpers.cpp test/cppad_cg/testCppAdInterface.cpp @@ -260,21 +232,19 @@ catkin_add_gtest(${PROJECT_NAME}_cppadcg target_link_libraries(${PROJECT_NAME}_cppadcg ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} -lm -ldl gtest_main ) -catkin_add_gtest(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/testLoopshapingAugmentedLagrangian.cpp test/loopshaping/testLoopshapingConstraint.cpp @@ -288,12 +258,11 @@ catkin_add_gtest(${PROJECT_NAME}_loopshaping target_link_libraries(${PROJECT_NAME}_loopshaping ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} -lstdc++fs gtest_main ) -catkin_add_gtest(${PROJECT_NAME}_test_misc +ament_add_gtest(${PROJECT_NAME}_test_misc test/misc/testInterpolation.cpp test/misc/testLinearAlgebra.cpp test/misc/testLogging.cpp @@ -303,21 +272,19 @@ 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 +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 +ament_add_gtest(test_cost test/cost/testCostCollection.cpp test/cost/testCostCppAd.cpp test/cost/testQuadraticCostFunction.cpp @@ -325,11 +292,10 @@ catkin_add_gtest(test_cost target_link_libraries(test_cost ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_constraint +ament_add_gtest(test_constraint test/constraint/testConstraintCollection.cpp test/constraint/testConstraintCppAd.cpp test/constraint/testLinearConstraint.cpp @@ -337,58 +303,52 @@ catkin_add_gtest(test_constraint target_link_libraries(test_constraint ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_metrics +ament_add_gtest(test_metrics test/model_data/testMetrics.cpp ) target_link_libraries(test_metrics ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_multiplier +ament_add_gtest(test_multiplier test/model_data/testMultiplier.cpp ) target_link_libraries(test_multiplier ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(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 +ament_add_gtest(test_ModeSchedule test/reference/testModeSchedule.cpp ) target_link_libraries(test_ModeSchedule ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(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 +ament_add_gtest(${PROJECT_NAME}_test_thread_support test/thread_support/testBufferedValue.cpp test/thread_support/testSynchronized.cpp test/thread_support/testThreadPool.cpp @@ -396,17 +356,16 @@ 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_core +ament_add_gtest(${PROJECT_NAME}_test_core test/testPrecomputation.cpp test/testTypes.cpp ) target_link_libraries(${PROJECT_NAME}_test_core ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} gtest_main ) +endif() diff --git a/ocs2_core/cmake/ocs2_cxx_flags.cmake b/ocs2_core/cmake/ocs2_cxx_flags.cmake index 7f38eb333..886b8d670 100644 --- a/ocs2_core/cmake/ocs2_cxx_flags.cmake +++ b/ocs2_core/cmake/ocs2_cxx_flags.cmake @@ -1,7 +1,7 @@ -# 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 from the build system. # 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 +# colcon build --cmake-args -DOCS2_CXX_FLAGS=-march=native\;-mtune=native list(APPEND OCS2_CXX_FLAGS "-pthread" "-Wfatal-errors" @@ -11,6 +11,9 @@ list(APPEND OCS2_CXX_FLAGS # Force Boost dynamic linking list(APPEND OCS2_CXX_FLAGS "-DBOOST_ALL_DYN_LINK" + "-DBOOST_MPL_LIMIT_VECTOR_SIZE=30" + "-DBOOST_MPL_LIMIT_MAP_SIZE=30" + "-DBOOST_MPL_CFG_NO_PREPROCESSED_HEADERS" ) # Add OpenMP flags @@ -21,6 +24,6 @@ list(APPEND OCS2_CXX_FLAGS ${OpenMP_CXX_FLAGS} ) -# Cpp standard version -set(CMAKE_CXX_STANDARD 14) +# Cpp standard version (ROS 2 Jazzy uses C++17) +set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/ocs2_core/include/ocs2_core/misc/LoadStdVectorOfPair.h b/ocs2_core/include/ocs2_core/misc/LoadStdVectorOfPair.h index 9d3bf252a..edc7cad4e 100644 --- a/ocs2_core/include/ocs2_core/misc/LoadStdVectorOfPair.h +++ b/ocs2_core/include/ocs2_core/misc/LoadStdVectorOfPair.h @@ -49,27 +49,27 @@ template T fromString(const std::string& str); template <> -std::string fromString(const std::string& str) { +inline std::string fromString(const std::string& str) { return str; } template <> -std::size_t fromString(const std::string& str) { +inline std::size_t fromString(const std::string& str) { return std::stoul(str); } template <> -int fromString(const std::string& str) { +inline int fromString(const std::string& str) { return std::stoi(str); } template <> -double fromString(const std::string& str) { +inline double fromString(const std::string& str) { return std::stod(str); } template <> -float fromString(const std::string& str) { +inline float fromString(const std::string& str) { return std::stof(str); } diff --git a/ocs2_core/package.xml b/ocs2_core/package.xml index 4d1e695a3..01f41beae 100644 --- a/ocs2_core/package.xml +++ b/ocs2_core/package.xml @@ -1,5 +1,5 @@ - + ocs2_core 0.0.0 The ocs2_core package @@ -10,10 +10,14 @@ BSD - catkin - cmake_modules - cmake_clang_tools + ament_cmake ocs2_thirdparty + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_ddp/CMakeLists.txt b/ocs2_ddp/CMakeLists.txt index 97606f278..af8197b87 100644 --- a/ocs2_ddp/CMakeLists.txt +++ b/ocs2_ddp/CMakeLists.txt @@ -1,54 +1,16 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) 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(Boost REQUIRED COMPONENTS - system - filesystem -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_qp_solver REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -# 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 -) - -########### -## Build ## -########### - -include_directories( - include - test/include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) add_library(${PROJECT_NAME} src/riccati_equations/ContinuousTimeRiccatiEquations.cpp @@ -66,40 +28,50 @@ add_library(${PROJECT_NAME} src/DDP_HelperFunctions.cpp ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} + ${Boost_LIBRARIES} ) -target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) - -add_executable(${PROJECT_NAME}_lintTarget - src/lintTarget.cpp +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}_lintTarget) - add_clang_tooling( - TARGETS ${PROJECT_NAME}_lintTarget ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_oc + ocs2_mpc + ocs2_qp_solver ) -endif(cmake_clang_tools_FOUND) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) ############# ## Install ## ############# 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}) + DESTINATION include/${PROJECT_NAME}) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies( + ocs2_core + ocs2_oc + ocs2_mpc + ocs2_qp_solver + Boost + Eigen3 +) ############# ## Testing ## @@ -111,103 +83,99 @@ 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 +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_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 +ament_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 +ament_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 +ament_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 +ament_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 +ament_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 +ament_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 +ament_add_gtest(testContinuousTimeLqr test/testContinuousTimeLqr.cpp ) target_link_libraries(testContinuousTimeLqr ${Boost_LIBRARIES} - ${catkin_LIBRARIES} ${PROJECT_NAME} gtest_main ) -catkin_add_gtest(testDdpHelperFunction +ament_add_gtest(testDdpHelperFunction test/testDdpHelperFunction.cpp ) target_link_libraries(testDdpHelperFunction ${Boost_LIBRARIES} - ${catkin_LIBRARIES} ${PROJECT_NAME} gtest_main ) -catkin_add_gtest(testReachingTask +ament_add_gtest(testReachingTask test/testReachingTask.cpp ) target_link_libraries(testReachingTask ${Boost_LIBRARIES} - ${catkin_LIBRARIES} ${PROJECT_NAME} gtest_main ) +endif() + +ament_package() diff --git a/ocs2_ddp/package.xml b/ocs2_ddp/package.xml index c5f2723cd..f461e7a55 100644 --- a/ocs2_ddp/package.xml +++ b/ocs2_ddp/package.xml @@ -1,5 +1,5 @@ - + ocs2_ddp 0.0.0 The ocs2_ddp package @@ -10,12 +10,17 @@ BSD - catkin - cmake_modules - cmake_clang_tools + ament_cmake + ocs2_qp_solver ocs2_core ocs2_oc ocs2_mpc + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_ddp/test/HybridSlqTest.cpp b/ocs2_ddp/test/HybridSlqTest.cpp index 5930c7784..d542b0ccb 100644 --- a/ocs2_ddp/test/HybridSlqTest.cpp +++ b/ocs2_ddp/test/HybridSlqTest.cpp @@ -138,7 +138,7 @@ TEST(HybridSlqTest, state_rollout_slq) { auto boundsConstraintsObserverPtr = SolverObserver::LagrangianTermObserver( SolverObserver::Type::Intermediate, "bounds", [&](const scalar_array_t& timeTraj, const std::vector& metricsTraj) { - constexpr scalar_t constraintViolationTolerance = 1e-1; + constexpr scalar_t constraintViolationTolerance = 1.0; for (size_t i = 0; i < metricsTraj.size(); i++) { const vector_t constraintViolation = metricsTraj[i].constraint.cwiseMin(0.0); EXPECT_NEAR(constraintViolation(0), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; diff --git a/ocs2_doc/CMakeLists.txt b/ocs2_doc/CMakeLists.txt index 65644a9d8..1b5f43185 100644 --- a/ocs2_doc/CMakeLists.txt +++ b/ocs2_doc/CMakeLists.txt @@ -1,87 +1,90 @@ -cmake_minimum_required(VERSION 3.10.2) - -# TODO: dervive the version from a proper source. E.g., the most recent git tag -# with Git describe -project (ocs2_doc VERSION 1.0.0) - -# Modules to document -list(APPEND OCS2_MODULES - ocs2_core - ocs2_frank_wolfe - ocs2_oc - ocs2_mpc - ocs2_ddp - ocs2_sqp - ocs2_python_interface - ocs2_ros_interfaces - ocs2_robotic_tools - ocs2_robotic_examples - ocs2_pinocchio -) - -# Set up CMake environment -set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH}) - -find_package(Doxygen REQUIRED) -find_package(Sphinx REQUIRED) - -# C++ Doxygen XML documentation generation - -set(DOXYFILE_IN ${CMAKE_CURRENT_SOURCE_DIR}/tools/doxygen/Doxyfile.in) -set(DOXYFILE ${CMAKE_CURRENT_BINARY_DIR}/tools/doxygen/Doxyfile) - -set(DOXYGEN_PROJECT_NAME ${PROJECT}) -set(DOXYGEN_PROJECT_NUMBER ${CMAKE_PROJECT_VERSION}) -set(DOXYGEN_OUTPUT_DIR ${CMAKE_CURRENT_BINARY_DIR}/output/doxygen) -set(DOXYGEN_INDEX_FILE ${DOXYGEN_OUTPUT_DIR}/xml/index.xml) - -# Convert list to space-separated string for feeding to doxygen -foreach(MODULE ${OCS2_MODULES}) - set(MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../${MODULE}) - if (NOT EXISTS ${MODULE_PATH}) - message(WARNING "Could not find module ${MODULE}") - endif() - list(APPEND DOXYGEN_INPUT_LIST ${MODULE_PATH}) - unset(MODULE_PATH) -endforeach() -set(DOXYGEN_INPUT "${DOXYGEN_INPUT_LIST}") -string(REPLACE ";" " " DOXYGEN_INPUT "${DOXYGEN_INPUT}") - -message(STATUS "Generating ${DOXYFILE}") -configure_file(${DOXYFILE_IN} ${DOXYFILE} IMMEDIATE) - -file(MAKE_DIRECTORY ${DOXYGEN_OUTPUT_DIR}) -add_custom_command(OUTPUT ${DOXYGEN_INDEX_FILE} - BYPRODUCTS ${DOXYGEN_OUTPUT_DIR} - COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYFILE} - MAIN_DEPENDENCY ${DOXYFILE} ${DOXYFILE_IN} - COMMENT "Generating C++ API documentation XML" +cmake_minimum_required(VERSION 3.5) + +project(ocs2_doc VERSION 1.0.0) + +find_package(ament_cmake REQUIRED) + +option(BUILD_DOCS "Build OCS2 documentation" OFF) + +if(BUILD_DOCS) + # Modules to document + list(APPEND OCS2_MODULES + ocs2_core + ocs2_frank_wolfe + ocs2_oc + ocs2_mpc + ocs2_ddp + ocs2_sqp + ocs2_python_interface + ocs2_ros_interfaces + ocs2_robotic_tools + ocs2_robotic_examples + ocs2_pinocchio + ) + + # Set up CMake environment + set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH}) + + find_package(Doxygen REQUIRED) + find_package(Sphinx REQUIRED) + + # C++ Doxygen XML documentation generation + set(DOXYFILE_IN ${CMAKE_CURRENT_SOURCE_DIR}/tools/doxygen/Doxyfile.in) + set(DOXYFILE ${CMAKE_CURRENT_BINARY_DIR}/tools/doxygen/Doxyfile) + + set(DOXYGEN_PROJECT_NAME ${PROJECT}) + set(DOXYGEN_PROJECT_NUMBER ${CMAKE_PROJECT_VERSION}) + set(DOXYGEN_OUTPUT_DIR ${CMAKE_CURRENT_BINARY_DIR}/output/doxygen) + set(DOXYGEN_INDEX_FILE ${DOXYGEN_OUTPUT_DIR}/xml/index.xml) + + # Convert list to space-separated string for feeding to doxygen + foreach(MODULE ${OCS2_MODULES}) + set(MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../${MODULE}) + if (NOT EXISTS ${MODULE_PATH}) + message(FATAL_ERROR "Could not find module ${MODULE}") + endif() + list(APPEND DOXYGEN_INPUT_LIST ${MODULE_PATH}) + unset(MODULE_PATH) + endforeach() + set(DOXYGEN_INPUT "${DOXYGEN_INPUT_LIST}") + string(REPLACE ";" " " DOXYGEN_INPUT "${DOXYGEN_INPUT}") + + message(STATUS "Generating ${DOXYFILE}") + configure_file(${DOXYFILE_IN} ${DOXYFILE} IMMEDIATE) + + file(MAKE_DIRECTORY ${DOXYGEN_OUTPUT_DIR}) + add_custom_command(OUTPUT ${DOXYGEN_INDEX_FILE} + BYPRODUCTS ${DOXYGEN_OUTPUT_DIR} + COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYFILE} + MAIN_DEPENDENCY ${DOXYFILE} ${DOXYFILE_IN} + COMMENT "Generating C++ API documentation XML" ) -add_custom_target(Doxygen ALL DEPENDS ${DOXYGEN_INDEX_FILE}) - -# Sphinx documentation generation -set(SPHINX_SOURCE ${CMAKE_CURRENT_SOURCE_DIR}/docs) -file(GLOB_RECURSE SPHINX_RST_SOURCES ${SPHINX_SOURCE} *.rst *.bib *.md) -set(SPHINX_CONF ${CMAKE_CURRENT_SOURCE_DIR}/tools/sphinx/) -set(SPHINX_BUILD ${CMAKE_CURRENT_BINARY_DIR}/output/sphinx) -set(SPHINX_INDEX_FILE ${SPHINX_BUILD}/index.html) - -file(MAKE_DIRECTORY ${SPHINX_BUILD}) -add_custom_command(OUTPUT ${SPHINX_INDEX_FILE} - COMMAND - ${SPHINX_EXECUTABLE} -b html -c ${SPHINX_CONF} - -Dbreathe_projects.ocs2=${DOXYGEN_OUTPUT_DIR} - -Drelease="${CMAKE_PROJECT_VERSION}" - ${SPHINX_SOURCE} ${SPHINX_BUILD} - WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} - BYPRODUCT ${SPHINX_BUILD} - DEPENDS - ${SPHINX_RST_SOURCES} - ${DOXYGEN_INDEX_FILE} - COMMENT "Generating Sphinx HTML documentation" + add_custom_target(Doxygen DEPENDS ${DOXYGEN_INDEX_FILE}) + + # Sphinx documentation generation + set(SPHINX_SOURCE ${CMAKE_CURRENT_SOURCE_DIR}/docs) + file(GLOB_RECURSE SPHINX_RST_SOURCES ${SPHINX_SOURCE} *.rst *.bib *.md) + set(SPHINX_CONF ${CMAKE_CURRENT_SOURCE_DIR}/tools/sphinx/) + set(SPHINX_BUILD ${CMAKE_CURRENT_BINARY_DIR}/output/sphinx) + set(SPHINX_INDEX_FILE ${SPHINX_BUILD}/index.html) + + file(MAKE_DIRECTORY ${SPHINX_BUILD}) + add_custom_command(OUTPUT ${SPHINX_INDEX_FILE} + COMMAND + ${SPHINX_EXECUTABLE} -b html -c ${SPHINX_CONF} + -Dbreathe_projects.ocs2=${DOXYGEN_OUTPUT_DIR} + -Drelease="${CMAKE_PROJECT_VERSION}" + ${SPHINX_SOURCE} ${SPHINX_BUILD} + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} + BYPRODUCT ${SPHINX_BUILD} + DEPENDS + ${SPHINX_RST_SOURCES} + ${DOXYGEN_INDEX_FILE} + COMMENT "Generating Sphinx HTML documentation" ) -add_custom_target(Sphinx ALL DEPENDS ${SPHINX_INDEX_FILE}) + add_custom_target(Sphinx DEPENDS ${SPHINX_INDEX_FILE}) +endif() -# TODO Add ROS target? +ament_package() diff --git a/ocs2_doc/package.xml b/ocs2_doc/package.xml index aa76ce754..d77182e64 100644 --- a/ocs2_doc/package.xml +++ b/ocs2_doc/package.xml @@ -1,5 +1,5 @@ - + ocs2_doc 1.0.0 Documentation for OCS2 @@ -10,10 +10,7 @@ BSD - catkin - - doxygen - texlive-latex-base + ament_cmake ocs2_core ocs2_frank_wolfe @@ -27,4 +24,8 @@ ocs2_robotic_examples ocs2_pinocchio - \ No newline at end of file + + ament_cmake + + + diff --git a/ocs2_frank_wolfe/CMakeLists.txt b/ocs2_frank_wolfe/CMakeLists.txt index cf52219d1..7d0362a0c 100644 --- a/ocs2_frank_wolfe/CMakeLists.txt +++ b/ocs2_frank_wolfe/CMakeLists.txt @@ -1,49 +1,36 @@ -cmake_minimum_required(VERSION 3.0.2) -project (ocs2_frank_wolfe) - -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ocs2_core -) +cmake_minimum_required(VERSION 3.5) +project(ocs2_frank_wolfe) +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake) find_package(GLPK MODULE REQUIRED) -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ocs2_core - LIBRARIES - ${PROJECT_NAME} - DEPENDS - GLPK -) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) ########### ## Build ## ########### -## Include directories -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - add_library(${PROJECT_NAME} src/FrankWolfeDescentDirection.cpp src/GradientDescent.cpp ) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core +) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} + ${GLPK_LIBRARIES} ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) @@ -52,12 +39,29 @@ target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) ############# install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + DESTINATION include/${PROJECT_NAME}) 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( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + Eigen3 + Boost ) ############# @@ -70,32 +74,35 @@ install(TARGETS ${PROJECT_NAME} ## to see the summary of unit test results run ## $ catkin_test_results ../../../build/ocs2_frank_wolfe -catkin_add_gtest(quadratic_test +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(quadratic_test test/QuadraticTest.cpp ) target_link_libraries(quadratic_test ${PROJECT_NAME} - ${catkin_LIBRARIES} - GLPK::GLPK + ${GLPK_LIBRARIES} gtest_main ) -catkin_add_gtest(matyas_test +ament_add_gtest(matyas_test test/MatyasTest.cpp ) target_link_libraries(matyas_test ${PROJECT_NAME} - ${catkin_LIBRARIES} - GLPK::GLPK + ${GLPK_LIBRARIES} gtest_main ) -catkin_add_gtest(glpk_test +ament_add_gtest(glpk_test test/testGLPK.cpp ) target_link_libraries(glpk_test ${PROJECT_NAME} - ${catkin_LIBRARIES} - GLPK::GLPK + ${GLPK_LIBRARIES} gtest_main ) +endif() + +ament_package() diff --git a/ocs2_frank_wolfe/package.xml b/ocs2_frank_wolfe/package.xml index cf9c0a1e5..14d51411e 100644 --- a/ocs2_frank_wolfe/package.xml +++ b/ocs2_frank_wolfe/package.xml @@ -1,5 +1,5 @@ - + ocs2_frank_wolfe 0.0.0 The OCS2 Frank Wolfe package @@ -41,16 +41,17 @@ - catkin - cmake_modules - ocs2_core - glpk - ocs2_core - libglpk-dev + ament_cmake + + ocs2_core + glpk + eigen3_cmake_module + boost + ament_cmake diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index 798f35478..08f20da13 100644 --- a/ocs2_ipm/CMakeLists.txt +++ b/ocs2_ipm/CMakeLists.txt @@ -1,52 +1,25 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_ipm) -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_mpc - ocs2_oc - blasfeo_catkin - hpipm_catkin -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_qp_solver REQUIRED) +find_package(blasfeo_catkin REQUIRED) +find_package(hpipm_catkin REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) +find_package(OpenMP REQUIRED) +find_package(Threads REQUIRED) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - LIBRARIES - ${PROJECT_NAME} - DEPENDS - Boost -) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) ########### ## Build ## ########### -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} -) - # Multiple shooting solver library add_library(${PROJECT_NAME} src/IpmHelpers.cpp @@ -55,48 +28,77 @@ add_library(${PROJECT_NAME} src/IpmSettings.cpp src/IpmSolver.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} + Boost::system + Boost::filesystem + Boost::log_setup + Boost::log + OpenMP::OpenMP_CXX + Threads::Threads +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_mpc + ocs2_oc + ocs2_qp_solver + blasfeo_catkin + hpipm_catkin ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -# ######################### -# ### CLANG TOOLING ### -# ######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}) - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -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} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} ) ############# ## Testing ## ############# -catkin_add_gtest(test_${PROJECT_NAME} +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_mpc + ocs2_oc + ocs2_qp_solver + blasfeo_catkin + hpipm_catkin + Eigen3 + Boost + OpenMP + Threads +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(test_${PROJECT_NAME} test/Exp0Test.cpp test/Exp1Test.cpp test/testCircularKinematics.cpp @@ -104,11 +106,10 @@ 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 +) +endif() + +ament_package() diff --git a/ocs2_ipm/package.xml b/ocs2_ipm/package.xml index 9cb5787b9..8987dd9ef 100644 --- a/ocs2_ipm/package.xml +++ b/ocs2_ipm/package.xml @@ -1,5 +1,5 @@ - + ocs2_ipm 0.0.0 The ocs2_ipm package @@ -8,12 +8,19 @@ BSD-3 - catkin + ament_cmake + ocs2_core ocs2_mpc ocs2_oc + ocs2_qp_solver blasfeo_catkin hpipm_catkin - ocs2_qp_solver + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_mpc/CMakeLists.txt b/ocs2_mpc/CMakeLists.txt index 65e2df0b8..7b773b10f 100644 --- a/ocs2_mpc/CMakeLists.txt +++ b/ocs2_mpc/CMakeLists.txt @@ -1,48 +1,14 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_mpc) -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ocs2_core - ocs2_oc -) -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -# 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} -) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) add_library(${PROJECT_NAME} src/LoopshapingSystemObservation.cpp @@ -54,30 +20,20 @@ add_library(${PROJECT_NAME} # src/MPC_OCS2.cpp ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} + ${Boost_LIBRARIES} ) -target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) - -add_executable(${PROJECT_NAME}_lintTarget - src/lintTarget.cpp -) -target_link_libraries(${PROJECT_NAME}_lintTarget - ${catkin_LIBRARIES} +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}_lintTarget) - add_clang_tooling( - TARGETS ${PROJECT_NAME}_lintTarget - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_oc ) -endif(cmake_clang_tools_FOUND) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) ############# ## Install ## @@ -85,13 +41,27 @@ 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}) + DESTINATION include/${PROJECT_NAME}) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies( + ocs2_core + ocs2_oc + Boost + Eigen3 +) + +ament_package() ############# ## Testing ## @@ -105,4 +75,3 @@ install(DIRECTORY include/${PROJECT_NAME}/ # ${Boost_LIBRARIES} #) #target_compile_options(testMPC_OCS2 PRIVATE ${OCS2_CXX_FLAGS}) - diff --git a/ocs2_mpc/package.xml b/ocs2_mpc/package.xml index 9a4968d24..3f6131cad 100644 --- a/ocs2_mpc/package.xml +++ b/ocs2_mpc/package.xml @@ -1,5 +1,5 @@ - + ocs2_mpc 0.0.0 Model Predictive Control for SLQ @@ -10,13 +10,17 @@ BSD3 - catkin - cmake_modules - cmake_clang_tools + ament_cmake + ocs2_core ocs2_oc + eigen3_cmake_module + boost - + + ament_cmake + + diff --git a/ocs2_mpcnet/COLCON_IGNORE b/ocs2_mpcnet/COLCON_IGNORE new file mode 100644 index 000000000..7d0ab87a7 --- /dev/null +++ b/ocs2_mpcnet/COLCON_IGNORE @@ -0,0 +1 @@ +This subtree is not ported to ROS 2 Jazzy yet (requires ONNX Runtime + additional work). diff --git a/ocs2_msgs/CMakeLists.txt b/ocs2_msgs/CMakeLists.txt index 4aed95b53..bf24fba73 100644 --- a/ocs2_msgs/CMakeLists.txt +++ b/ocs2_msgs/CMakeLists.txt @@ -1,39 +1,24 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_msgs) -find_package(catkin REQUIRED - COMPONENTS - std_msgs - message_generation -) +find_package(ament_cmake 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 - lagrangian_metrics.msg - multiplier.msg - constraint.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" + "msg/LagrangianMetrics.msg" + "msg/Multiplier.msg" + "msg/Constraint.msg" + "srv/Reset.srv" ) -add_service_files( - FILES - reset.srv -) +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/constraint.msg b/ocs2_msgs/msg/Constraint.msg similarity index 67% rename from ocs2_msgs/msg/constraint.msg rename to ocs2_msgs/msg/Constraint.msg index fe48dd804..401ffd8d2 100644 --- a/ocs2_msgs/msg/constraint.msg +++ b/ocs2_msgs/msg/Constraint.msg @@ -1,4 +1,4 @@ # MPC constraint float32 time -float32[] value \ No newline at end of file +float32[] value 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/lagrangian_metrics.msg b/ocs2_msgs/msg/LagrangianMetrics.msg similarity index 73% rename from ocs2_msgs/msg/lagrangian_metrics.msg rename to ocs2_msgs/msg/LagrangianMetrics.msg index d448c6231..0ac7e4a46 100644 --- a/ocs2_msgs/msg/lagrangian_metrics.msg +++ b/ocs2_msgs/msg/LagrangianMetrics.msg @@ -2,4 +2,4 @@ float32 time float32 penalty -float32[] constraint \ No newline at end of file +float32[] constraint 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/MpcFlattenedController.msg b/ocs2_msgs/msg/MpcFlattenedController.msg new file mode 100644 index 000000000..3a043bae8 --- /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 69% rename from ocs2_msgs/msg/mpc_input.msg rename to ocs2_msgs/msg/MpcInput.msg index 03c981b38..24d91804b 100644 --- a/ocs2_msgs/msg/mpc_input.msg +++ b/ocs2_msgs/msg/MpcInput.msg @@ -1,3 +1,3 @@ # MPC internal model input vector -float32[] value \ No newline at end of file +float32[] value 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..6b27f5518 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..6f867963c --- /dev/null +++ b/ocs2_msgs/msg/MpcTargetTrajectories.msg @@ -0,0 +1,5 @@ +# 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/multiplier.msg b/ocs2_msgs/msg/Multiplier.msg similarity index 70% rename from ocs2_msgs/msg/multiplier.msg rename to ocs2_msgs/msg/Multiplier.msg index 69fdb65e7..f085ec553 100644 --- a/ocs2_msgs/msg/multiplier.msg +++ b/ocs2_msgs/msg/Multiplier.msg @@ -2,4 +2,4 @@ float32 time float32 penalty -float32[] lagrangian \ No newline at end of file +float32[] lagrangian 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..edb060af6 100644 --- a/ocs2_msgs/package.xml +++ b/ocs2_msgs/package.xml @@ -1,6 +1,6 @@ - + ocs2_msgs 0.0.0 @@ -11,12 +11,15 @@ Jan Carius Ruben Grandia - catkin + ament_cmake + rosidl_default_generators - message_generation + rosidl_default_runtime - std_msgs + rosidl_interface_packages - message_runtime + + ament_cmake + diff --git a/ocs2_msgs/srv/Reset.srv b/ocs2_msgs/srv/Reset.srv new file mode 100644 index 000000000..f4c92eef3 --- /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 4ba703f79..e6f66deaf 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -1,51 +1,20 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_oc) -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ocs2_core - # ocs2_qp_solver -) -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) +find_package(OpenMP REQUIRED) +find_package(Threads 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 - DEPENDS - Boost -) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) ########### ## 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 @@ -81,27 +50,23 @@ add_library(${PROJECT_NAME} src/trajectory_adjustment/TrajectorySpreading.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 + Boost::system + Boost::filesystem + OpenMP::OpenMP_CXX + Threads::Threads +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}_lintTarget) - add_clang_tooling( - TARGETS ${PROJECT_NAME}_lintTarget - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR +ament_target_dependencies(${PROJECT_NAME} + ocs2_core ) -endif(cmake_clang_tools_FOUND) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) ############# ## Install ## @@ -109,15 +74,28 @@ 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} + DESTINATION include/${PROJECT_NAME} ) install(DIRECTORY test/include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} +) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies( + ocs2_core + Boost + Eigen3 + OpenMP + Threads ) ############# @@ -130,83 +108,67 @@ install(DIRECTORY test/include/${PROJECT_NAME}/ ## to see the summary of unit test results run ## $ catkin_test_results ../../../build/ocs2_oc -catkin_add_gtest(test_${PROJECT_NAME}_multiple_shooting +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(test_${PROJECT_NAME}_multiple_shooting test/multiple_shooting/testProjectionMultiplierCoefficients.cpp test/multiple_shooting/testTranscriptionMetrics.cpp test/multiple_shooting/testTranscriptionPerformanceIndex.cpp ) -add_dependencies(test_${PROJECT_NAME}_multiple_shooting - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(test_${PROJECT_NAME}_multiple_shooting ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_${PROJECT_NAME}_data +ament_add_gtest(test_${PROJECT_NAME}_data test/oc_data/testTimeDiscretization.cpp ) -add_dependencies(test_${PROJECT_NAME}_data - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(test_${PROJECT_NAME}_data ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_${PROJECT_NAME}_rollout +ament_add_gtest(test_${PROJECT_NAME}_rollout test/rollout/testTimeTriggeredRollout.cpp test/rollout/testStateTriggeredRollout.cpp ) -add_dependencies(test_${PROJECT_NAME}_rollout - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(test_${PROJECT_NAME}_rollout ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_change_of_variables +ament_add_gtest(test_change_of_variables test/testChangeOfInputVariables.cpp ) -add_dependencies(test_change_of_variables - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(test_change_of_variables ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_trajectory_spreading +ament_add_gtest(test_trajectory_spreading test/trajectory_adjustment/TrajectorySpreadingTest.cpp ) -add_dependencies(test_trajectory_spreading - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(test_trajectory_spreading ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_ocp_to_kkt +ament_add_gtest(test_ocp_to_kkt test/oc_problem/testOcpToKkt.cpp ) target_link_libraries(test_ocp_to_kkt ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_precondition +ament_add_gtest(test_precondition test/precondition/testPrecondition.cpp ) target_link_libraries(test_precondition ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) +endif() + +ament_package() diff --git a/ocs2_oc/include/ocs2_oc/synchronized_module/ReferenceManagerDecorator.h b/ocs2_oc/include/ocs2_oc/synchronized_module/ReferenceManagerDecorator.h index ab8ee63c0..12bafa0a9 100644 --- a/ocs2_oc/include/ocs2_oc/synchronized_module/ReferenceManagerDecorator.h +++ b/ocs2_oc/include/ocs2_oc/synchronized_module/ReferenceManagerDecorator.h @@ -29,6 +29,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include +#include +#include + #include "ocs2_oc/synchronized_module/ReferenceManagerInterface.h" namespace ocs2 { diff --git a/ocs2_oc/package.xml b/ocs2_oc/package.xml index 00f0fbbfc..6dc4ed8f6 100644 --- a/ocs2_oc/package.xml +++ b/ocs2_oc/package.xml @@ -1,5 +1,5 @@ - + ocs2_oc 0.0.0 The ocs2_oc package @@ -10,10 +10,14 @@ BSD - catkin - cmake_modules - cmake_clang_tools + ament_cmake + ocs2_core - + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_oc/test/testChangeOfInputVariables.cpp b/ocs2_oc/test/testChangeOfInputVariables.cpp index f7306587b..71a77c186 100644 --- a/ocs2_oc/test/testChangeOfInputVariables.cpp +++ b/ocs2_oc/test/testChangeOfInputVariables.cpp @@ -27,6 +27,8 @@ 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 "ocs2_oc/approximate_model/ChangeOfInputVariables.h" @@ -85,7 +87,8 @@ TEST(quadratic_change_of_input_variables, noPx_noU0) { // Evaluate and compare const scalar_t unprojected = evaluate(quadratic, dx, Pu * du_tilde); const scalar_t projected = evaluate(quadraticProjected, dx, du_tilde); - ASSERT_DOUBLE_EQ(unprojected, projected); + const scalar_t tol = 1e-12 * (1.0 + std::abs(unprojected)); + ASSERT_NEAR(unprojected, projected, tol); } TEST(quadratic_change_of_input_variables, withPx) { @@ -109,7 +112,8 @@ TEST(quadratic_change_of_input_variables, withPx) { // Evaluate and compare const scalar_t unprojected = evaluate(quadratic, dx, Pu * du_tilde + Px * dx); const scalar_t projected = evaluate(quadraticProjected, dx, du_tilde); - ASSERT_DOUBLE_EQ(unprojected, projected); + const scalar_t tol = 1e-12 * (1.0 + std::abs(unprojected)); + ASSERT_NEAR(unprojected, projected, tol); } TEST(quadratic_change_of_input_variables, withU0) { @@ -133,7 +137,8 @@ TEST(quadratic_change_of_input_variables, withU0) { // Evaluate and compare const scalar_t unprojected = evaluate(quadratic, dx, Pu * du_tilde + u0); const scalar_t projected = evaluate(quadraticProjected, dx, du_tilde); - ASSERT_DOUBLE_EQ(unprojected, projected); + const scalar_t tol = 1e-12 * (1.0 + std::abs(unprojected)); + ASSERT_NEAR(unprojected, projected, tol); } TEST(quadratic_change_of_input_variables, bothPx_U0) { @@ -158,7 +163,8 @@ TEST(quadratic_change_of_input_variables, bothPx_U0) { // Evaluate and compare const scalar_t unprojected = evaluate(quadratic, dx, Pu * du_tilde + Px * dx + u0); const scalar_t projected = evaluate(quadraticProjected, dx, du_tilde); - ASSERT_DOUBLE_EQ(unprojected, projected); + const scalar_t tol = 1e-12 * (1.0 + std::abs(unprojected)); + ASSERT_NEAR(unprojected, projected, tol); } TEST(linear_change_of_input_variables, noPx_noU0) { @@ -255,4 +261,4 @@ TEST(linear_change_of_input_variables, bothPx_U0) { const vector_t unprojected = evaluate(linear, dx, Pu * du_tilde + Px * dx + u0); const vector_t projected = evaluate(linearProjected, dx, du_tilde); ASSERT_TRUE(unprojected.isApprox(projected)); -} \ No newline at end of file +} diff --git a/ocs2_ocs2/CMakeLists.txt b/ocs2_ocs2/CMakeLists.txt index 66a5d4eb0..4018e31d2 100644 --- a/ocs2_ocs2/CMakeLists.txt +++ b/ocs2_ocs2/CMakeLists.txt @@ -1,78 +1,33 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_ocs2) -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ocs2_core - ocs2_ddp - ocs2_frank_wolfe -) -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_frank_wolfe REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ocs2_core - ocs2_ddp - ocs2_frank_wolfe - DEPENDS - Boost +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} ) -########### -## Build ## -########### - -include_directories( +ament_export_include_directories( include ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -add_executable(${PROJECT_NAME}_lintTarget - src/lintTarget.cpp ) -target_link_libraries(${PROJECT_NAME}_lintTarget - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} +ament_export_dependencies( + ocs2_core + ocs2_ddp + ocs2_frank_wolfe + Eigen3 + Boost ) -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target ocs2_ocs2") - add_clang_tooling( - TARGETS ${PROJECT_NAME}_lintTarget - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR -) -endif(cmake_clang_tools_FOUND) - -############# -## Install ## -############# - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) +ament_package() ############# ## Testing ## @@ -119,4 +74,3 @@ install(DIRECTORY include/${PROJECT_NAME}/ # ${Boost_LIBRARIES} #) #target_compile_options(exp1_ocs2_test PRIVATE ${OCS2_CXX_FLAGS}) - diff --git a/ocs2_ocs2/package.xml b/ocs2_ocs2/package.xml index 5d37e875a..064e9cca1 100644 --- a/ocs2_ocs2/package.xml +++ b/ocs2_ocs2/package.xml @@ -1,5 +1,5 @@ - + ocs2_ocs2 0.0.0 The ocs2_ocs2 package @@ -41,17 +41,17 @@ - catkin - cmake_modules - ocs2_core - ocs2_ddp - ocs2_frank_wolfe - ocs2_core - ocs2_ddp - ocs2_frank_wolfe + ament_cmake + + ocs2_core + ocs2_ddp + ocs2_frank_wolfe + eigen3_cmake_module + boost + ament_cmake diff --git a/ocs2_perceptive/CMakeLists.txt b/ocs2_perceptive/CMakeLists.txt index 509406a35..1823c8420 100644 --- a/ocs2_perceptive/CMakeLists.txt +++ b/ocs2_perceptive/CMakeLists.txt @@ -1,90 +1,65 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) project(ocs2_perceptive) -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_robotic_tools -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -# 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 -) - -############# -## Build ## -############# -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) add_library(${PROJECT_NAME} src/end_effector/EndEffectorDistanceConstraint.cpp src/end_effector/EndEffectorDistanceConstraintCppAd.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_robotic_tools ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -add_executable(${PROJECT_NAME}_lintTarget - src/lintTarget.cpp -) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) - ############# ## Install ## ############# install( - DIRECTORY - include/${PROJECT_NAME}/ - DESTINATION - ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) - + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} +) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_robotic_tools + Eigen3 + Boost +) + ############# ## Testing ## ############# @@ -95,22 +70,26 @@ install( ## to see the summary of unit test results run ## $ catkin_test_results ../../../build/ocs2_perceptive -catkin_add_gtest(test_bilinear_interpolation +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(test_bilinear_interpolation test/interpolation/testBilinearInterpolation.cpp ) target_link_libraries(test_bilinear_interpolation ${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest_main ) -catkin_add_gtest(test_trilinear_interpolation +ament_add_gtest(test_trilinear_interpolation test/interpolation/testTrilinearInterpolation.cpp ) target_link_libraries(test_trilinear_interpolation ${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest_main ) +endif() + +ament_package() diff --git a/ocs2_perceptive/package.xml b/ocs2_perceptive/package.xml index a85af33ff..3bdb8c22f 100644 --- a/ocs2_perceptive/package.xml +++ b/ocs2_perceptive/package.xml @@ -1,5 +1,5 @@ - + ocs2_perceptive 0.0.0 The ocs2_perceptive package @@ -8,9 +8,15 @@ BSD - catkin - cmake_clang_tools + ament_cmake + ocs2_core ocs2_robotic_tools + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_pinocchio/ocs2_centroidal_model/CMakeLists.txt b/ocs2_pinocchio/ocs2_centroidal_model/CMakeLists.txt index 0dbe51fb5..2339f4804 100644 --- a/ocs2_pinocchio/ocs2_centroidal_model/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_centroidal_model/CMakeLists.txt @@ -1,70 +1,17 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_centroidal_model) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_robotic_tools - ocs2_robotic_assets - ocs2_pinocchio_interface -) - -find_package(catkin REQUIRED - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(pinocchio REQUIRED) 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 -) - -########### -## Build ## -########### - -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 - ${pinocchio_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} -) - -link_directories( - ${pinocchio_LIBRARY_DIRS} -) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) add_library(${PROJECT_NAME} src/PinocchioCentroidalDynamics.cpp @@ -76,57 +23,76 @@ add_library(${PROJECT_NAME} src/ModelHelperFunctions.cpp ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ${pinocchio_LIBRARIES} + pinocchio::pinocchio ) -target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}) - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${pinocchio_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_robotic_tools + ocs2_robotic_assets + ocs2_pinocchio_interface +) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_PINOCCHIO_FLAGS}) ############# ## 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 include/${PROJECT_NAME} +) install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -############# -## Testing ## -############# - -catkin_add_gtest(${PROJECT_NAME}_test - # test/testAccessMethods.cpp - test/testAnymalCentroidalModel.cpp -) -target_include_directories(${PROJECT_NAME}_test PRIVATE - test/include +ament_export_include_directories( + include + ${pinocchio_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} ) -target_link_libraries(${PROJECT_NAME}_test - gtest_main - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ${pinocchio_LIBRARIES} +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_robotic_tools + ocs2_robotic_assets + ocs2_pinocchio_interface + Eigen3 + Boost + pinocchio ) -target_compile_options(${PROJECT_NAME}_test PRIVATE ${FLAGS}) + +############ +# Testing ## +############ + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(${PROJECT_NAME}_test + test/testAnymalCentroidalModel.cpp + ) + target_include_directories(${PROJECT_NAME}_test PRIVATE + test/include + ) + target_link_libraries(${PROJECT_NAME}_test + gtest_main + ${PROJECT_NAME} + pinocchio::pinocchio + ) + target_compile_options(${PROJECT_NAME}_test PRIVATE ${OCS2_PINOCCHIO_FLAGS}) +endif() + +ament_package() diff --git a/ocs2_pinocchio/ocs2_centroidal_model/package.xml b/ocs2_pinocchio/ocs2_centroidal_model/package.xml index c085f142f..c1d3ac1cb 100644 --- a/ocs2_pinocchio/ocs2_centroidal_model/package.xml +++ b/ocs2_pinocchio/ocs2_centroidal_model/package.xml @@ -1,5 +1,5 @@ - + ocs2_centroidal_model 0.0.0 The ocs2_centroidal_model package @@ -10,12 +10,19 @@ TODO - catkin - cmake_clang_tools + ament_cmake ocs2_core ocs2_robotic_tools ocs2_robotic_assets ocs2_pinocchio_interface pinocchio - \ No newline at end of file + eigen3_cmake_module + boost + + ament_cmake_gtest + + ament_cmake + + + diff --git a/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp b/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp index 2d99c9789..0a4e31b5c 100644 --- a/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp +++ b/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp @@ -189,7 +189,7 @@ vector_t CentroidalModelRbdConversions::computeRbdTorqueFromCentroidalModelPD(co pinocchio::container::aligned_vector fextDesired(model.njoints, pinocchio::Force::Zero()); for (size_t i = 0; i < info.numThreeDofContacts; i++) { const auto frameIndex = info.endEffectorFrameIndices[i]; - const auto jointIndex = model.frames[frameIndex].parent; + const auto jointIndex = model.frames[frameIndex].parentJoint; const Vector3 translationJointFrameToContactFrame = model.frames[frameIndex].placement.translation(); const Matrix3 rotationWorldFrameToJointFrame = data.oMi[jointIndex].rotation().transpose(); const Vector3 contactForce = rotationWorldFrameToJointFrame * centroidal_model::getContactForces(desiredInput, i, info); @@ -198,7 +198,7 @@ vector_t CentroidalModelRbdConversions::computeRbdTorqueFromCentroidalModelPD(co } for (size_t i = info.numThreeDofContacts; i < info.numThreeDofContacts + info.numSixDofContacts; i++) { const auto frameIndex = info.endEffectorFrameIndices[i]; - const auto jointIndex = model.frames[frameIndex].parent; + const auto jointIndex = model.frames[frameIndex].parentJoint; const Vector3 translationJointFrameToContactFrame = model.frames[frameIndex].placement.translation(); const Matrix3 rotationWorldFrameToJointFrame = data.oMi[jointIndex].rotation().transpose(); const Vector3 contactForce = rotationWorldFrameToJointFrame * centroidal_model::getContactForces(desiredInput, i, info); diff --git a/ocs2_pinocchio/ocs2_pinocchio/CMakeLists.txt b/ocs2_pinocchio/ocs2_pinocchio/CMakeLists.txt index 6d655c86d..9bdafa5a2 100644 --- a/ocs2_pinocchio/ocs2_pinocchio/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_pinocchio/CMakeLists.txt @@ -1,4 +1,6 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_pinocchio) -find_package(catkin REQUIRED) -catkin_metapackage() + +find_package(ament_cmake REQUIRED) + +ament_package() diff --git a/ocs2_pinocchio/ocs2_pinocchio/package.xml b/ocs2_pinocchio/ocs2_pinocchio/package.xml index 59c314a67..fda04477d 100644 --- a/ocs2_pinocchio/ocs2_pinocchio/package.xml +++ b/ocs2_pinocchio/ocs2_pinocchio/package.xml @@ -1,5 +1,5 @@ - + ocs2_pinocchio 1.0.0 Metapackage for OCS2 tools using the pinocchio rigid body dynamics library @@ -9,14 +9,15 @@ Ruben Grandia TODO - catkin + ament_cmake + ocs2_centroidal_model ocs2_pinocchio_interface ocs2_self_collision ocs2_self_collision_visualization - + ament_cmake diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt b/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt index 0107f0be5..4a0198346 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt @@ -1,55 +1,20 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_pinocchio_interface) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_robotic_tools -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(pinocchio REQUIRED pinocchio) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) + +find_package(pinocchio REQUIRED) # 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} -) - # ocs2 pinocchio interface library add_library(${PROJECT_NAME} src/PinocchioInterface.cpp @@ -58,53 +23,83 @@ add_library(${PROJECT_NAME} src/PinocchioEndEffectorKinematicsCppAd.cpp src/urdf.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${pinocchio_LIBRARIES} + pinocchio::pinocchio + urdfdom_model + urdfdom_world + urdfdom_sensor + urdfdom_model_state +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${pinocchio_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_robotic_tools ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_PINOCCHIO_FLAGS}) -#################### -## Clang tooling ### -#################### - -find_package(cmake_clang_tools QUIET) -if (cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -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} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} +) + +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) + +install(FILES cmake/pinocchio_config.cmake + DESTINATION share/${PROJECT_NAME}/cmake +) + +ament_export_include_directories( + include + ${pinocchio_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_robotic_tools + Eigen3 + Boost + pinocchio ) ############ # Testing ## ############ -catkin_add_gtest(testPinocchioInterface +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(testPinocchioInterface test/testPinocchioInterface.cpp test/testPinocchioEndEffectorKinematics.cpp ) target_link_libraries(testPinocchioInterface gtest_main ${PROJECT_NAME} - ${catkin_LIBRARIES} + pinocchio::pinocchio + urdfdom_model + urdfdom_world + urdfdom_sensor + urdfdom_model_state ) +endif() + +ament_package(CONFIG_EXTRAS "cmake/pinocchio_config.cmake") diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/cmake/pinocchio_config.cmake b/ocs2_pinocchio/ocs2_pinocchio_interface/cmake/pinocchio_config.cmake index cca51d551..a23827547 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/cmake/pinocchio_config.cmake +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/cmake/pinocchio_config.cmake @@ -1,14 +1,8 @@ -# Add pinocchio library to the linker's search path -link_directories( - ${pinocchio_LIBRARY_DIRS} -) - # Add pinocchio flags set(OCS2_PINOCCHIO_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 -) \ No newline at end of file +) diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/package.xml b/ocs2_pinocchio/ocs2_pinocchio_interface/package.xml index 25503afcb..463222442 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/package.xml +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/package.xml @@ -1,5 +1,5 @@ - + ocs2_pinocchio_interface 0.0.0 The ocs2_pinocchio_interface package @@ -9,11 +9,17 @@ TODO - catkin - cmake_clang_tools + ament_cmake ocs2_core ocs2_robotic_tools pinocchio + urdfdom + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematics.cpp b/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematics.cpp index 49a1a9ec8..0e343dd61 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematics.cpp +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematics.cpp @@ -129,7 +129,7 @@ std::vector PinocchioEndEffectorKinematics::g std::vector positions; for (const auto& frameId : endEffectorFrameIds_) { - matrix_t J = matrix_t::Zero(6, model.nq); + matrix_t J = matrix_t::Zero(6, model.nv); pinocchio::getFrameJacobian(model, data, frameId, rf, J); VectorFunctionLinearApproximation pos; @@ -215,7 +215,7 @@ std::vector PinocchioEndEffectorKinematics::g const size_t frameId = endEffectorFrameIds_[i]; const quaternion_t q = matrixToQuaternion(data.oMf[frameId].rotation()); err.f = quaternionDistance(q, referenceOrientations[i]); - matrix_t J = matrix_t::Zero(6, model.nq); + matrix_t J = matrix_t::Zero(6, model.nv); pinocchio::getFrameJacobian(model, data, frameId, rf, J); const matrix_t Jqdist = (quaternionDistanceJacobian(q, referenceOrientations[i]) * angularVelocityToQuaternionTimeDerivative(q)) * J.bottomRows<3>(); diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioInterface.cpp b/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioInterface.cpp index 4a9e441da..56c593853 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioInterface.cpp +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioInterface.cpp @@ -75,7 +75,7 @@ std::ostream& operator<<(std::ostream& os, const PinocchioInterface& p) { for (int k = 0; k < model.nframes; ++k) { os << std::setw(20) << model.frames[k].name << ": "; os << " ID = " << k; - os << ", parent = " << model.frames[k].parent; + os << ", parentJoint = " << model.frames[k].parentJoint; os << ", type = "; std::string frameType; diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/src/urdf.cpp b/ocs2_pinocchio/ocs2_pinocchio_interface/src/urdf.cpp index f8f1b6d04..da311e647 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/src/urdf.cpp +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/src/urdf.cpp @@ -31,7 +31,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include +#include #include "ocs2_pinocchio_interface/urdf.h" diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/test/testPinocchioEndEffectorKinematics.cpp b/ocs2_pinocchio/ocs2_pinocchio_interface/test/testPinocchioEndEffectorKinematics.cpp index d226f10ae..8223d5462 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/test/testPinocchioEndEffectorKinematics.cpp +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/test/testPinocchioEndEffectorKinematics.cpp @@ -30,8 +30,12 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include +#include +#include +#include #include #include @@ -131,7 +135,7 @@ TEST_F(TestEndEffectorKinematics, testKinematicsPosition) { const auto id = model.getBodyId("WRIST_2"); const ocs2::vector_t pos = data.oMf[id].translation(); - ocs2::matrix_t J = ocs2::matrix_t::Zero(6, model.nq); + ocs2::matrix_t J = ocs2::matrix_t::Zero(6, model.nv); pinocchio::getFrameJacobian(model, data, id, pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, J); eeKinematicsPtr->setPinocchioInterface(*pinocchioInterfacePtr); @@ -143,6 +147,75 @@ TEST_F(TestEndEffectorKinematics, testKinematicsPosition) { EXPECT_TRUE(J.topRows<3>().isApprox(eePosLin.dfdx)); } +class FreeFlyerTangentMapping final : public ocs2::PinocchioStateInputMapping { + public: + using scalar_t = ocs2::scalar_t; + using vector_t = Eigen::Matrix; + using matrix_t = Eigen::Matrix; + + FreeFlyerTangentMapping() = default; + ~FreeFlyerTangentMapping() override = default; + FreeFlyerTangentMapping* clone() const override { return new FreeFlyerTangentMapping(*this); } + + void setPinocchioInterface(const ocs2::PinocchioInterfaceTpl& pinocchioInterface) override { + pinocchioInterfacePtr_ = &pinocchioInterface; + q0_ = pinocchio::neutral(pinocchioInterface.getModel()); + } + + vector_t getPinocchioJointPosition(const vector_t& state) const override { + if (pinocchioInterfacePtr_ == nullptr) { + throw std::runtime_error("[FreeFlyerTangentMapping] pinocchio interface is not set."); + } + if (state.rows() != pinocchioInterfacePtr_->getModel().nv) { + throw std::runtime_error("[FreeFlyerTangentMapping] state size must equal model.nv (tangent dimension)."); + } + return pinocchio::integrate(pinocchioInterfacePtr_->getModel(), q0_, state); + } + + vector_t getPinocchioJointVelocity(const vector_t& /*state*/, const vector_t& input) const override { return input; } + + std::pair getOcs2Jacobian(const vector_t& /*state*/, const matrix_t& Jq, const matrix_t& Jv) const override { + return {Jq, Jv}; + } + + private: + const ocs2::PinocchioInterfaceTpl* pinocchioInterfacePtr_ = nullptr; + vector_t q0_; +}; + +TEST(PinocchioEndEffectorKinematics, testPositionLinearApproximation_nqNotEqualNv) { + // Build a model with nq != nv by attaching a translation + quaternion spherical joint at the root. + pinocchio::JointModelComposite rootJoint(2); + rootJoint.addJoint(pinocchio::JointModelTranslation()); + rootJoint.addJoint(pinocchio::JointModelSpherical()); // nq=4, nv=3 + + auto pinocchioInterface = ocs2::getPinocchioInterfaceFromUrdfString(manipulatorArmUrdf, rootJoint); + + const auto& model = pinocchioInterface.getModel(); + auto& data = pinocchioInterface.getData(); + ASSERT_NE(model.nq, model.nv); + + FreeFlyerTangentMapping mapping; + ocs2::PinocchioEndEffectorKinematics eeKinematics(pinocchioInterface, mapping, {"WRIST_2"}); + + // Evaluate at the neutral configuration (x=0 in tangent space). + const ocs2::vector_t x = ocs2::vector_t::Zero(model.nv); + const ocs2::vector_t q = pinocchio::neutral(model); + pinocchio::forwardKinematics(model, data, q); + pinocchio::updateFramePlacements(model, data); + pinocchio::computeJointJacobians(model, data); + + eeKinematics.setPinocchioInterface(pinocchioInterface); + + const pinocchio::ReferenceFrame rf = pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED; + ocs2::matrix_t J = ocs2::matrix_t::Zero(6, model.nv); + pinocchio::Data dataCopy(pinocchioInterface.getData()); + pinocchio::getFrameJacobian(model, dataCopy, model.getBodyId("WRIST_2"), rf, J); + + const auto eePosLin = eeKinematics.getPositionLinearApproximation(x)[0]; + EXPECT_TRUE(J.topRows<3>().isApprox(eePosLin.dfdx)); +} + TEST_F(TestEndEffectorKinematics, testPosition) { const auto& model = pinocchioInterfacePtr->getModel(); auto& data = pinocchioInterfacePtr->getData(); diff --git a/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt b/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt index 37e06a0e8..d8fe54374 100644 --- a/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt @@ -1,66 +1,27 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_self_collision) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_robotic_tools - ocs2_pinocchio_interface -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(pinocchio REQUIRED pinocchio) -pkg_check_modules(hpp-fcl REQUIRED hpp-fcl) -# requires liboctomap-dev and libassimp-dev - +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(pinocchio REQUIRED) +find_package(hpp-fcl REQUIRED) +find_package(tinyxml2 REQUIRED) 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 ## -########### +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) 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} src/PinocchioGeometryInterface.cpp @@ -69,43 +30,64 @@ add_library(${PROJECT_NAME} src/SelfCollisionConstraint.cpp src/SelfCollisionConstraintCppAd.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${pinocchio_LIBRARIES} + pinocchio::pinocchio + hpp-fcl::hpp-fcl + tinyxml2::tinyxml2 +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${pinocchio_INCLUDE_DIRS} + ${hpp-fcl_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_robotic_tools + ocs2_pinocchio_interface ) target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) -#################### -## Clang tooling ### -#################### - -find_package(cmake_clang_tools QUIET) -if (cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -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} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} +) + +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) + +ament_export_include_directories( + include + ${pinocchio_INCLUDE_DIRS} + ${hpp-fcl_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_robotic_tools + ocs2_pinocchio_interface + pinocchio + hpp-fcl + tinyxml2 + Eigen3 + Boost ) +ament_package() ############ # Testing ## ############ diff --git a/ocs2_pinocchio/ocs2_self_collision/package.xml b/ocs2_pinocchio/ocs2_self_collision/package.xml index 4935695bb..b7abfef43 100644 --- a/ocs2_pinocchio/ocs2_self_collision/package.xml +++ b/ocs2_pinocchio/ocs2_self_collision/package.xml @@ -1,5 +1,5 @@ - + ocs2_self_collision 0.0.0 The ocs2_self_collision package @@ -9,12 +9,18 @@ TODO - catkin - cmake_clang_tools + ament_cmake ocs2_core ocs2_robotic_tools ocs2_pinocchio_interface pinocchio - + hpp-fcl + tinyxml2 + eigen3_cmake_module + boost + + ament_cmake + + diff --git a/ocs2_pinocchio/ocs2_self_collision/src/PinocchioGeometryInterface.cpp b/ocs2_pinocchio/ocs2_self_collision/src/PinocchioGeometryInterface.cpp index 41bf223a9..35099a119 100644 --- a/ocs2_pinocchio/ocs2_self_collision/src/PinocchioGeometryInterface.cpp +++ b/ocs2_pinocchio/ocs2_self_collision/src/PinocchioGeometryInterface.cpp @@ -32,6 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include #include @@ -39,6 +40,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include namespace ocs2 { @@ -92,12 +94,13 @@ void PinocchioGeometryInterface::buildGeomFromPinocchioInterface(const Pinocchio throw std::runtime_error("The PinocchioInterface passed to PinocchioGeometryInterface(...) does not contain a urdf model!"); } - // TODO: Replace with pinocchio function that uses the ModelInterface directly - // As of 19-04-21 there is no buildGeom that takes a ModelInterface, so we deconstruct the modelInterface into a std::stringstream first - const std::unique_ptr urdfAsXml(urdf::exportURDF(*pinocchioInterface.getUrdfModelPtr())); - TiXmlPrinter printer; - urdfAsXml->Accept(&printer); - const std::stringstream urdfAsStringStream(printer.Str()); + // TODO: Replace with pinocchio function that uses the ModelInterface directly. + // Pinocchio still expects a URDF stream here, so export the URDFDOM model to a tinyxml2 document first. + const std::unique_ptr urdfAsXml(urdf::exportURDF(*pinocchioInterface.getUrdfModelPtr())); + tinyxml2::XMLPrinter printer; + urdfAsXml->Print(&printer); + std::stringstream urdfAsStringStream; + urdfAsStringStream << printer.CStr(); pinocchio::urdf::buildGeom(pinocchioInterface.getModel(), urdfAsStringStream, pinocchio::COLLISION, geomModel); } diff --git a/ocs2_pinocchio/ocs2_self_collision_visualization/CMakeLists.txt b/ocs2_pinocchio/ocs2_self_collision_visualization/CMakeLists.txt index b61f92809..8509d6a75 100644 --- a/ocs2_pinocchio/ocs2_self_collision_visualization/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_self_collision_visualization/CMakeLists.txt @@ -1,111 +1,93 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_self_collision_visualization) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(ocs2_self_collision REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) + +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) + +find_package(pinocchio REQUIRED) +find_package(hpp-fcl REQUIRED) +find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -set(CATKIN_PACKAGE_DEPENDENCIES +add_library(${PROJECT_NAME} + src/GeometryInterfaceVisualization.cpp +) +target_link_libraries(${PROJECT_NAME} + pinocchio::pinocchio + hpp-fcl::hpp-fcl +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${pinocchio_INCLUDE_DIRS} + ${hpp-fcl_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} ocs2_core ocs2_robotic_tools ocs2_pinocchio_interface ocs2_self_collision ocs2_ros_interfaces - - # visualization deps - roscpp + rclcpp std_msgs geometry_msgs visualization_msgs ) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_PINOCCHIO_FLAGS}) -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(pinocchio REQUIRED pinocchio) -pkg_check_modules(hpp-fcl REQUIRED hpp-fcl) -# requires liboctomap-dev and libassimp-dev - -find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -################################### -## catkin specific configuration ## -################################### +############# +## Install ## +############# -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - pinocchio +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) - -########### -## Build ## -########### - -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 +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} +) +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -include_directories( +ament_export_include_directories( include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} ${pinocchio_INCLUDE_DIRS} ${hpp-fcl_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} ) - -link_directories( - ${pinocchio_LIBRARY_DIRS} -) - -# ocs2 pinocchio interface library -add_library(${PROJECT_NAME} - src/GeometryInterfaceVisualization.cpp -) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${pinocchio_LIBRARIES} +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_robotic_tools + ocs2_pinocchio_interface + ocs2_self_collision + ocs2_ros_interfaces + rclcpp + std_msgs + geometry_msgs + visualization_msgs + pinocchio + hpp-fcl + Eigen3 + Boost ) -target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) - -#################### -## Clang tooling ### -#################### - -find_package(cmake_clang_tools QUIET) -if (cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -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(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) +ament_package() diff --git a/ocs2_pinocchio/ocs2_self_collision_visualization/include/ocs2_self_collision_visualization/GeometryInterfaceVisualization.h b/ocs2_pinocchio/ocs2_self_collision_visualization/include/ocs2_self_collision_visualization/GeometryInterfaceVisualization.h index 5fa6ce7d3..264113daa 100644 --- a/ocs2_pinocchio/ocs2_self_collision_visualization/include/ocs2_self_collision_visualization/GeometryInterfaceVisualization.h +++ b/ocs2_pinocchio/ocs2_self_collision_visualization/include/ocs2_self_collision_visualization/GeometryInterfaceVisualization.h @@ -29,16 +29,19 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include - #include #include +#include + +#include "rclcpp/rclcpp.hpp" + namespace ocs2 { -class GeometryInterfaceVisualization { +class GeometryInterfaceVisualization : public rclcpp::Node { public: - GeometryInterfaceVisualization(PinocchioInterface pinocchioInterface, PinocchioGeometryInterface geometryInterface, ros::NodeHandle& nh, + GeometryInterfaceVisualization(PinocchioInterface pinocchioInterface, + PinocchioGeometryInterface geometryInterface, std::string pinocchioWorldFrame = "world"); virtual ~GeometryInterfaceVisualization() = default; @@ -48,7 +51,8 @@ class GeometryInterfaceVisualization { PinocchioInterface pinocchioInterface_; PinocchioGeometryInterface geometryInterface_; - ros::Publisher markerPublisher_; + rclcpp::Publisher::SharedPtr + markerPublisher_; std::string pinocchioWorldFrame_; }; diff --git a/ocs2_pinocchio/ocs2_self_collision_visualization/package.xml b/ocs2_pinocchio/ocs2_self_collision_visualization/package.xml index 46cb2614b..52751fddc 100644 --- a/ocs2_pinocchio/ocs2_self_collision_visualization/package.xml +++ b/ocs2_pinocchio/ocs2_self_collision_visualization/package.xml @@ -1,5 +1,5 @@ - + ocs2_self_collision_visualization 0.0.0 The ocs2_self_collision_visualization package @@ -8,10 +8,9 @@ TODO - catkin - cmake_clang_tools + ament_cmake - roscpp + rclcpp std_msgs geometry_msgs visualization_msgs @@ -20,4 +19,12 @@ ocs2_pinocchio_interface ocs2_ros_interfaces ocs2_self_collision + pinocchio + hpp-fcl + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_pinocchio/ocs2_self_collision_visualization/src/GeometryInterfaceVisualization.cpp b/ocs2_pinocchio/ocs2_self_collision_visualization/src/GeometryInterfaceVisualization.cpp index 291bacd40..ffe9b6226 100644 --- a/ocs2_pinocchio/ocs2_self_collision_visualization/src/GeometryInterfaceVisualization.cpp +++ b/ocs2_pinocchio/ocs2_self_collision_visualization/src/GeometryInterfaceVisualization.cpp @@ -27,28 +27,29 @@ 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 "ocs2_self_collision_visualization/GeometryInterfaceVisualization.h" #include -#include "ocs2_self_collision_visualization/GeometryInterfaceVisualization.h" +#include +#include +#include namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -GeometryInterfaceVisualization::GeometryInterfaceVisualization(PinocchioInterface pinocchioInterface, - PinocchioGeometryInterface geometryInterface, ros::NodeHandle& nh, - std::string pinocchioWorldFrame) - : pinocchioInterface_(std::move(pinocchioInterface)), +GeometryInterfaceVisualization::GeometryInterfaceVisualization( + PinocchioInterface pinocchioInterface, + PinocchioGeometryInterface geometryInterface, + std::string pinocchioWorldFrame) + : Node("GeometryInterfaceVisualization"), + pinocchioInterface_(std::move(pinocchioInterface)), geometryInterface_(std::move(geometryInterface)), - markerPublisher_(nh.advertise("distance_markers", 1, true)), + markerPublisher_( + this->create_publisher( + "distance_markers", 1)), pinocchioWorldFrame_(std::move(pinocchioWorldFrame)) {} /******************************************************************************************************/ @@ -60,70 +61,103 @@ void GeometryInterfaceVisualization::publishDistances(const ocs2::vector_t& q) { pinocchio::forwardKinematics(model, data, q); const auto results = geometryInterface_.computeDistances(pinocchioInterface_); - visualization_msgs::MarkerArray markerArray; + visualization_msgs::msg::MarkerArray markerArray; constexpr size_t numMarkersPerResult = 5; - visualization_msgs::Marker markerTemplate; + visualization_msgs::msg::Marker markerTemplate; markerTemplate.color = ros_msg_helpers::getColor({0, 1, 0}, 1); markerTemplate.header.frame_id = pinocchioWorldFrame_; - markerTemplate.header.stamp = ros::Time::now(); - markerTemplate.pose.orientation = ros_msg_helpers::getOrientationMsg({1, 0, 0, 0}); - markerArray.markers.resize(results.size() * numMarkersPerResult, markerTemplate); + markerTemplate.header.stamp = rclcpp::Clock().now(); + markerTemplate.pose.orientation = + ros_msg_helpers::getOrientationMsg({1, 0, 0, 0}); + markerArray.markers.resize(results.size() * numMarkersPerResult, + markerTemplate); for (size_t i = 0; i < results.size(); ++i) { - // I apologize for the magic numbers, it's mostly just visualization numbers(so 0.02 scale corresponds rougly to 0.02 cm) + // I apologize for the magic numbers, it's mostly just visualization + // numbers(so 0.02 scale corresponds rougly to 0.02 cm) for (size_t j = 0; j < numMarkersPerResult; ++j) { - markerArray.markers[numMarkersPerResult * i + j].ns = std::to_string(geometryInterface_.getGeometryModel().collisionPairs[i].first) + - " - " + - std::to_string(geometryInterface_.getGeometryModel().collisionPairs[i].second); + markerArray.markers[numMarkersPerResult * i + j].ns = + std::to_string( + geometryInterface_.getGeometryModel().collisionPairs[i].first) + + " - " + + std::to_string( + geometryInterface_.getGeometryModel().collisionPairs[i].second); } // The actual distance line, also denoting direction of the distance - markerArray.markers[numMarkersPerResult * i].type = visualization_msgs::Marker::ARROW; - markerArray.markers[numMarkersPerResult * i].points.push_back(ros_msg_helpers::getPointMsg(results[i].nearest_points[0])); - markerArray.markers[numMarkersPerResult * i].points.push_back(ros_msg_helpers::getPointMsg(results[i].nearest_points[1])); + markerArray.markers[numMarkersPerResult * i].type = + visualization_msgs::msg::Marker::ARROW; + markerArray.markers[numMarkersPerResult * i].points.push_back( + ros_msg_helpers::getPointMsg(results[i].nearest_points[0])); + markerArray.markers[numMarkersPerResult * i].points.push_back( + ros_msg_helpers::getPointMsg(results[i].nearest_points[1])); markerArray.markers[numMarkersPerResult * i].id = numMarkersPerResult * i; markerArray.markers[numMarkersPerResult * i].scale.x = 0.01; markerArray.markers[numMarkersPerResult * i].scale.y = 0.02; markerArray.markers[numMarkersPerResult * i].scale.z = 0.04; // Dots at the end of the arrow, denoting the close locations on the body - markerArray.markers[numMarkersPerResult * i + 1].type = visualization_msgs::Marker::SPHERE_LIST; - markerArray.markers[numMarkersPerResult * i + 1].points.push_back(ros_msg_helpers::getPointMsg(results[i].nearest_points[0])); - markerArray.markers[numMarkersPerResult * i + 1].points.push_back(ros_msg_helpers::getPointMsg(results[i].nearest_points[1])); + markerArray.markers[numMarkersPerResult * i + 1].type = + visualization_msgs::msg::Marker::SPHERE_LIST; + markerArray.markers[numMarkersPerResult * i + 1].points.push_back( + ros_msg_helpers::getPointMsg(results[i].nearest_points[0])); + markerArray.markers[numMarkersPerResult * i + 1].points.push_back( + ros_msg_helpers::getPointMsg(results[i].nearest_points[1])); markerArray.markers[numMarkersPerResult * i + 1].scale.x = 0.02; markerArray.markers[numMarkersPerResult * i + 1].scale.y = 0.02; markerArray.markers[numMarkersPerResult * i + 1].scale.z = 0.02; - markerArray.markers[numMarkersPerResult * i + 1].id = numMarkersPerResult * i + 1; - // Text denoting the object number in the geometry model, raised above the spheres - markerArray.markers[numMarkersPerResult * i + 2].id = numMarkersPerResult * i + 2; - markerArray.markers[numMarkersPerResult * i + 2].type = visualization_msgs::Marker::TEXT_VIEW_FACING; + markerArray.markers[numMarkersPerResult * i + 1].id = + numMarkersPerResult * i + 1; + // Text denoting the object number in the geometry model, raised above the + // spheres + markerArray.markers[numMarkersPerResult * i + 2].id = + numMarkersPerResult * i + 2; + markerArray.markers[numMarkersPerResult * i + 2].type = + visualization_msgs::msg::Marker::TEXT_VIEW_FACING; markerArray.markers[numMarkersPerResult * i + 2].scale.z = 0.02; - markerArray.markers[numMarkersPerResult * i + 2].pose.position = ros_msg_helpers::getPointMsg(results[i].nearest_points[0]); + markerArray.markers[numMarkersPerResult * i + 2].pose.position = + ros_msg_helpers::getPointMsg(results[i].nearest_points[0]); markerArray.markers[numMarkersPerResult * i + 2].pose.position.z += 0.015; markerArray.markers[numMarkersPerResult * i + 2].text = - "obj " + std::to_string(geometryInterface_.getGeometryModel().collisionPairs[i].first); - markerArray.markers[numMarkersPerResult * i + 3].id = numMarkersPerResult * i + 3; - markerArray.markers[numMarkersPerResult * i + 3].type = visualization_msgs::Marker::TEXT_VIEW_FACING; - markerArray.markers[numMarkersPerResult * i + 3].pose.position = ros_msg_helpers::getPointMsg(results[i].nearest_points[1]); + "obj " + + std::to_string( + geometryInterface_.getGeometryModel().collisionPairs[i].first); + markerArray.markers[numMarkersPerResult * i + 3].id = + numMarkersPerResult * i + 3; + markerArray.markers[numMarkersPerResult * i + 3].type = + visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + markerArray.markers[numMarkersPerResult * i + 3].pose.position = + ros_msg_helpers::getPointMsg(results[i].nearest_points[1]); markerArray.markers[numMarkersPerResult * i + 3].pose.position.z += 0.015; markerArray.markers[numMarkersPerResult * i + 3].text = - "obj " + std::to_string(geometryInterface_.getGeometryModel().collisionPairs[i].second); + "obj " + + std::to_string( + geometryInterface_.getGeometryModel().collisionPairs[i].second); markerArray.markers[numMarkersPerResult * i + 3].scale.z = 0.02; // Text above the arrow, denoting the distance - markerArray.markers[numMarkersPerResult * i + 4].id = numMarkersPerResult * i + 4; - markerArray.markers[numMarkersPerResult * i + 4].type = visualization_msgs::Marker::TEXT_VIEW_FACING; + markerArray.markers[numMarkersPerResult * i + 4].id = + numMarkersPerResult * i + 4; + markerArray.markers[numMarkersPerResult * i + 4].type = + visualization_msgs::msg::Marker::TEXT_VIEW_FACING; markerArray.markers[numMarkersPerResult * i + 4].pose.position = - ros_msg_helpers::getPointMsg((results[i].nearest_points[0] + results[i].nearest_points[1]) / 2.0); + ros_msg_helpers::getPointMsg( + (results[i].nearest_points[0] + results[i].nearest_points[1]) / + 2.0); markerArray.markers[numMarkersPerResult * i + 4].pose.position.z += 0.015; markerArray.markers[numMarkersPerResult * i + 4].text = - "dist " + std::to_string(geometryInterface_.getGeometryModel().collisionPairs[i].first) + " - " + - std::to_string(geometryInterface_.getGeometryModel().collisionPairs[i].second) + ": " + std::to_string(results[i].min_distance); + "dist " + + std::to_string( + geometryInterface_.getGeometryModel().collisionPairs[i].first) + + " - " + + std::to_string( + geometryInterface_.getGeometryModel().collisionPairs[i].second) + + ": " + std::to_string(results[i].min_distance); markerArray.markers[numMarkersPerResult * i + 4].scale.z = 0.02; } - markerPublisher_.publish(markerArray); + markerPublisher_->publish(markerArray); } } // namespace ocs2 diff --git a/ocs2_pinocchio/ocs2_sphere_approximation/CMakeLists.txt b/ocs2_pinocchio/ocs2_sphere_approximation/CMakeLists.txt index c72f39302..bdf025247 100644 --- a/ocs2_pinocchio/ocs2_sphere_approximation/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_sphere_approximation/CMakeLists.txt @@ -1,117 +1,95 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_sphere_approximation) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_robotic_tools - ocs2_pinocchio_interface - ocs2_robotic_assets -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(pinocchio REQUIRED pinocchio) - +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(pinocchio REQUIRED) +find_package(tinyxml2 REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - pinocchio -) - -########### -## Build ## -########### - -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} -) - -link_directories( - ${pinocchio_LIBRARY_DIRS} -) - -# ocs2 sphere approximation library add_library(${PROJECT_NAME} src/SphereApproximation.cpp src/PinocchioSphereInterface.cpp src/PinocchioSphereKinematics.cpp src/PinocchioSphereKinematicsCppAd.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${pinocchio_LIBRARIES} + pinocchio::pinocchio + tinyxml2::tinyxml2 ) -target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) - -#################### -## Clang tooling ### -#################### - -find_package(cmake_clang_tools QUIET) -if (cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif (cmake_clang_tools_FOUND) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${pinocchio_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_robotic_tools + ocs2_pinocchio_interface + ocs2_robotic_assets +) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_PINOCCHIO_FLAGS}) ############# ## Install ## ############# 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} + DESTINATION include/${PROJECT_NAME} +) +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) + +ament_export_include_directories( + include + ${pinocchio_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_robotic_tools + ocs2_pinocchio_interface + ocs2_robotic_assets + Eigen3 + Boost + pinocchio + tinyxml2 ) ############ # Testing ## ############ -catkin_add_gtest(PinocchioSphereKinematicsTest - test/testPinocchioSphereKinematics.cpp -) -target_link_libraries(PinocchioSphereKinematicsTest - gtest_main - ${PROJECT_NAME} - ${catkin_LIBRARIES} -) \ No newline at end of file +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(PinocchioSphereKinematicsTest + test/testPinocchioSphereKinematics.cpp + ) + target_link_libraries(PinocchioSphereKinematicsTest + gtest_main + ${PROJECT_NAME} + pinocchio::pinocchio + ) + target_compile_options(PinocchioSphereKinematicsTest PRIVATE ${OCS2_PINOCCHIO_FLAGS}) +endif() + +ament_package() diff --git a/ocs2_pinocchio/ocs2_sphere_approximation/package.xml b/ocs2_pinocchio/ocs2_sphere_approximation/package.xml index 934ba99ee..21ca1dd0d 100644 --- a/ocs2_pinocchio/ocs2_sphere_approximation/package.xml +++ b/ocs2_pinocchio/ocs2_sphere_approximation/package.xml @@ -1,5 +1,5 @@ - + ocs2_sphere_approximation 0.0.0 The ocs2_sphere_approximation package @@ -8,12 +8,20 @@ TODO - catkin - cmake_clang_tools + ament_cmake ocs2_core ocs2_robotic_tools ocs2_pinocchio_interface ocs2_robotic_assets pinocchio - \ No newline at end of file + tinyxml2 + eigen3_cmake_module + boost + + ament_cmake_gtest + + ament_cmake + + + diff --git a/ocs2_pinocchio/ocs2_sphere_approximation/src/PinocchioSphereInterface.cpp b/ocs2_pinocchio/ocs2_sphere_approximation/src/PinocchioSphereInterface.cpp index 17f562deb..0f6b97eaf 100644 --- a/ocs2_pinocchio/ocs2_sphere_approximation/src/PinocchioSphereInterface.cpp +++ b/ocs2_pinocchio/ocs2_sphere_approximation/src/PinocchioSphereInterface.cpp @@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include @@ -109,12 +110,13 @@ void PinocchioSphereInterface::buildGeomFromPinocchioInterface(const PinocchioIn "does not contain a urdf model!"); } - // TODO: Replace with pinocchio function that uses the ModelInterface directly - // As of 19-04-21 there is no buildGeom that takes a ModelInterface, so we deconstruct the modelInterface into a std::stringstream first - const std::unique_ptr urdfAsXml(urdf::exportURDF(*pinocchioInterface.getUrdfModelPtr())); - TiXmlPrinter printer; - urdfAsXml->Accept(&printer); - const std::stringstream urdfAsStringStream(printer.Str()); + // TODO: Replace with pinocchio function that uses the ModelInterface directly. + // Pinocchio still expects a URDF stream here, so export the URDFDOM model to a tinyxml2 document first. + const std::unique_ptr urdfAsXml(urdf::exportURDF(*pinocchioInterface.getUrdfModelPtr())); + tinyxml2::XMLPrinter printer; + urdfAsXml->Print(&printer); + std::stringstream urdfAsStringStream; + urdfAsStringStream << printer.CStr(); pinocchio::urdf::buildGeom(pinocchioInterface.getModel(), urdfAsStringStream, pinocchio::COLLISION, geomModel); } diff --git a/ocs2_pinocchio/ocs2_sphere_approximation/test/testPinocchioSphereKinematics.cpp b/ocs2_pinocchio/ocs2_sphere_approximation/test/testPinocchioSphereKinematics.cpp index 137d60254..9e61b8657 100644 --- a/ocs2_pinocchio/ocs2_sphere_approximation/test/testPinocchioSphereKinematics.cpp +++ b/ocs2_pinocchio/ocs2_sphere_approximation/test/testPinocchioSphereKinematics.cpp @@ -76,10 +76,10 @@ class TestSphereKinematics : public ::testing::Test { {0.20, 0.10, 0.05, 0.05}, 0.7)); sphereKinematicsPtr.reset(new ocs2::PinocchioSphereKinematics(*pinocchioSphereInterfacePtr, pinocchioMapping)); sphereKinematicsCppAdPtr.reset(new ocs2::PinocchioSphereKinematicsCppAd( - *pinocchioInterfacePtr, *pinocchioSphereInterfacePtr, pinocchioMappingCppAd, pinocchioInterfacePtr->getModel().njoints, 0, + *pinocchioInterfacePtr, *pinocchioSphereInterfacePtr, pinocchioMappingCppAd, pinocchioInterfacePtr->getModel().nq, 0, "pinocchio_sphere_kinematics", "/tmp/ocs2", true, true)); - x.resize(pinocchioInterfacePtr->getModel().njoints); + x.resize(pinocchioInterfacePtr->getModel().nq); // taken form config/mpc/task.info x(0) = 2.5; // SH_ROT x(1) = -1.0; // SH_FLE @@ -201,4 +201,4 @@ TEST_F(TestSphereKinematics, testClone) { const auto spherePos = clonePtr->getPosition(x)[0]; const auto spherePosAd = cloneCppAdPtr->getPosition(x)[0]; EXPECT_TRUE(spherePos.isApprox(spherePosAd)); -} \ No newline at end of file +} diff --git a/ocs2_python_interface/CMakeLists.txt b/ocs2_python_interface/CMakeLists.txt index 558ea0b8e..514e786de 100644 --- a/ocs2_python_interface/CMakeLists.txt +++ b/ocs2_python_interface/CMakeLists.txt @@ -1,72 +1,71 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_python_interface) -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_mpc - ocs2_ddp - ocs2_robotic_tools -) - -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ${CATKIN_PACKAGE_DEPENDENCIES} -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include +add_library(${PROJECT_NAME} + src/PythonInterface.cpp +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} ) - -########### -## Build ## -########### - -include_directories( - include - ${catkin_INCLUDE_DIRS} +ament_target_dependencies(${PROJECT_NAME} + ocs2_mpc + ocs2_ddp + ocs2_robotic_tools ) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -add_library(${PROJECT_NAME} - src/PythonInterface.cpp +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -############# -## Install ## -############# - -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) - -############# -## Testing ## -############# + DESTINATION include/${PROJECT_NAME} +) -catkin_add_gtest(testDummyPyBindings - test/testDummyPyBindings.cpp +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} ) -target_link_libraries(testDummyPyBindings - ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_mpc + ocs2_ddp + ocs2_robotic_tools + Eigen3 ) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(testDummyPyBindings + test/testDummyPyBindings.cpp + ) + target_link_libraries(testDummyPyBindings + ${PROJECT_NAME} + gtest_main + ) +endif() + +ament_package() diff --git a/ocs2_python_interface/package.xml b/ocs2_python_interface/package.xml index 993fced46..e544a9e4a 100644 --- a/ocs2_python_interface/package.xml +++ b/ocs2_python_interface/package.xml @@ -1,5 +1,5 @@ - + ocs2_python_interface 0.0.0 The ocs2_python_interface package @@ -10,13 +10,16 @@ TODO - catkin - - cmake_modules - cmake_clang_tools + ament_cmake ocs2_mpc ocs2_ddp ocs2_robotic_tools + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_raisim/COLCON_IGNORE b/ocs2_raisim/COLCON_IGNORE new file mode 100644 index 000000000..42c43e047 --- /dev/null +++ b/ocs2_raisim/COLCON_IGNORE @@ -0,0 +1 @@ +This subtree is not ported to ROS 2 Jazzy yet (requires RaiSim + ROS 2 integration work). diff --git a/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt b/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt index 577c679ce..eca23796a 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt @@ -1,153 +1,129 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_ballbot) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -set(CATKIN_PACKAGE_DEPENDENCIES - pybind11_catkin - ocs2_core - ocs2_mpc - ocs2_ddp - ocs2_slp - ocs2_sqp - ocs2_robotic_tools - ocs2_python_interface -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(Python3 REQUIRED COMPONENTS Interpreter Development.Module) +find_package(pybind11 REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_slp REQUIRED) +find_package(ocs2_sqp REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_python_interface REQUIRED) 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 ## -########### +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) # Resolve for the package path at compile time. -configure_file ( +configure_file( "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h.in" - "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY -) - -# Add directories for all targets -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} + "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" + @ONLY ) -# Ballbot interface library add_library(${PROJECT_NAME} src/BallbotInterface.cpp src/dynamics/BallbotSystemDynamics.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_mpc + ocs2_ddp + ocs2_slp + ocs2_sqp + ocs2_robotic_tools + ocs2_python_interface ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} + Boost::filesystem + Boost::system dl ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) - # python bindings -pybind11_add_module(BallbotPyBindings SHARED +pybind11_add_module(BallbotPyBindings src/pyBindModule.cpp ) -add_dependencies(BallbotPyBindings - ${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(BallbotPyBindings PRIVATE ${PROJECT_NAME} - ${catkin_LIBRARIES} -) -set_target_properties(BallbotPyBindings - PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} ) -catkin_python_setup() - - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target ocs2_ballbot") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_EXCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}/generated - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) +ament_python_install_package(${PROJECT_NAME} + PACKAGE_DIR src/${PROJECT_NAME} +) ############# ## Install ## ############# 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} + DESTINATION include/${PROJECT_NAME} ) - install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) - install(TARGETS BallbotPyBindings - ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} + LIBRARY DESTINATION ${PYTHON_INSTALL_DIR}/${PROJECT_NAME} ) -############# -## Testing ## -############# - -catkin_add_gtest(test_BallbotPyBindings - test/testBallbotPyBindings.cpp +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -target_include_directories(test_BallbotPyBindings PRIVATE + +ament_export_include_directories( + include ${PROJECT_BINARY_DIR}/include + ${EIGEN3_INCLUDE_DIRS} ) -target_link_libraries(test_BallbotPyBindings - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_mpc + ocs2_ddp + ocs2_slp + ocs2_sqp + ocs2_robotic_tools + ocs2_python_interface + Eigen3 + Boost + pybind11_vendor ) -# python tests -catkin_add_nosetests(test) +############ +# Testing ## +############ + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(test_BallbotPyBindings + test/testBallbotPyBindings.cpp + ) + target_include_directories(test_BallbotPyBindings PRIVATE + ${PROJECT_BINARY_DIR}/include + ) + target_link_libraries(test_BallbotPyBindings + ${PROJECT_NAME} + ) +endif() + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_ballbot/package.xml b/ocs2_robotic_examples/ocs2_ballbot/package.xml index 3c65f409c..4fd5e3ffd 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/package.xml +++ b/ocs2_robotic_examples/ocs2_ballbot/package.xml @@ -1,5 +1,5 @@ - + ocs2_ballbot 0.0.0 The ocs2_ballbot package @@ -10,10 +10,10 @@ TODO - catkin - cmake_clang_tools + ament_cmake + ament_cmake_python - pybind11_catkin + pybind11_vendor ocs2_core ocs2_mpc @@ -22,5 +22,12 @@ ocs2_sqp ocs2_python_interface ocs2_robotic_tools + eigen3_cmake_module + boost + + ament_cmake_gtest + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt index 99f6d33bf..cc15cf10b 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt @@ -1,201 +1,151 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_ballbot_ros) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(urdf REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(robot_state_publisher REQUIRED) + +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_sqp REQUIRED) +find_package(ocs2_slp REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(ocs2_ballbot REQUIRED) -set(CATKIN_PACKAGE_DEPENDENCIES - tf +find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) + +add_library(${PROJECT_NAME} + src/BallbotDummyVisualization.cpp +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ament_index_cpp + rclcpp + tf2_ros + sensor_msgs + geometry_msgs + visualization_msgs urdf kdl_parser robot_state_publisher - visualization_msgs ocs2_core + ocs2_oc ocs2_ddp ocs2_mpc ocs2_sqp ocs2_slp ocs2_ros_interfaces ocs2_robotic_tools - ocs2_ballbot ocs2_robotic_assets -) - -find_package(catkin REQUIRED COMPONENTS - roslib - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - -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 - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -# main library -add_library(${PROJECT_NAME} - src/BallbotDummyVisualization.cpp -) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} + ocs2_ballbot ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -# Mpc node -add_executable(ballbot_ddp - src/BallbotDdpMpcNode.cpp -) -add_dependencies(ballbot_ddp - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(ballbot_ddp - ${catkin_LIBRARIES} -) +add_executable(ballbot_ddp src/BallbotDdpMpcNode.cpp) +target_link_libraries(ballbot_ddp ${PROJECT_NAME}) target_compile_options(ballbot_ddp PRIVATE ${OCS2_CXX_FLAGS}) -# Dummy node -add_executable(ballbot_dummy_test - src/DummyBallbotNode.cpp -) -add_dependencies(ballbot_dummy_test - ${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(ballbot_dummy_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} -) +add_executable(ballbot_sqp src/BallbotSqpMpcNode.cpp) +target_link_libraries(ballbot_sqp ${PROJECT_NAME}) +target_compile_options(ballbot_sqp PRIVATE ${OCS2_CXX_FLAGS}) + +add_executable(ballbot_slp src/BallbotSlpMpcNode.cpp) +target_link_libraries(ballbot_slp ${PROJECT_NAME}) +target_compile_options(ballbot_slp PRIVATE ${OCS2_CXX_FLAGS}) + +add_executable(ballbot_dummy_test src/DummyBallbotNode.cpp) +target_link_libraries(ballbot_dummy_test ${PROJECT_NAME}) target_compile_options(ballbot_dummy_test PRIVATE ${OCS2_CXX_FLAGS}) -# Target node -add_executable(ballbot_target - src/BallbotTargetPoseCommand.cpp -) -add_dependencies(ballbot_target - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(ballbot_target - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} -) +add_executable(ballbot_target src/BallbotTargetPoseCommand.cpp) +target_link_libraries(ballbot_target ${PROJECT_NAME}) target_compile_options(ballbot_target PRIVATE ${OCS2_CXX_FLAGS}) -## Combined MPC MRT example -add_executable(ballbot_mpc_mrt - src/BallbotMpcMrtNode.cpp - src/BallbotDummyVisualization.cpp - ) -add_dependencies(ballbot_mpc_mrt - ${catkin_EXPORTED_TARGETS} - ) -target_link_libraries(ballbot_mpc_mrt - ${catkin_LIBRARIES} - ) -target_compile_options(ballbot_mpc_mrt PRIVATE ${OCS2_CXX_FLAGS}) - -## SQP node for ballbot -add_executable(ballbot_sqp - src/BallbotSqpMpcNode.cpp -) -add_dependencies(ballbot_sqp - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(ballbot_sqp - ${catkin_LIBRARIES} -) -target_compile_options(ballbot_sqp PRIVATE ${OCS2_CXX_FLAGS}) - -## SLP node for ballbot -add_executable(ballbot_slp - src/BallbotSlpMpcNode.cpp -) -add_dependencies(ballbot_slp - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(ballbot_slp - ${catkin_LIBRARIES} -) -target_compile_options(ballbot_slp PRIVATE ${OCS2_CXX_FLAGS}) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target ocs2_ballbot") - add_clang_tooling( - TARGETS - ${PROJECT_NAME} - ballbot_ddp - ballbot_sqp - ballbot_dummy_test - ballbot_target - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) +add_executable(ballbot_mpc_mrt src/BallbotMpcMrtNode.cpp) +target_link_libraries(ballbot_mpc_mrt ${PROJECT_NAME}) +target_compile_options(ballbot_mpc_mrt PRIVATE ${OCS2_CXX_FLAGS}) ############# ## Install ## ############# 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 + ballbot_ddp + ballbot_sqp + ballbot_slp + ballbot_dummy_test + ballbot_target + ballbot_mpc_mrt + RUNTIME DESTINATION lib/${PROJECT_NAME} ) -install(TARGETS ballbot_ddp ballbot_dummy_test ballbot_target ballbot_sqp - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} ) + install(DIRECTORY launch rviz - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) -############# -## Testing ## -############# - -catkin_add_gtest(ballbot_integration_test - test/ballbotIntegrationTest.cpp +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} ) -target_link_libraries(ballbot_integration_test - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ament_index_cpp + rclcpp + tf2_ros + sensor_msgs + geometry_msgs + visualization_msgs + urdf + kdl_parser + robot_state_publisher + ocs2_core + ocs2_oc + ocs2_ddp + ocs2_mpc + ocs2_sqp + ocs2_slp + ocs2_ros_interfaces + ocs2_robotic_tools + ocs2_robotic_assets + ocs2_ballbot + Eigen3 + Boost ) + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/include/ocs2_ballbot_ros/BallbotDummyVisualization.h b/ocs2_robotic_examples/ocs2_ballbot_ros/include/ocs2_ballbot_ros/BallbotDummyVisualization.h index dc3e25e8b..e37cd1758 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/include/ocs2_ballbot_ros/BallbotDummyVisualization.h +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/include/ocs2_ballbot_ros/BallbotDummyVisualization.h @@ -29,29 +29,30 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include -#include - +#include #include +#include -#include +#include +#include namespace ocs2 { namespace ballbot { class BallbotDummyVisualization final : public DummyObserver { public: - explicit BallbotDummyVisualization(ros::NodeHandle& nodeHandle) { launchVisualizerNode(nodeHandle); } + explicit BallbotDummyVisualization(const rclcpp::Node::SharedPtr& node); ~BallbotDummyVisualization() override = default; - void update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) override; + void update(const SystemObservation& observation, + const PrimalSolution& policy, + const CommandData& command) override; private: - void launchVisualizerNode(ros::NodeHandle& nodeHandle); - - std::unique_ptr robotStatePublisherPtr_; - tf::TransformBroadcaster tfBroadcaster_; + rclcpp::Node::SharedPtr node_; + tf2_ros::TransformBroadcaster tfBroadcaster_; + rclcpp::Publisher::SharedPtr jointPublisher_; }; } // namespace ballbot diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_ddp.launch.py b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_ddp.launch.py new file mode 100644 index 000000000..ec5ddb70f --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_ddp.launch.py @@ -0,0 +1,61 @@ +import os +from launch.substitutions import LaunchConfiguration + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='task_name', + default_value='mpc' + ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_ballbot_ros'), 'launch/visualize.launch.py') + ), + launch_arguments={ + 'use_joint_state_publisher': 'false' + }.items() + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_ddp', + name='ballbot_ddp', + prefix= "", + arguments=[LaunchConfiguration('task_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_dummy_test', + name='ballbot_dummy_test', + prefix=LaunchConfiguration('terminal_prefix'), + arguments=[LaunchConfiguration('task_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_target', + name='ballbot_target', + prefix=LaunchConfiguration('terminal_prefix'), + arguments=[LaunchConfiguration('task_name')], + output='screen' + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_mpc_mrt.launch.py b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_mpc_mrt.launch.py new file mode 100644 index 000000000..1a83f758e --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_mpc_mrt.launch.py @@ -0,0 +1,52 @@ +import os +from launch.substitutions import LaunchConfiguration + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='task_name', + default_value='mpc' + ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_ballbot_ros'), 'launch/visualize.launch.py') + ), + launch_arguments={ + 'use_joint_state_publisher': 'false' + }.items() + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_mpc_mrt', + name='ballbot_mpc_mrt', + arguments=[LaunchConfiguration('task_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_target', + name='ballbot_target', + prefix=LaunchConfiguration('terminal_prefix'), + arguments=[LaunchConfiguration('task_name')], + output='screen' + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_slp.launch.py b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_slp.launch.py new file mode 100644 index 000000000..83d4a39e4 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_slp.launch.py @@ -0,0 +1,61 @@ +import os +from launch.substitutions import LaunchConfiguration + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='task_name', + default_value='mpc' + ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_ballbot_ros'), 'launch/visualize.launch.py') + ), + launch_arguments={ + 'use_joint_state_publisher': 'false' + }.items() + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_slp', + name='ballbot_slp', + prefix= "", + arguments=[LaunchConfiguration('task_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_dummy_test', + name='ballbot_dummy_test', + prefix=LaunchConfiguration('terminal_prefix'), + arguments=[LaunchConfiguration('task_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_target', + name='ballbot_target', + prefix=LaunchConfiguration('terminal_prefix'), + arguments=[LaunchConfiguration('task_name')], + output='screen' + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_sqp.launch.py b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_sqp.launch.py new file mode 100644 index 000000000..e9d38815e --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_sqp.launch.py @@ -0,0 +1,61 @@ +import os +from launch.substitutions import LaunchConfiguration + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='task_name', + default_value='mpc' + ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_ballbot_ros'), 'launch/visualize.launch.py') + ), + launch_arguments={ + 'use_joint_state_publisher': 'false' + }.items() + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_sqp', + name='ballbot_sqp', + prefix= "", + arguments=[LaunchConfiguration('task_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_dummy_test', + name='ballbot_dummy_test', + prefix=LaunchConfiguration('terminal_prefix'), + arguments=[LaunchConfiguration('task_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_ballbot_ros', + executable='ballbot_target', + name='ballbot_target', + prefix=LaunchConfiguration('terminal_prefix'), + arguments=[LaunchConfiguration('task_name')], + output='screen' + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/multiplot.launch.py b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/multiplot.launch.py new file mode 100644 index 000000000..8fdbaf199 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/multiplot.launch.py @@ -0,0 +1,25 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_ros_interfaces'), 'launch/performance_indices.launch.py') + ), + launch_arguments={ + 'mpc_policy_topic_name': 'ballbot_mpc_policy' + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/visualize.launch.py b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/visualize.launch.py new file mode 100644 index 000000000..f602792dd --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/visualize.launch.py @@ -0,0 +1,50 @@ +import os +import launch +import launch_ros.actions +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +def generate_launch_description(): + rviz_config_file = get_package_share_directory('ocs2_ballbot_ros') + "/rviz/ballbot.rviz" + urdf_dir = get_package_share_directory("ocs2_robotic_assets") + urdf_model_path = os.path.join(urdf_dir, "resources/ballbot/urdf", "ballbot.urdf") + + use_joint_state_publisher_argument = launch.actions.DeclareLaunchArgument( + name='use_joint_state_publisher', + default_value='true' + ) + use_joint_state_publisher = LaunchConfiguration("use_joint_state_publisher") + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + arguments=[urdf_model_path], + ) + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + arguments=[urdf_model_path], + condition=IfCondition(use_joint_state_publisher), + ) + rviz_node = launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='ballbot', + output='screen', + arguments=["-d", rviz_config_file] + ) + + ld = launch.LaunchDescription([ + use_joint_state_publisher_argument, + robot_state_publisher_node, + joint_state_publisher_node, + rviz_node + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml b/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml index 00a7b00b6..9bc3330dc 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml @@ -1,5 +1,5 @@ - + ocs2_ballbot_ros 0.0.0 The ocs2_ballbot_ros package @@ -10,24 +10,33 @@ TODO - catkin - cmake_clang_tools + ament_cmake - tf + ament_index_cpp + rclcpp + tf2_ros + sensor_msgs + geometry_msgs + visualization_msgs urdf kdl_parser robot_state_publisher - visualization_msgs + ocs2_ros_interfaces ocs2_core ocs2_oc ocs2_ddp ocs2_mpc ocs2_sqp ocs2_slp - ocs2_ros_interfaces ocs2_ballbot ocs2_robotic_tools ocs2_robotic_assets + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/rviz/ballbot.rviz b/ocs2_robotic_examples/ocs2_ballbot_ros/rviz/ballbot.rviz index 1ae86a94e..97963d518 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/rviz/ballbot.rviz +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/rviz/ballbot.rviz @@ -1,50 +1,48 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 + - /Grid1 - /TF1/Frames1 - /TF1/Tree1 + - /RobotModel1 + - /RobotModel1/Description Topic1 Splitter Ratio: 0.5 - Tree Height: 745 - - Class: rviz/Selection + Tree Height: 507 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 SyncSource: "" - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 Tree Height: 363 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -58,9 +56,9 @@ Visualization Manager: Z: 0 Plane: XY Plane Cell Count: 10 - Reference Frame: odom + Reference Frame: Value: true - - Class: rviz/TF + - Class: rviz_default_plugins/TF Enabled: true Frame Timeout: 15 Frames: @@ -69,16 +67,12 @@ Visualization Manager: Value: false base: Value: true - command: - Value: true dummy_ball1: Value: false dummy_base1: Value: false dummy_base2: Value: false - odom: - Value: true world: Value: true world_inertia: @@ -89,30 +83,39 @@ Visualization Manager: Show Axes: true Show Names: false Tree: - odom: - command: - {} - world: - world_inertia: - dummy_ball1: - ball: - dummy_base1: - dummy_base2: - base: - {} + world: + world_inertia: + dummy_ball1: + ball: + dummy_base1: + dummy_base2: + base: + {} Update Interval: 0 Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true - Marker Topic: /ballbot_vis Name: MarkerArray Namespaces: {} - Queue Size: 100 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array Value: true - Alpha: 1 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description Enabled: true Links: All Links Enabled: true @@ -150,8 +153,10 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false + Mass Properties: + Inertia: false + Mass: false Name: RobotModel - Robot Description: robot_description TF Prefix: "" Update Interval: 0 Value: true @@ -173,36 +178,59 @@ Visualization Manager: Min Intensity: 0 Name: GridMap Show Grid Lines: true - Topic: /raisim_heightmap - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /raisim_heightmap Use Rainbow: true Value: true Enabled: true Global Options: Background Color: 238; 238; 238 - Default Light: true Fixed Frame: world Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/Select - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit + Class: rviz_default_plugins/Orbit Distance: 5.161567687988281 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -226,10 +254,10 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 974 + Height: 736 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000015a00000374fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000374000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000fb000000100044006900730070006c006100790073000000011300000219000000c900ffffff000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d0065000000000000000640000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004d90000037400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000015a00000286fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000286000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000fb000000100044006900730070006c006100790073000000011300000219000000c900ffffff000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d0065000000000000000640000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000035a0000028600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -238,6 +266,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1593 - X: 3318 - Y: 138 + Width: 1210 + X: 70 + Y: 27 diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDdpMpcNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDdpMpcNode.cpp index 627cf6d49..38646fb8c 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDdpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDdpMpcNode.cpp @@ -27,47 +27,57 @@ 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 #include -#include +#include + +#include + +#include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { const std::string robotName = "ballbot"; // task file - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName = std::string(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mpc"); // Robot interface - const std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_ballbot") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_ballbot_auto_generated"); ocs2::ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); // ROS ReferenceManager - auto rosReferenceManagerPtr = std::make_shared(robotName, ballbotInterface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); + auto rosReferenceManagerPtr = std::make_shared( + robotName, ballbotInterface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(node); // MPC - ocs2::GaussNewtonDDP_MPC mpc(ballbotInterface.mpcSettings(), ballbotInterface.ddpSettings(), ballbotInterface.getRollout(), - ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer()); + ocs2::GaussNewtonDDP_MPC mpc(ballbotInterface.mpcSettings(), + ballbotInterface.ddpSettings(), + ballbotInterface.getRollout(), + ballbotInterface.getOptimalControlProblem(), + ballbotInterface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // Launch MPC ROS node ocs2::MPC_ROS_Interface mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); // Successful exit return 0; diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDummyVisualization.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDummyVisualization.cpp index a45da067a..03263f9aa 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDummyVisualization.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDummyVisualization.cpp @@ -27,24 +27,32 @@ 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 "ocs2_ballbot_ros/BallbotDummyVisualization.h" #include +#include -#include "ocs2_ballbot_ros/BallbotDummyVisualization.h" +#include +#include namespace ocs2 { namespace ballbot { -void BallbotDummyVisualization::update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) { +BallbotDummyVisualization::BallbotDummyVisualization( + const rclcpp::Node::SharedPtr& node) + : node_(node), + tfBroadcaster_(node), + jointPublisher_(node_->create_publisher( + "joint_states", 1)) {} + +void BallbotDummyVisualization::update(const SystemObservation& observation, + const PrimalSolution& policy, + const CommandData& command) { const auto& targetTrajectories = command.mpcTargetTrajectories_; // publish world transform - ros::Time timeMsg = ros::Time::now(); - geometry_msgs::TransformStamped world_transform; + builtin_interfaces::msg::Time timeMsg = node_->get_clock()->now(); + geometry_msgs::msg::TransformStamped world_transform; world_transform.header.stamp = timeMsg; world_transform.header.frame_id = "odom"; world_transform.child_frame_id = "world"; @@ -58,46 +66,48 @@ void BallbotDummyVisualization::update(const SystemObservation& observation, con tfBroadcaster_.sendTransform(world_transform); // publish command transform - const Eigen::Vector3d desiredPositionWorldToTarget(targetTrajectories.stateTrajectory.back()(0), - targetTrajectories.stateTrajectory.back()(1), 0.0); + const Eigen::Vector3d desiredPositionWorldToTarget( + targetTrajectories.stateTrajectory.back()(0), + targetTrajectories.stateTrajectory.back()(1), 0.0); const auto desiredQuaternionBaseToWorld = - getQuaternionFromEulerAnglesZyx(targetTrajectories.stateTrajectory.back().segment<3>(2)); - geometry_msgs::TransformStamped command_frame_transform; + getQuaternionFromEulerAnglesZyx( + targetTrajectories.stateTrajectory.back().segment<3>(2)); + geometry_msgs::msg::TransformStamped command_frame_transform; command_frame_transform.header.stamp = timeMsg; command_frame_transform.header.frame_id = "odom"; command_frame_transform.child_frame_id = "command"; - command_frame_transform.transform.translation.x = desiredPositionWorldToTarget.x(); - command_frame_transform.transform.translation.y = desiredPositionWorldToTarget.y(); - command_frame_transform.transform.translation.z = desiredPositionWorldToTarget.z(); - command_frame_transform.transform.rotation.w = desiredQuaternionBaseToWorld.w(); - command_frame_transform.transform.rotation.x = desiredQuaternionBaseToWorld.x(); - command_frame_transform.transform.rotation.y = desiredQuaternionBaseToWorld.y(); - command_frame_transform.transform.rotation.z = desiredQuaternionBaseToWorld.z(); + command_frame_transform.transform.translation.x = + desiredPositionWorldToTarget.x(); + command_frame_transform.transform.translation.y = + desiredPositionWorldToTarget.y(); + command_frame_transform.transform.translation.z = + desiredPositionWorldToTarget.z(); + command_frame_transform.transform.rotation.w = + desiredQuaternionBaseToWorld.w(); + command_frame_transform.transform.rotation.x = + desiredQuaternionBaseToWorld.x(); + command_frame_transform.transform.rotation.y = + desiredQuaternionBaseToWorld.y(); + command_frame_transform.transform.rotation.z = + desiredQuaternionBaseToWorld.z(); tfBroadcaster_.sendTransform(command_frame_transform); // publish joints transforms - std::map jointPositions{{"jball_x", observation.state(0)}, - {"jball_y", observation.state(1)}, - {"jbase_z", observation.state(2)}, - {"jbase_y", observation.state(3)}, - {"jbase_x", observation.state(4)}}; - robotStatePublisherPtr_->publishTransforms(jointPositions, timeMsg); -} - -void BallbotDummyVisualization::launchVisualizerNode(ros::NodeHandle& nodeHandle) { - // load a kdl-tree from the urdf robot description and initialize the robot state publisher - std::string urdfName = "robot_description"; - urdf::Model model; - if (!model.initParam(urdfName)) { - ROS_ERROR("URDF model load was NOT successful"); - } - KDL::Tree tree; - if (!kdl_parser::treeFromUrdfModel(model, tree)) { - ROS_ERROR("Failed to extract kdl tree from xml robot description"); - } - - robotStatePublisherPtr_.reset(new robot_state_publisher::RobotStatePublisher(tree)); - robotStatePublisherPtr_->publishFixedTransforms(true); + sensor_msgs::msg::JointState joint_state; + joint_state.header.stamp = node_->get_clock()->now(); + joint_state.name.resize(5); + joint_state.position.resize(5); + joint_state.name[0] = "jball_x"; + joint_state.name[1] = "jball_y"; + joint_state.name[2] = "jbase_z"; + joint_state.name[3] = "jbase_y"; + joint_state.name[4] = "jbase_x"; + joint_state.position[0] = observation.state(0); + joint_state.position[1] = observation.state(1); + joint_state.position[2] = observation.state(2); + joint_state.position[3] = observation.state(3); + joint_state.position[4] = observation.state(4); + jointPublisher_->publish(joint_state); } } // namespace ballbot diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp index 0c9b5aec4..c630ffc0b 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp @@ -28,35 +28,45 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ /* - * This file contains on an example on how to integrate the OCS2 MPC into your own control loop. - * In contrast to the MPC-node + Dummy-node setup, in this case we will not use ROS to communicate between the MPC and MRT. - * The MPC will run in a separate thread, and the ocs2::MPC_MRT_Interface facilitates all communication with this thread. + * This file contains on an example on how to integrate the OCS2 MPC into your + * own control loop. In contrast to the MPC-node + Dummy-node setup, in this + * case we will not use ROS to communicate between the MPC and MRT. The MPC will + * run in a separate thread, and the ocs2::MPC_MRT_Interface facilitates all + * communication with this thread. * - * This removes latency that arises when trying to do high frequency control over ROS. - * ROS will only be used to send commands, and to publish visualization topics. + * This removes latency that arises when trying to do high frequency control + * over ROS. ROS will only be used to send commands, and to publish + * visualization topics. */ -#include -#include - +#include #include #include #include #include -#include #include #include -#include +#include +#include + +#include + #include "ocs2_ballbot_ros/BallbotDummyVisualization.h" +#include "rclcpp/rclcpp.hpp" /** * This function implements the evaluation of the MPC policy - * @param currentObservation : current system observation {time, state, input} to compute the input for. (input can be left empty) - * @param mpcMrtInterface : interface used for communication with the MPC optimization (running in a different thread) + * @param currentObservation : current system observation {time, state, input} + * to compute the input for. (input can be left empty) + * @param mpcMrtInterface : interface used for communication with the MPC + * optimization (running in a different thread) * @return system input u(t) */ -ocs2::vector_t mpcTrackingController(const ocs2::SystemObservation& currentObservation, ocs2::MPC_MRT_Interface& mpcMrtInterface); +ocs2::vector_t mpcTrackingController( + const ocs2::SystemObservation& currentObservation, + ocs2::MPC_MRT_Interface& mpcMrtInterface); +const rclcpp::Logger LOGGER = rclcpp::get_logger("ballbot_mpc_mrt_node"); int main(int argc, char** argv) { /* @@ -65,39 +75,52 @@ int main(int argc, char** argv) { const std::string robotName = "ballbot"; // task file - std::vector programArgs{}; - ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName = std::string(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mpc_mrt"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_mpc_mrt"); // Robot interface - const std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_ballbot") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_ballbot_auto_generated"); ocs2::ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); /* * Set up the MPC and the MPC_MRT_interface. - * For this example we add a command interface and a visualization which both communicate over ros + * For this example we add a command interface and a visualization which both + * communicate over ros */ // MPC - ocs2::GaussNewtonDDP_MPC mpc(ballbotInterface.mpcSettings(), ballbotInterface.ddpSettings(), ballbotInterface.getRollout(), - ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer()); - - // ROS ReferenceManager. This gives us the command interface. Requires the observations to be published - auto rosReferenceManagerPtr = std::make_shared(robotName, ballbotInterface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); - auto observationPublisher = nodeHandle.advertise(robotName + "_mpc_observation", 1); + ocs2::GaussNewtonDDP_MPC mpc(ballbotInterface.mpcSettings(), + ballbotInterface.ddpSettings(), + ballbotInterface.getRollout(), + ballbotInterface.getOptimalControlProblem(), + ballbotInterface.getInitializer()); + + // ROS ReferenceManager. This gives us the command interface. Requires the + // observations to be published + auto rosReferenceManagerPtr = std::make_shared( + robotName, ballbotInterface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(node); + auto observationPublisher = + node->create_publisher( + robotName + "_mpc_observation", 1); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // Visualization - ocs2::ballbot::BallbotDummyVisualization ballbotDummyVisualization(nodeHandle); + ocs2::ballbot::BallbotDummyVisualization ballbotDummyVisualization(node); // Create the MPC MRT Interface ocs2::MPC_MRT_Interface mpcMrtInterface(mpc); @@ -114,75 +137,94 @@ int main(int argc, char** argv) { initObservation.time = 0.0; // Initial command - const ocs2::TargetTrajectories initTargetTrajectories({initObservation.time}, {initObservation.state}, {initObservation.input}); + const ocs2::TargetTrajectories initTargetTrajectories( + {initObservation.time}, {initObservation.state}, {initObservation.input}); // Set the first observation and command and wait for optimization to finish - ROS_INFO_STREAM("Waiting for the initial policy ..."); + RCLCPP_INFO_STREAM(LOGGER, "Waiting for the initial policy ..."); mpcMrtInterface.setCurrentObservation(initObservation); - mpcMrtInterface.getReferenceManager().setTargetTrajectories(initTargetTrajectories); - while (!mpcMrtInterface.initialPolicyReceived() && ros::ok() && ros::master::check()) { + mpcMrtInterface.getReferenceManager().setTargetTrajectories( + initTargetTrajectories); + while (!mpcMrtInterface.initialPolicyReceived() && rclcpp::ok()) { mpcMrtInterface.advanceMpc(); - ros::WallRate(ballbotInterface.mpcSettings().mrtDesiredFrequency_).sleep(); + rclcpp::WallRate(ballbotInterface.mpcSettings().mrtDesiredFrequency_) + .sleep(); } - ROS_INFO_STREAM("Initial policy has been received."); + RCLCPP_INFO_STREAM(LOGGER, "Initial policy has been received."); /* * Launch the computation of the MPC in a separate thread. - * This thread will be triggered at a given frequency and execute an optimization based on the latest available observation. + * This thread will be triggered at a given frequency and execute an + * optimization based on the latest available observation. */ std::atomic_bool mpcRunning{true}; auto mpcThread = std::thread([&]() { while (mpcRunning) { try { - ocs2::executeAndSleep([&]() { mpcMrtInterface.advanceMpc(); }, ballbotInterface.mpcSettings().mpcDesiredFrequency_); + ocs2::executeAndSleep( + [&]() { mpcMrtInterface.advanceMpc(); }, + ballbotInterface.mpcSettings().mpcDesiredFrequency_); } catch (const std::exception& e) { mpcRunning = false; - ROS_ERROR_STREAM("[Ocs2 MPC thread] Error : " << e.what()); + RCLCPP_ERROR_STREAM(LOGGER, "[Ocs2 MPC thread] Error : " << e.what()); } } }); - ocs2::setThreadPriority(ballbotInterface.ddpSettings().threadPriority_, mpcThread); + ocs2::setThreadPriority(ballbotInterface.ddpSettings().threadPriority_, + mpcThread); /* * Main control loop. */ ocs2::SystemObservation currentObservation = initObservation; - while (mpcRunning && ros::ok()) { + while (mpcRunning && rclcpp::ok()) { ocs2::executeAndSleep( // timed execution of the control loop [&]() { - ROS_INFO_STREAM("### Current time " << currentObservation.time); + RCLCPP_INFO_STREAM(LOGGER, + "### Current time " << currentObservation.time); /* * State estimation would go here to fill "currentObservation". - * In this example we receive the measurement directly after forward integration at the end of the loop. + * In this example we receive the measurement directly after forward + * integration at the end of the loop. */ // Evaluate the control input - const auto systemInput = mpcTrackingController(currentObservation, mpcMrtInterface); + const auto systemInput = + mpcTrackingController(currentObservation, mpcMrtInterface); /* * Sending the commands to the actuators would go here. - * In this example, we instead do a forward simulation + visualization. - * Simulation is done with the rollout functionality of the mpcMrtInterface, but this can be replaced by any other simulation. + * In this example, we instead do a forward simulation + + * visualization. Simulation is done with the rollout functionality of + * the mpcMrtInterface, but this can be replaced by any other + * simulation. */ // Forward simulation - const auto dt = 1.0 / ballbotInterface.mpcSettings().mrtDesiredFrequency_; + const auto dt = + 1.0 / ballbotInterface.mpcSettings().mrtDesiredFrequency_; ocs2::SystemObservation nextObservation; nextObservation.time = currentObservation.time + dt; - mpcMrtInterface.rolloutPolicy(currentObservation.time, currentObservation.state, dt, nextObservation.state, nextObservation.input, - nextObservation.mode); + mpcMrtInterface.rolloutPolicy( + currentObservation.time, currentObservation.state, dt, + nextObservation.state, nextObservation.input, + nextObservation.mode); // "state estimation" currentObservation = nextObservation; // Visualization - ballbotDummyVisualization.update(currentObservation, mpcMrtInterface.getPolicy(), mpcMrtInterface.getCommand()); + ballbotDummyVisualization.update(currentObservation, + mpcMrtInterface.getPolicy(), + mpcMrtInterface.getCommand()); // Publish the observation. Only needed for the command interface - observationPublisher.publish(ocs2::ros_msg_conversions::createObservationMsg(currentObservation)); + observationPublisher->publish( + ocs2::ros_msg_conversions::createObservationMsg( + currentObservation)); - ros::spinOnce(); + rclcpp::spin_some(node); }, ballbotInterface.mpcSettings().mrtDesiredFrequency_); } @@ -197,21 +239,29 @@ int main(int argc, char** argv) { return 0; } -ocs2::vector_t mpcTrackingController(const ocs2::SystemObservation& currentObservation, ocs2::MPC_MRT_Interface& mpcMrtInterface) { +ocs2::vector_t mpcTrackingController( + const ocs2::SystemObservation& currentObservation, + ocs2::MPC_MRT_Interface& mpcMrtInterface) { // Update the current state of the system mpcMrtInterface.setCurrentObservation(currentObservation); // Load the latest MPC policy bool policyUpdated = mpcMrtInterface.updatePolicy(); if (policyUpdated) { - ROS_INFO_STREAM("<<< New MPC policy received at " << currentObservation.time); + RCLCPP_INFO_STREAM( + LOGGER, "<<< New MPC policy received at " << currentObservation.time); } // Evaluate the current policy - ocs2::vector_t optimizedState; // Evaluation of the optimized state trajectory. - ocs2::vector_t optimizedInput; // Evaluation of the optimized input trajectory. - size_t plannedMode; // The mode that is active at the time the policy is evaluated at. - mpcMrtInterface.evaluatePolicy(currentObservation.time, currentObservation.state, optimizedState, optimizedInput, plannedMode); + ocs2::vector_t + optimizedState; // Evaluation of the optimized state trajectory. + ocs2::vector_t + optimizedInput; // Evaluation of the optimized input trajectory. + size_t plannedMode; // The mode that is active at the time the policy is + // evaluated at. + mpcMrtInterface.evaluatePolicy(currentObservation.time, + currentObservation.state, optimizedState, + optimizedInput, plannedMode); return optimizedInput; } diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSlpMpcNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSlpMpcNode.cpp index 606841c7f..91137f414 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSlpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSlpMpcNode.cpp @@ -27,48 +27,57 @@ 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 #include -#include +#include + +#include + +#include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { const std::string robotName = "ballbot"; // task file - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName = std::string(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mpc"); // Robot interface - const std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_ballbot") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_ballbot_auto_generated"); ocs2::ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); // ROS ReferenceManager std::shared_ptr rosReferenceManagerPtr( - new ocs2::RosReferenceManager(robotName, ballbotInterface.getReferenceManagerPtr())); - rosReferenceManagerPtr->subscribe(nodeHandle); + new ocs2::RosReferenceManager(robotName, + ballbotInterface.getReferenceManagerPtr())); + rosReferenceManagerPtr->subscribe(node); // MPC - ocs2::SlpMpc mpc(ballbotInterface.mpcSettings(), ballbotInterface.slpSettings(), ballbotInterface.getOptimalControlProblem(), + ocs2::SlpMpc mpc(ballbotInterface.mpcSettings(), + ballbotInterface.slpSettings(), + ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // Launch MPC ROS node ocs2::MPC_ROS_Interface mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); // Successful exit return 0; diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSqpMpcNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSqpMpcNode.cpp index 64ef42c49..7e25fcb47 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSqpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSqpMpcNode.cpp @@ -27,47 +27,56 @@ 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 #include -#include +#include + +#include + +#include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { const std::string robotName = "ballbot"; // task file - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName = std::string(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mpc"); // Robot interface - const std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_ballbot") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_ballbot_auto_generated"); ocs2::ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); // ROS ReferenceManager - auto rosReferenceManagerPtr = std::make_shared(robotName, ballbotInterface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); + auto rosReferenceManagerPtr = std::make_shared( + robotName, ballbotInterface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(node); // MPC - ocs2::SqpMpc mpc(ballbotInterface.mpcSettings(), ballbotInterface.sqpSettings(), ballbotInterface.getOptimalControlProblem(), + ocs2::SqpMpc mpc(ballbotInterface.mpcSettings(), + ballbotInterface.sqpSettings(), + ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // Launch MPC ROS node ocs2::MPC_ROS_Interface mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); // Successful exit return 0; diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotTargetPoseCommand.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotTargetPoseCommand.cpp index c4f3bc0a6..98266f776 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotTargetPoseCommand.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotTargetPoseCommand.cpp @@ -38,12 +38,14 @@ using namespace ballbot; * @param [in] commadLineTarget : [deltaX, deltaY, deltaYaw] * @param [in] observation : the current observation */ -TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTarget, const SystemObservation& observation) { +TargetTrajectories commandLineToTargetTrajectories( + const vector_t& commadLineTarget, const SystemObservation& observation) { const vector_t targetState = [&]() { vector_t targetState = vector_t::Zero(STATE_DIM); targetState(0) = observation.state(0) + commadLineTarget(0); targetState(1) = observation.state(1) + commadLineTarget(1); - targetState(2) = observation.state(2) + commadLineTarget(2) * M_PI / 180.0; // deg2rad + targetState(2) = + observation.state(2) + commadLineTarget(2) * M_PI / 180.0; // deg2rad targetState(3) = observation.state(3); targetState(4) = observation.state(4); return targetState; @@ -52,7 +54,8 @@ TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTar // Target reaching duration constexpr scalar_t averageSpeed = 2.0; const vector_t deltaPose = (targetState - observation.state).head<5>(); - const scalar_t targetTime = observation.time + deltaPose.norm() / averageSpeed; + const scalar_t targetTime = + observation.time + deltaPose.norm() / averageSpeed; // Desired time trajectory const scalar_array_t timeTrajectory{observation.time, targetTime}; @@ -71,14 +74,17 @@ TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTar */ int main(int argc, char* argv[]) { const std::string robotName = "ballbot"; - ::ros::init(argc, argv, robotName + "_target"); - ::ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_target"); // [deltaX, deltaY, deltaYaw] const scalar_array_t relativeStateLimit{10.0, 10.0, 360.0}; - TargetTrajectoriesKeyboardPublisher targetPoseCommand(nodeHandle, robotName, relativeStateLimit, &commandLineToTargetTrajectories); + TargetTrajectoriesKeyboardPublisher targetPoseCommand( + node, robotName, relativeStateLimit, &commandLineToTargetTrajectories); - const std::string commadMsg = "Enter XY and Yaw (deg) displacements, separated by spaces"; + const std::string commadMsg = + "Enter XY and Yaw (deg) displacements, separated by spaces"; targetPoseCommand.publishKeyboardCommand(commadMsg); // Successful exit diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/DummyBallbotNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/DummyBallbotNode.cpp index d969c468b..a8888cb13 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/DummyBallbotNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/DummyBallbotNode.cpp @@ -27,49 +27,56 @@ 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 #include #include -#include -#include +#include + +#include #include "ocs2_ballbot_ros/BallbotDummyVisualization.h" +#include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { const std::string robotName = "ballbot"; // task file - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName = std::string(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mrt"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mpc"); // ballbotInterface - const std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_ballbot") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_ballbot_auto_generated"); ocs2::ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); // MRT ocs2::MRT_ROS_Interface mrt(robotName); mrt.initRollout(&ballbotInterface.getRollout()); - mrt.launchNodes(nodeHandle); + mrt.launchNodes(node); // Visualization - auto ballbotDummyVisualization = std::make_shared(nodeHandle); + auto ballbotDummyVisualization = + std::make_shared(node); // Dummy ballbot - ocs2::MRT_ROS_Dummy_Loop dummyBallbot(mrt, ballbotInterface.mpcSettings().mrtDesiredFrequency_, - ballbotInterface.mpcSettings().mpcDesiredFrequency_); + ocs2::MRT_ROS_Dummy_Loop dummyBallbot( + mrt, ballbotInterface.mpcSettings().mrtDesiredFrequency_, + ballbotInterface.mpcSettings().mpcDesiredFrequency_); dummyBallbot.subscribeObservers({ballbotDummyVisualization}); // initial state @@ -79,7 +86,8 @@ int main(int argc, char** argv) { initObservation.time = 0.0; // initial command - const ocs2::TargetTrajectories initTargetTrajectories({initObservation.time}, {initObservation.state}, {initObservation.input}); + const ocs2::TargetTrajectories initTargetTrajectories( + {initObservation.time}, {initObservation.state}, {initObservation.input}); // Run dummy (loops while ros is ok) dummyBallbot.run(initObservation, initTargetTrajectories); diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/test/ballbotIntegrationTest.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/test/ballbotIntegrationTest.cpp index f239973f1..d76a6804b 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/test/ballbotIntegrationTest.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/test/ballbotIntegrationTest.cpp @@ -28,28 +28,31 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ #include - -#include - +#include +#include #include #include #include -#include -#include +#include + +#include "rclcpp/rclcpp.hpp" using namespace ocs2; TEST(BallbotIntegrationTest, createDummyMRT) { - const std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/mpc/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_ballbot") + + "/config/mpc/task.info"; + const std::string libFolder = "/tmp/ocs2_ballbot_auto_generated"; ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); MRT_ROS_Interface mrt("ballbot"); // Dummy ballbot - MRT_ROS_Dummy_Loop dummyBallbot(mrt, ballbotInterface.mpcSettings().mrtDesiredFrequency_, - ballbotInterface.mpcSettings().mpcDesiredFrequency_); + MRT_ROS_Dummy_Loop dummyBallbot( + mrt, ballbotInterface.mpcSettings().mrtDesiredFrequency_, + ballbotInterface.mpcSettings().mpcDesiredFrequency_); // Initialize dummy SystemObservation initObservation; @@ -59,20 +62,29 @@ TEST(BallbotIntegrationTest, createDummyMRT) { } TEST(BallbotIntegrationTest, createMPC) { - const std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/mpc/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_ballbot") + + "/config/mpc/task.info"; + const std::string libFolder = "/tmp/ocs2_ballbot_auto_generated"; ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); // MPC - ocs2::GaussNewtonDDP_MPC mpc(ballbotInterface.mpcSettings(), ballbotInterface.ddpSettings(), ballbotInterface.getRollout(), - ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer()); - mpc.getSolverPtr()->setReferenceManager(ballbotInterface.getReferenceManagerPtr()); + ocs2::GaussNewtonDDP_MPC mpc(ballbotInterface.mpcSettings(), + ballbotInterface.ddpSettings(), + ballbotInterface.getRollout(), + ballbotInterface.getOptimalControlProblem(), + ballbotInterface.getInitializer()); + mpc.getSolverPtr()->setReferenceManager( + ballbotInterface.getReferenceManagerPtr()); // Create MPC ROS node MPC_ROS_Interface mpcNode(mpc, "ballbot"); } int main(int argc, char** argv) { + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; } diff --git a/ocs2_robotic_examples/ocs2_cartpole/CMakeLists.txt b/ocs2_robotic_examples/ocs2_cartpole/CMakeLists.txt index 277939027..f048b129b 100644 --- a/ocs2_robotic_examples/ocs2_cartpole/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_cartpole/CMakeLists.txt @@ -1,128 +1,101 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_cartpole) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_ddp - ocs2_mpc - ocs2_robotic_tools -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_robotic_tools 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 - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost -) - -########### -## Build ## -########### +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) # Resolve for the package path at compile time. -configure_file ( +configure_file( "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h.in" - "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY -) - -# Add directories for all targets -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} + "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" + @ONLY ) -# cartpole interface library add_library(${PROJECT_NAME} src/CartPoleInterface.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_ddp + ocs2_mpc + ocs2_robotic_tools ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} + Boost::filesystem + Boost::system dl ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Running clang tooling.") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_EXCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}/generated - CF_WERROR - ) -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} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) - install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} ) - install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${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_cartpole - -catkin_add_gtest(test_cartpole - test/testCartpole.cpp +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -target_include_directories(test_cartpole PRIVATE + +ament_export_include_directories( + include ${PROJECT_BINARY_DIR}/include + ${EIGEN3_INCLUDE_DIRS} ) -target_link_libraries(test_cartpole - gtest_main - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_ddp + ocs2_mpc + ocs2_robotic_tools + Eigen3 + Boost ) + +############ +# Testing ## +############ + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(test_cartpole + test/testCartpole.cpp + ) + target_include_directories(test_cartpole PRIVATE + ${PROJECT_BINARY_DIR}/include + ) + target_link_libraries(test_cartpole + gtest_main + ${PROJECT_NAME} + ) + set_tests_properties(test_cartpole PROPERTIES TIMEOUT 1200) +endif() + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_cartpole/package.xml b/ocs2_robotic_examples/ocs2_cartpole/package.xml index 09a2d9601..9d005ca8a 100644 --- a/ocs2_robotic_examples/ocs2_cartpole/package.xml +++ b/ocs2_robotic_examples/ocs2_cartpole/package.xml @@ -1,5 +1,5 @@ - + ocs2_cartpole 0.0.0 The ocs2_cartpole package @@ -10,14 +10,18 @@ TODO - catkin - - roslib - cmake_clang_tools + ament_cmake ocs2_core ocs2_ddp ocs2_mpc ocs2_robotic_tools - + eigen3_cmake_module + boost + + ament_cmake_gtest + + ament_cmake + + diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_cartpole_ros/CMakeLists.txt index 0144a76fa..af0ba51ca 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/CMakeLists.txt @@ -1,10 +1,38 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_cartpole_ros) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) -set(CATKIN_PACKAGE_DEPENDENCIES +find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) + +find_package(ocs2_core REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_cartpole REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) + +find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) + +add_library(${PROJECT_NAME} + src/CartpoleDummyVisualization.cpp +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ament_index_cpp + rclcpp + sensor_msgs ocs2_core ocs2_ddp ocs2_mpc @@ -13,95 +41,59 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_cartpole ocs2_robotic_assets ) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -find_package(catkin REQUIRED COMPONENTS - roslib - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - -find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -################################### -## catkin specific configuration ## -################################### +add_executable(cartpole_mpc src/CartpoleMpcNode.cpp) +target_link_libraries(cartpole_mpc ${PROJECT_NAME}) +target_compile_options(cartpole_mpc PRIVATE ${OCS2_CXX_FLAGS}) -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost -) +add_executable(cartpole_dummy_test src/DummyCartpoleNode.cpp) +target_link_libraries(cartpole_dummy_test ${PROJECT_NAME}) +target_compile_options(cartpole_dummy_test PRIVATE ${OCS2_CXX_FLAGS}) -########### -## Build ## -########### +############# +## Install ## +############# -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) - -# Mpc node -add_executable(cartpole_mpc - src/CartpoleMpcNode.cpp +install(TARGETS cartpole_mpc cartpole_dummy_test + RUNTIME DESTINATION lib/${PROJECT_NAME} ) -add_dependencies(cartpole_mpc - ${catkin_EXPORTED_TARGETS} +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} ) -target_link_libraries(cartpole_mpc - ${catkin_LIBRARIES} +install(DIRECTORY launch rviz + DESTINATION share/${PROJECT_NAME} ) -# Dummy node -add_executable(cartpole_dummy_test - src/DummyCartpoleNode.cpp - src/CartpoleDummyVisualization.cpp -) -add_dependencies(cartpole_dummy_test - ${catkin_EXPORTED_TARGETS} +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -target_link_libraries(cartpole_dummy_test - ${catkin_LIBRARIES} -) - - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Running clang tooling.") - add_clang_tooling( - TARGETS - cartpole_mpc - cartpole_dummy_test - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_EXCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}/generated - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) -############# -## Install ## -############# - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) -install(TARGETS cartpole_mpc cartpole_dummy_test - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} ) -install(DIRECTORY launch rviz - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ament_index_cpp + rclcpp + sensor_msgs + ocs2_core + ocs2_ddp + ocs2_mpc + ocs2_ros_interfaces + ocs2_robotic_tools + ocs2_cartpole + ocs2_robotic_assets + Eigen3 + Boost ) + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/include/ocs2_cartpole_ros/CartpoleDummyVisualization.h b/ocs2_robotic_examples/ocs2_cartpole_ros/include/ocs2_cartpole_ros/CartpoleDummyVisualization.h index 516bce341..a6678a48b 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/include/ocs2_cartpole_ros/CartpoleDummyVisualization.h +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/include/ocs2_cartpole_ros/CartpoleDummyVisualization.h @@ -29,28 +29,29 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include -#include - #include +#include + #include "ocs2_cartpole/definitions.h" +#include "rclcpp/rclcpp.hpp" namespace ocs2 { namespace cartpole { class CartpoleDummyVisualization : public DummyObserver { public: - explicit CartpoleDummyVisualization(ros::NodeHandle& nodeHandle) { launchVisualizerNode(nodeHandle); } + explicit CartpoleDummyVisualization(const rclcpp::Node::SharedPtr& node); ~CartpoleDummyVisualization() override = default; - void update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) override; + void update(const SystemObservation& observation, + const PrimalSolution& policy, + const CommandData& command) override; private: - void launchVisualizerNode(ros::NodeHandle& nodeHandle); - - ros::Publisher jointPublisher_; + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr jointPublisher_; }; } // namespace cartpole diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/launch/cartpole.launch.py b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/cartpole.launch.py new file mode 100644 index 000000000..820c1ca7c --- /dev/null +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/cartpole.launch.py @@ -0,0 +1,53 @@ +import os +from launch.substitutions import LaunchConfiguration + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='task_name', + default_value='mpc' + ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_cartpole_ros'), 'launch/visualize.launch.py') + ), + launch_arguments={ + 'use_joint_state_publisher': 'false' + }.items() + ), + launch_ros.actions.Node( + package='ocs2_cartpole_ros', + executable='cartpole_mpc', + name='cartpole_mpc', + arguments=[LaunchConfiguration('task_name')], + prefix= "", + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_cartpole_ros', + executable='cartpole_dummy_test', + name='cartpole_dummy_test', + arguments=[LaunchConfiguration('task_name')], + prefix=LaunchConfiguration('terminal_prefix'), + output='screen' + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch.py b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch.py new file mode 100644 index 000000000..1daa2cb60 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch.py @@ -0,0 +1,47 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='observation_config', + default_value=get_package_share_directory( + 'ocs2_cartpole') + '/config/multiplot/mpc_observation.xml' + ), + launch.actions.DeclareLaunchArgument( + name='metrics_config', + default_value=get_package_share_directory( + 'ocs2_cartpole') + '/config/multiplot/mpc_metrics.xml' + ), + launch_ros.actions.Node( + package='rqt_multiplot', + executable='rqt_multiplot', + name='mpc_observation', + output='screen' + ), + launch_ros.actions.Node( + package='rqt_multiplot', + executable='rqt_multiplot', + name='mpc_metrics', + output='screen' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_ros_interfaces'), 'launch/performance_indices.launch.py') + ), + launch_arguments={ + 'mpc_policy_topic_name': 'cartpole_mpc_policy' + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/launch/visualize.launch.py b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/visualize.launch.py new file mode 100644 index 000000000..a31e6fa4b --- /dev/null +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/visualize.launch.py @@ -0,0 +1,50 @@ +import os +import launch +import launch_ros.actions +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + + +def generate_launch_description(): + rviz_config_file = get_package_share_directory('ocs2_cartpole_ros') + "/rviz/cartpole.rviz" + urdf_dir = get_package_share_directory("ocs2_robotic_assets") + urdf_model_path = os.path.join(urdf_dir, "resources/cartpole/urdf", "cartpole.urdf") + + use_joint_state_publisher_argument = launch.actions.DeclareLaunchArgument( + name='use_joint_state_publisher', + default_value='true' + ) + use_joint_state_publisher = LaunchConfiguration("use_joint_state_publisher") + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + arguments=[urdf_model_path], + ) + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + arguments=[urdf_model_path], + condition=IfCondition(use_joint_state_publisher), + ) + rviz_node = launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='cartpole', + output='screen', + arguments=["-d", rviz_config_file] + ) + + ld = launch.LaunchDescription([ + use_joint_state_publisher_argument, + robot_state_publisher_node, + joint_state_publisher_node, + rviz_node + ]) + return ld + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/package.xml b/ocs2_robotic_examples/ocs2_cartpole_ros/package.xml index ea4defbe7..5df881e77 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/package.xml @@ -1,5 +1,5 @@ - + ocs2_cartpole_ros 0.0.0 The ocs2_cartpole_ros package @@ -10,10 +10,11 @@ TODO - catkin + ament_cmake - roslib - cmake_clang_tools + ament_index_cpp + rclcpp + sensor_msgs ocs2_core ocs2_ddp @@ -21,7 +22,12 @@ ocs2_ros_interfaces ocs2_robotic_tools ocs2_cartpole - robot_state_publisher ocs2_robotic_assets - + + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/rviz/cartpole.rviz b/ocs2_robotic_examples/ocs2_cartpole_ros/rviz/cartpole.rviz index e4a40bc8a..f338aa207 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/rviz/cartpole.rviz +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/rviz/cartpole.rviz @@ -1,5 +1,5 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 138 Name: Displays Property Tree Widget: @@ -10,21 +10,21 @@ Panels: - /Grid1/Offset1 Splitter Ratio: 0.5 Tree Height: 270 - - Class: rviz/Selection + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 @@ -38,7 +38,7 @@ Visualization Manager: Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -55,7 +55,7 @@ Visualization Manager: Reference Frame: Value: true - Alpha: 1 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false Enabled: true Links: @@ -97,26 +97,26 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + - Class: rviz_default_plugins/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - - Class: rviz/SetGoal + - Class: rviz_default_plugins/SetGoal Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: - Class: rviz/Orbit + Class: rviz_default_plugins/Orbit Distance: 5.092568397521973 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleDummyVisualization.cpp b/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleDummyVisualization.cpp index 304226832..de06e5725 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleDummyVisualization.cpp +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleDummyVisualization.cpp @@ -32,20 +32,24 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace cartpole { -void CartpoleDummyVisualization::update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) { - sensor_msgs::JointState joint_state; - joint_state.header.stamp = ros::Time::now(); +CartpoleDummyVisualization::CartpoleDummyVisualization( + const rclcpp::Node::SharedPtr& node) + : node_(node), + jointPublisher_(node_->create_publisher( + "joint_states", 1)) {} + +void CartpoleDummyVisualization::update(const SystemObservation& observation, + const PrimalSolution& policy, + const CommandData& command) { + sensor_msgs::msg::JointState joint_state; + joint_state.header.stamp = node_->get_clock()->now(); joint_state.name.resize(2); joint_state.position.resize(2); joint_state.name[0] = "slider_to_cart"; joint_state.name[1] = "cart_to_pole"; joint_state.position[0] = observation.state(1); joint_state.position[1] = observation.state(0); - jointPublisher_.publish(joint_state); -} - -void CartpoleDummyVisualization::launchVisualizerNode(ros::NodeHandle& nodeHandle) { - jointPublisher_ = nodeHandle.advertise("joint_states", 1); + jointPublisher_->publish(joint_state); } } // namespace cartpole diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp b/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp index 6c932202c..d6f0882ac 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp @@ -27,39 +27,49 @@ 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 #include #include +#include +#include + +#include + +#include "rclcpp/rclcpp.hpp" + int main(int argc, char** argv) { const std::string robotName = "cartpole"; // task file - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mpc"); // Robot interface - const std::string taskFile = ros::package::getPath("ocs2_cartpole") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_cartpole") + "/auto_generated"; - ocs2::cartpole::CartPoleInterface cartPoleInterface(taskFile, libFolder, true /*verbose*/); + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_cartpole") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_cartpole_auto_generated"); + ocs2::cartpole::CartPoleInterface cartPoleInterface(taskFile, libFolder, + true /*verbose*/); // MPC - ocs2::GaussNewtonDDP_MPC mpc(cartPoleInterface.mpcSettings(), cartPoleInterface.ddpSettings(), cartPoleInterface.getRollout(), - cartPoleInterface.getOptimalControlProblem(), cartPoleInterface.getInitializer()); + ocs2::GaussNewtonDDP_MPC mpc(cartPoleInterface.mpcSettings(), + cartPoleInterface.ddpSettings(), + cartPoleInterface.getRollout(), + cartPoleInterface.getOptimalControlProblem(), + cartPoleInterface.getInitializer()); // observer for the input limits constraints auto createStateInputBoundsObserver = [&]() { @@ -69,21 +79,27 @@ int main(int argc, char** argv) { std::vector multiplierTopicNames; for (const auto& t : observingTimePoints) { const int timeMs = static_cast(t * 1000.0); - metricsTopicNames.push_back("metrics/" + observingLagrangianTerm + "/" + std::to_string(timeMs) + "MsLookAhead"); - multiplierTopicNames.push_back("multipliers/" + observingLagrangianTerm + "/" + std::to_string(timeMs) + "MsLookAhead"); + metricsTopicNames.push_back("metrics/" + observingLagrangianTerm + "/" + + "MsLookAhead" + std::to_string(timeMs)); + multiplierTopicNames.push_back("multipliers/" + observingLagrangianTerm + + "/" + "MsLookAhead" + + std::to_string(timeMs)); } - auto lagrangianCallback = ocs2::ros::createLagrangianCallback(nodeHandle, observingTimePoints, metricsTopicNames, - ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); - auto multiplierCallback = ocs2::ros::createMultiplierCallback(nodeHandle, observingTimePoints, multiplierTopicNames, - ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); - return ocs2::SolverObserver::LagrangianTermObserver(ocs2::SolverObserver::Type::Intermediate, observingLagrangianTerm, - std::move(lagrangianCallback), std::move(multiplierCallback)); + auto lagrangianCallback = ocs2::ros::createLagrangianCallback( + node, observingTimePoints, metricsTopicNames, + ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + auto multiplierCallback = ocs2::ros::createMultiplierCallback( + node, observingTimePoints, multiplierTopicNames, + ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + return ocs2::SolverObserver::LagrangianTermObserver( + ocs2::SolverObserver::Type::Intermediate, observingLagrangianTerm, + std::move(lagrangianCallback), std::move(multiplierCallback)); }; mpc.getSolverPtr()->addSolverObserver(createStateInputBoundsObserver()); // Launch MPC ROS node ocs2::MPC_ROS_Interface mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); return 0; } diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/src/DummyCartpoleNode.cpp b/ocs2_robotic_examples/ocs2_cartpole_ros/src/DummyCartpoleNode.cpp index 98195b382..5e1e588be 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/src/DummyCartpoleNode.cpp +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/src/DummyCartpoleNode.cpp @@ -27,48 +27,56 @@ 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 #include -#include -#include +#include + +#include #include "ocs2_cartpole_ros/CartpoleDummyVisualization.h" +#include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { const std::string robotName = "cartpole"; // task file - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName = std::string(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mrt"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mrt"); // Robot interface - const std::string taskFile = ros::package::getPath("ocs2_cartpole") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_cartpole") + "/auto_generated"; - ocs2::cartpole::CartPoleInterface cartPoleInterface(taskFile, libFolder, false /*verbose*/); + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_cartpole") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_cartpole_auto_generated"); + ocs2::cartpole::CartPoleInterface cartPoleInterface(taskFile, libFolder, + false /*verbose*/); // MRT ocs2::MRT_ROS_Interface mrt(robotName); mrt.initRollout(&cartPoleInterface.getRollout()); - mrt.launchNodes(nodeHandle); + mrt.launchNodes(node); // Visualization - auto cartpoleDummyVisualization = std::make_shared(nodeHandle); + auto cartpoleDummyVisualization = + std::make_shared(node); // Dummy loop - ocs2::MRT_ROS_Dummy_Loop dummyCartpole(mrt, cartPoleInterface.mpcSettings().mrtDesiredFrequency_, - cartPoleInterface.mpcSettings().mpcDesiredFrequency_); + ocs2::MRT_ROS_Dummy_Loop dummyCartpole( + mrt, cartPoleInterface.mpcSettings().mrtDesiredFrequency_, + cartPoleInterface.mpcSettings().mpcDesiredFrequency_); dummyCartpole.subscribeObservers({cartpoleDummyVisualization}); // initial state @@ -78,8 +86,9 @@ int main(int argc, char** argv) { initObservation.time = 0.0; // initial command - const ocs2::TargetTrajectories initTargetTrajectories({0.0}, {cartPoleInterface.getInitialTarget()}, - {ocs2::vector_t::Zero(ocs2::cartpole::INPUT_DIM)}); + const ocs2::TargetTrajectories initTargetTrajectories( + {0.0}, {cartPoleInterface.getInitialTarget()}, + {ocs2::vector_t::Zero(ocs2::cartpole::INPUT_DIM)}); // Run dummy (loops while ros is ok) dummyCartpole.run(initObservation, initTargetTrajectories); diff --git a/ocs2_robotic_examples/ocs2_double_integrator/CMakeLists.txt b/ocs2_robotic_examples/ocs2_double_integrator/CMakeLists.txt index 6df44a37f..1e650db59 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_double_integrator/CMakeLists.txt @@ -1,165 +1,133 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_double_integrator) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -set(CATKIN_PACKAGE_DEPENDENCIES - pybind11_catkin - ocs2_core - ocs2_mpc - ocs2_ddp - ocs2_python_interface - ocs2_robotic_tools -) - -find_package(catkin REQUIRED COMPONENTS - roslib - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(Python3 REQUIRED COMPONENTS Interpreter Development.Module) +find_package(pybind11 REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_python_interface REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost - LIBRARIES - ${PROJECT_NAME} -) - -########### -## Build ## -########### +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) # Resolve for the package path at compile time. -configure_file ( +configure_file( "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h.in" - "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY -) - -# Add directories for all targets -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} + "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" + @ONLY ) -# double integrator interface library add_library(${PROJECT_NAME} src/DoubleIntegratorInterface.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_mpc + ocs2_ddp + ocs2_python_interface + ocs2_robotic_tools ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} + Boost::filesystem + Boost::system dl ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) - # python bindings -pybind11_add_module(DoubleIntegratorPyBindings SHARED +pybind11_add_module(DoubleIntegratorPyBindings src/pyBindModule.cpp ) -add_dependencies(DoubleIntegratorPyBindings - ${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(DoubleIntegratorPyBindings PRIVATE ${PROJECT_NAME} - ${catkin_LIBRARIES} -) -set_target_properties(DoubleIntegratorPyBindings - PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} ) -catkin_python_setup() - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target ocs2_double_integrator") - add_clang_tooling( - TARGETS - ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) +ament_python_install_package(${PROJECT_NAME} + PACKAGE_DIR src/${PROJECT_NAME} +) ############# ## Install ## ############# 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} + DESTINATION include/${PROJECT_NAME} ) install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) install(TARGETS DoubleIntegratorPyBindings - ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} + LIBRARY DESTINATION ${PYTHON_INSTALL_DIR}/${PROJECT_NAME} ) -############# -## Testing ## -############# - -catkin_add_gtest(ocs2_double_integrator_no_ros_integration_test - test/DoubleIntegratorNoRosIntegrationTest.cpp -) -add_dependencies(ocs2_double_integrator_no_ros_integration_test - ${catkin_EXPORTED_TARGETS} -) -target_include_directories(ocs2_double_integrator_no_ros_integration_test - PRIVATE ${PROJECT_BINARY_DIR}/include -) -target_link_libraries(ocs2_double_integrator_no_ros_integration_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -catkin_add_gtest(ocs2_double_integrator_pybinding_test - test/DoubleIntegratorPyBindingTest.cpp -) -add_dependencies(ocs2_double_integrator_pybinding_test - ${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) -target_include_directories(ocs2_double_integrator_pybinding_test - PRIVATE ${PROJECT_BINARY_DIR}/include +ament_export_include_directories( + include + ${PROJECT_BINARY_DIR}/include + ${EIGEN3_INCLUDE_DIRS} ) -target_link_libraries(ocs2_double_integrator_pybinding_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_mpc + ocs2_ddp + ocs2_python_interface + ocs2_robotic_tools + Eigen3 + Boost + pybind11_vendor ) -# python tests -catkin_add_nosetests(test/DoubleIntegratorPyBindingTest.py) +############ +# Testing ## +############ + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(ocs2_double_integrator_no_ros_integration_test + test/DoubleIntegratorNoRosIntegrationTest.cpp + ) + target_include_directories(ocs2_double_integrator_no_ros_integration_test + PRIVATE ${PROJECT_BINARY_DIR}/include + ) + target_link_libraries(ocs2_double_integrator_no_ros_integration_test + gtest_main + ${PROJECT_NAME} + ) + + ament_add_gtest(ocs2_double_integrator_pybinding_test + test/DoubleIntegratorPyBindingTest.cpp + ) + target_include_directories(ocs2_double_integrator_pybinding_test + PRIVATE ${PROJECT_BINARY_DIR}/include + ) + target_link_libraries(ocs2_double_integrator_pybinding_test + ${PROJECT_NAME} + ) +endif() + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_double_integrator/package.xml b/ocs2_robotic_examples/ocs2_double_integrator/package.xml index 62828b4a5..069222976 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator/package.xml +++ b/ocs2_robotic_examples/ocs2_double_integrator/package.xml @@ -1,5 +1,5 @@ - + ocs2_double_integrator 0.0.0 The ocs2_double_integrator package @@ -10,17 +10,22 @@ TODO - catkin + ament_cmake + ament_cmake_python - cmake_clang_tools - - roslib - pybind11_catkin + pybind11_vendor ocs2_core ocs2_mpc ocs2_ddp ocs2_python_interface ocs2_robotic_tools + eigen3_cmake_module + boost + + ament_cmake_gtest + + ament_cmake + - \ No newline at end of file + diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_double_integrator_ros/CMakeLists.txt index 7b4057cbd..6af038cbf 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/CMakeLists.txt @@ -1,124 +1,106 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_double_integrator_ros) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) -set(CATKIN_PACKAGE_DEPENDENCIES +find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) + +find_package(ocs2_core REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_double_integrator REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) + +find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) + +add_library(${PROJECT_NAME} + src/DoubleIntegratorDummyVisualization.cpp +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ament_index_cpp + rclcpp + sensor_msgs ocs2_core ocs2_ddp ocs2_mpc ocs2_ros_interfaces - ocs2_double_integrator ocs2_robotic_tools + ocs2_double_integrator ocs2_robotic_assets ) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -find_package(catkin REQUIRED COMPONENTS - roslib - ${CATKIN_PACKAGE_DEPENDENCIES} -) +add_executable(double_integrator_mpc src/DoubleIntegratorMpcNode.cpp) +target_link_libraries(double_integrator_mpc ${PROJECT_NAME}) +target_compile_options(double_integrator_mpc PRIVATE ${OCS2_CXX_FLAGS}) -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - -find_package(Eigen3 3.3 REQUIRED NO_MODULE) +add_executable(double_integrator_dummy_test src/DummyDoubleIntegratorNode.cpp) +target_link_libraries(double_integrator_dummy_test ${PROJECT_NAME}) +target_compile_options(double_integrator_dummy_test PRIVATE ${OCS2_CXX_FLAGS}) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost -) +add_executable(double_integrator_target src/DoubleIntegratorTargetPoseCommand.cpp) +target_link_libraries(double_integrator_target ${PROJECT_NAME}) +target_compile_options(double_integrator_target PRIVATE ${OCS2_CXX_FLAGS}) -########### -## Build ## -########### +############# +## Install ## +############# -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) - -# Mpc node -add_executable(double_integrator_mpc - src/DoubleIntegratorMpcNode.cpp +install(TARGETS + double_integrator_mpc + double_integrator_dummy_test + double_integrator_target + RUNTIME DESTINATION lib/${PROJECT_NAME} ) -add_dependencies(double_integrator_mpc - ${catkin_EXPORTED_TARGETS} +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} ) -target_link_libraries(double_integrator_mpc - ${catkin_LIBRARIES} +install(DIRECTORY launch rviz + DESTINATION share/${PROJECT_NAME} ) -# Dummy node -add_executable(double_integrator_dummy_test - src/DummyDoubleIntegratorNode.cpp - src/DoubleIntegratorDummyVisualization.cpp -) -add_dependencies(double_integrator_dummy_test - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(double_integrator_dummy_test - ${catkin_LIBRARIES} +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -# Target node -add_executable(double_integrator_target - src/DoubleIntegratorTargetPoseCommand.cpp -) -add_dependencies(double_integrator_target - ${catkin_EXPORTED_TARGETS} +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} ) -target_link_libraries(double_integrator_target - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ament_index_cpp + rclcpp + sensor_msgs + ocs2_core + ocs2_ddp + ocs2_mpc + ocs2_ros_interfaces + ocs2_robotic_tools + ocs2_double_integrator + ocs2_robotic_assets + Eigen3 + Boost ) -target_compile_options(double_integrator_target PRIVATE ${OCS2_CXX_FLAGS}) - - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling.") - add_clang_tooling( - TARGETS - double_integrator_mpc - double_integrator_dummy_test - double_integrator_target - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) - -############# -## Install ## -############# -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) -install( - TARGETS - double_integrator_mpc - double_integrator_dummy_test - double_integrator_target - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) -install(DIRECTORY launch rviz - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) +ament_package() diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/include/ocs2_double_integrator_ros/DoubleIntegratorDummyVisualization.h b/ocs2_robotic_examples/ocs2_double_integrator_ros/include/ocs2_double_integrator_ros/DoubleIntegratorDummyVisualization.h index 6eee89559..bae6f8b1c 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/include/ocs2_double_integrator_ros/DoubleIntegratorDummyVisualization.h +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/include/ocs2_double_integrator_ros/DoubleIntegratorDummyVisualization.h @@ -29,28 +29,30 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include -#include - #include +#include + #include "ocs2_double_integrator/definitions.h" +#include "rclcpp/rclcpp.hpp" namespace ocs2 { namespace double_integrator { class DoubleIntegratorDummyVisualization final : public DummyObserver { public: - explicit DoubleIntegratorDummyVisualization(ros::NodeHandle& nodeHandle) { launchVisualizerNode(nodeHandle); } + explicit DoubleIntegratorDummyVisualization( + const rclcpp::Node::SharedPtr& node); ~DoubleIntegratorDummyVisualization() override = default; - void update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) override; + void update(const SystemObservation& observation, + const PrimalSolution& policy, + const CommandData& command) override; private: - void launchVisualizerNode(ros::NodeHandle& nodeHandle); - - ros::Publisher jointPublisher_; + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr jointPublisher_; }; } // namespace double_integrator diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/double_integrator.launch.py b/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/double_integrator.launch.py new file mode 100644 index 000000000..cc70da6a1 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/double_integrator.launch.py @@ -0,0 +1,102 @@ +import os +import sys + +import launch +import launch_ros.actions + +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + rviz_arg = launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ) + task_name_arg = launch.actions.DeclareLaunchArgument( + name='task_name', + default_value='mpc' + ) + debug_arg = launch.actions.DeclareLaunchArgument( + name='debug', + default_value='false' + ) + gdb_prefix_arg = launch.actions.DeclareLaunchArgument( + name='gdb_prefix', + default_value='gdb --args' + ) + terminal_prefix_arg = launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ) + + ld = launch.LaunchDescription([ + rviz_arg, + task_name_arg, + debug_arg, + gdb_prefix_arg, + terminal_prefix_arg + ]) + + # TODO rviz_condition=launch.conditions.IfCondition(LaunchConfiguration("rviz")) + ld.add_action( + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_double_integrator_ros'), 'launch/visualize.launch.py') + ), + launch_arguments={ + 'use_joint_state_publisher': 'false' + }.items() + ) + ) + + ld.add_action( + launch_ros.actions.Node( + package='ocs2_double_integrator_ros', + executable='double_integrator_mpc', + name='double_integrator_mpc', + arguments=[LaunchConfiguration('task_name')], + prefix=LaunchConfiguration('gdb_prefix'), + condition=launch.conditions.IfCondition(LaunchConfiguration("debug")), + output='screen' + ) + ) + + ld.add_action( + launch_ros.actions.Node( + package='ocs2_double_integrator_ros', + executable='double_integrator_mpc', + name='double_integrator_mpc', + arguments=[LaunchConfiguration('task_name')], + prefix= "", + condition=launch.conditions.UnlessCondition(LaunchConfiguration("debug")), + output='screen' + ) + ) + + ld.add_action( + launch_ros.actions.Node( + package='ocs2_double_integrator_ros', + executable='double_integrator_dummy_test', + name='double_integrator_dummy_test', + arguments=[LaunchConfiguration('task_name')], + prefix=LaunchConfiguration('terminal_prefix'), + output='screen' + ) + ) + + ld.add_action( + launch_ros.actions.Node( + package='ocs2_double_integrator_ros', + executable='double_integrator_target', + name='double_integrator_target', + arguments=[LaunchConfiguration('task_name')], + prefix=LaunchConfiguration('terminal_prefix'), + output='screen' + ) + ) + return ld + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/multiplot.launch.py b/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/multiplot.launch.py new file mode 100644 index 000000000..7e38680dc --- /dev/null +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/multiplot.launch.py @@ -0,0 +1,25 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_ros_interfaces'), 'launch/performance_indices.launch.py') + ), + launch_arguments={ + 'mpc_policy_topic_name': 'double_integrator_mpc_policy' + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/visualize.launch.py b/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/visualize.launch.py new file mode 100644 index 000000000..1a0bdb265 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/launch/visualize.launch.py @@ -0,0 +1,50 @@ +import os +import launch +import launch_ros.actions +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +def generate_launch_description(): + rviz_config_file = get_package_share_directory('ocs2_double_integrator_ros') + "/rviz/double_integrator.rviz" + urdf_dir = get_package_share_directory("ocs2_robotic_assets") + urdf_model_path = os.path.join(urdf_dir, "resources/double_integrator/urdf", "double_integrator.urdf") + + use_joint_state_publisher_argument = launch.actions.DeclareLaunchArgument( + name='use_joint_state_publisher', + default_value='true' + ) + use_joint_state_publisher = LaunchConfiguration("use_joint_state_publisher") + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + arguments=[urdf_model_path], + ) + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + arguments=[urdf_model_path], + condition=IfCondition(use_joint_state_publisher), + ) + rviz_node = launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='double_integrator', + output='screen', + arguments=["-d", rviz_config_file] + ) + + ld = launch.LaunchDescription([ + use_joint_state_publisher_argument, + robot_state_publisher_node, + joint_state_publisher_node, + rviz_node + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/package.xml b/ocs2_robotic_examples/ocs2_double_integrator_ros/package.xml index 1cd0de99b..0509c5294 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/package.xml @@ -1,5 +1,5 @@ - + ocs2_double_integrator_ros 0.0.0 The ocs2_double_integrator_ros package @@ -10,19 +10,24 @@ TODO - catkin + ament_cmake - cmake_clang_tools - - roslib - pybind11_catkin + ament_index_cpp + rclcpp + sensor_msgs ocs2_core ocs2_ddp ocs2_mpc ocs2_ros_interfaces - ocs2_double_integrator ocs2_robotic_tools + ocs2_double_integrator ocs2_robotic_assets + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/rviz/double_integrator.rviz b/ocs2_robotic_examples/ocs2_double_integrator_ros/rviz/double_integrator.rviz index cc2a65e1c..73319b0cc 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/rviz/double_integrator.rviz +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/rviz/double_integrator.rviz @@ -1,5 +1,5 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: @@ -13,21 +13,21 @@ Panels: - /RobotModel1/Links1/cart1 Splitter Ratio: 0.5 Tree Height: 836 - - Class: rviz/Selection + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.588679 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 @@ -37,7 +37,7 @@ Visualization Manager: Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -54,7 +54,7 @@ Visualization Manager: Reference Frame: Value: true - Alpha: 1 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false Enabled: true Links: @@ -86,23 +86,23 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + - Class: rviz_default_plugins/SetInitialPose Topic: /initialpose - - Class: rviz/SetGoal + - Class: rviz_default_plugins/SetGoal Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: - Class: rviz/Orbit + Class: rviz_default_plugins/Orbit Distance: 6.14627 Enable Stereo Rendering: Stereo Eye Separation: 0.06 diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorDummyVisualization.cpp b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorDummyVisualization.cpp index 513815f39..eac3145bf 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorDummyVisualization.cpp +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorDummyVisualization.cpp @@ -32,15 +32,18 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace double_integrator { -void DoubleIntegratorDummyVisualization::launchVisualizerNode(ros::NodeHandle& nodeHandle) { - jointPublisher_ = nodeHandle.advertise("joint_states", 1); -} - -void DoubleIntegratorDummyVisualization::update(const SystemObservation& observation, const PrimalSolution& policy, - const CommandData& command) { +DoubleIntegratorDummyVisualization::DoubleIntegratorDummyVisualization( + const rclcpp::Node::SharedPtr& node) + : node_(node), + jointPublisher_(node_->create_publisher( + "joint_states", 1)) {} + +void DoubleIntegratorDummyVisualization::update( + const SystemObservation& observation, const PrimalSolution& policy, + const CommandData& command) { const auto& targetTrajectories = command.mpcTargetTrajectories_; - sensor_msgs::JointState joint_state; - joint_state.header.stamp = ros::Time::now(); + sensor_msgs::msg::JointState joint_state; + joint_state.header.stamp = node_->get_clock()->now(); joint_state.name.resize(2); joint_state.position.resize(2); joint_state.name[0] = "slider_to_cart"; @@ -48,7 +51,7 @@ void DoubleIntegratorDummyVisualization::update(const SystemObservation& observa joint_state.name[1] = "slider_to_target"; joint_state.position[1] = targetTrajectories.stateTrajectory[0](0); - jointPublisher_.publish(joint_state); + jointPublisher_->publish(joint_state); } } // namespace double_integrator diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorMpcNode.cpp b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorMpcNode.cpp index 1e5d5c1f9..28926b1e7 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorMpcNode.cpp @@ -27,14 +27,16 @@ 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 +#include + +#include + #include "ocs2_double_integrator/DoubleIntegratorInterface.h" +#include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { const std::string robotName = "double_integrator"; @@ -42,35 +44,43 @@ int main(int argc, char** argv) { using mpc_ros_t = ocs2::MPC_ROS_Interface; // task file - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName = std::string(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mpc"); // Robot interface - const std::string taskFile = ros::package::getPath("ocs2_double_integrator") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_double_integrator") + "/auto_generated"; + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_double_integrator") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_double_integrator_auto_generated"); interface_t doubleIntegratorInterface(taskFile, libFolder); // ROS ReferenceManager - auto rosReferenceManagerPtr = std::make_shared(robotName, doubleIntegratorInterface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); + auto rosReferenceManagerPtr = std::make_shared( + robotName, doubleIntegratorInterface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(node); // MPC - ocs2::GaussNewtonDDP_MPC mpc(doubleIntegratorInterface.mpcSettings(), doubleIntegratorInterface.ddpSettings(), - doubleIntegratorInterface.getRollout(), doubleIntegratorInterface.getOptimalControlProblem(), - doubleIntegratorInterface.getInitializer()); + ocs2::GaussNewtonDDP_MPC mpc( + doubleIntegratorInterface.mpcSettings(), + doubleIntegratorInterface.ddpSettings(), + doubleIntegratorInterface.getRollout(), + doubleIntegratorInterface.getOptimalControlProblem(), + doubleIntegratorInterface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // Launch MPC ROS node mpc_ros_t mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); // Successful exit return 0; diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorTargetPoseCommand.cpp b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorTargetPoseCommand.cpp index 281c56bf5..3ac93893f 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorTargetPoseCommand.cpp +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorTargetPoseCommand.cpp @@ -38,20 +38,27 @@ using namespace double_integrator; * @param [in] commadLineTarget : [deltaX] * @param [in] observation : the current observation */ -TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTarget, const SystemObservation& observation) { - const vector_t displacement = (vector_t(STATE_DIM) << observation.state(0) + commadLineTarget(0), 0.0).finished(); - return TargetTrajectories({observation.time}, {displacement}, {vector_t::Zero(INPUT_DIM)}); +TargetTrajectories commandLineToTargetTrajectories( + const vector_t& commadLineTarget, const SystemObservation& observation) { + const vector_t displacement = + (vector_t(STATE_DIM) << observation.state(0) + commadLineTarget(0), 0.0) + .finished(); + return TargetTrajectories({observation.time}, {displacement}, + {vector_t::Zero(INPUT_DIM)}); } int main(int argc, char* argv[]) { const std::string robotName = "double_integrator"; - ::ros::init(argc, argv, robotName + "_target"); - ::ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_target"); const scalar_array_t goalLimit{10.0}; // [deltaX] - TargetTrajectoriesKeyboardPublisher targetPoseCommand(nodeHandle, robotName, goalLimit, &commandLineToTargetTrajectories); + TargetTrajectoriesKeyboardPublisher targetPoseCommand( + node, robotName, goalLimit, &commandLineToTargetTrajectories); - const std::string commadMsg = "Enter the desired displacement for the double-integrator"; + const std::string commadMsg = + "Enter the desired displacement for the double-integrator"; targetPoseCommand.publishKeyboardCommand(commadMsg); // Successful exit diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DummyDoubleIntegratorNode.cpp b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DummyDoubleIntegratorNode.cpp index 1cf3b6d30..a85323d69 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DummyDoubleIntegratorNode.cpp +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DummyDoubleIntegratorNode.cpp @@ -27,59 +27,70 @@ 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 #include -#include -#include +#include + +#include #include "ocs2_double_integrator_ros/DoubleIntegratorDummyVisualization.h" +#include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { const std::string robotName = "double_integrator"; // task file - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName = std::string(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mrt"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mrt"); // Robot interface - const std::string taskFile = ros::package::getPath("ocs2_double_integrator") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_double_integrator") + "/auto_generated"; - ocs2::double_integrator::DoubleIntegratorInterface doubleIntegratorInterface(taskFile, libFolder); + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_double_integrator") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_double_integrator_auto_generated"); + ocs2::double_integrator::DoubleIntegratorInterface doubleIntegratorInterface( + taskFile, libFolder); // MRT ocs2::MRT_ROS_Interface mrt(robotName); mrt.initRollout(&doubleIntegratorInterface.getRollout()); - mrt.launchNodes(nodeHandle); + mrt.launchNodes(node); // Visualization - auto doubleIntegratorDummyVisualization = std::make_shared(nodeHandle); + auto doubleIntegratorDummyVisualization = std::make_shared< + ocs2::double_integrator::DoubleIntegratorDummyVisualization>(node); // Dummy loop - ocs2::MRT_ROS_Dummy_Loop dummyDoubleIntegrator(mrt, doubleIntegratorInterface.mpcSettings().mrtDesiredFrequency_, - doubleIntegratorInterface.mpcSettings().mpcDesiredFrequency_); - dummyDoubleIntegrator.subscribeObservers({doubleIntegratorDummyVisualization}); + ocs2::MRT_ROS_Dummy_Loop dummyDoubleIntegrator( + mrt, doubleIntegratorInterface.mpcSettings().mrtDesiredFrequency_, + doubleIntegratorInterface.mpcSettings().mpcDesiredFrequency_); + dummyDoubleIntegrator.subscribeObservers( + {doubleIntegratorDummyVisualization}); // initial state ocs2::SystemObservation initObservation; initObservation.time = 0.0; initObservation.state = doubleIntegratorInterface.getInitialState(); - initObservation.input = ocs2::vector_t::Zero(ocs2::double_integrator::INPUT_DIM); + initObservation.input = + ocs2::vector_t::Zero(ocs2::double_integrator::INPUT_DIM); // initial command - const ocs2::TargetTrajectories initTargetTrajectories({0.0}, {doubleIntegratorInterface.getInitialTarget()}, - {ocs2::vector_t::Zero(ocs2::double_integrator::INPUT_DIM)}); + const ocs2::TargetTrajectories initTargetTrajectories( + {0.0}, {doubleIntegratorInterface.getInitialTarget()}, + {ocs2::vector_t::Zero(ocs2::double_integrator::INPUT_DIM)}); // Run dummy (loops while ros is ok) dummyDoubleIntegrator.run(initObservation, initTargetTrajectories); diff --git a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt index c6d07ed02..d82cd5e5f 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt @@ -1,85 +1,31 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_legged_robot) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_oc - ocs2_ddp - ocs2_mpc - ocs2_sqp - ocs2_ipm - ocs2_robotic_tools - ocs2_pinocchio_interface - ocs2_centroidal_model - ocs2_robotic_assets -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_sqp REQUIRED) +find_package(ocs2_ipm REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(ocs2_centroidal_model REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(pinocchio REQUIRED) 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 - Boost - pinocchio -) - -########### -## Build ## -########### +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) # Resolve for the package path at compile time. -configure_file ( +configure_file( "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h.in" - "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY -) - -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 + "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" + @ONLY ) -include_directories( - include - ${pinocchio_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -link_directories( - ${pinocchio_LIBRARY_DIRS} -) - -# Legged robot interface library add_library(${PROJECT_NAME} src/common/ModelSettings.cpp src/dynamics/LeggedRobotDynamicsAD.cpp @@ -100,66 +46,101 @@ add_library(${PROJECT_NAME} src/LeggedRobotInterface.cpp src/LeggedRobotPreComputation.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ${pinocchio_LIBRARIES} + pinocchio::pinocchio + Boost::filesystem + Boost::system dl ) -target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) - -######################### -### CLANG TOOLING ### -######################### - -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target ocs2_legged_robot") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${pinocchio_INCLUDE_DIRS} ) -endif(cmake_clang_tools_FOUND) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_oc + ocs2_ddp + ocs2_mpc + ocs2_sqp + ocs2_ipm + ocs2_robotic_tools + ocs2_pinocchio_interface + ocs2_centroidal_model + ocs2_robotic_assets +) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_PINOCCHIO_FLAGS}) ############# ## Install ## ############# 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} + DESTINATION include/${PROJECT_NAME} ) install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) -############# -## Testing ## -############# - -catkin_add_gtest(${PROJECT_NAME}_test - test/AnymalFactoryFunctions.cpp - test/constraint/testEndEffectorLinearConstraint.cpp - test/constraint/testFrictionConeConstraint.cpp - test/constraint/testZeroForceConstraint.cpp +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -target_include_directories(${PROJECT_NAME}_test PRIVATE - test/include + +ament_export_include_directories( + include ${PROJECT_BINARY_DIR}/include + ${pinocchio_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} ) -target_link_libraries(${PROJECT_NAME}_test - gtest_main - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} -) -target_compile_options(${PROJECT_NAME}_test PRIVATE ${FLAGS}) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_oc + ocs2_ddp + ocs2_mpc + ocs2_sqp + ocs2_ipm + ocs2_robotic_tools + ocs2_pinocchio_interface + ocs2_centroidal_model + ocs2_robotic_assets + pinocchio + Eigen3 + Boost +) + +############ +# Testing ## +############ + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_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} + ) + target_compile_options(${PROJECT_NAME}_test PRIVATE ${OCS2_PINOCCHIO_FLAGS}) +endif() + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_legged_robot/package.xml b/ocs2_robotic_examples/ocs2_legged_robot/package.xml index a9f76f9dd..0daa13664 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,7 +10,7 @@ BSD-3 - catkin + ament_cmake ocs2_core ocs2_oc @@ -23,5 +23,13 @@ ocs2_pinocchio_interface ocs2_centroidal_model pinocchio + eigen3_cmake_module + boost + + ament_cmake_gtest + + + 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 a8819c8f8..2a35c2bd6 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt @@ -1,13 +1,58 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_legged_robot_ros) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(urdf REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(robot_state_publisher REQUIRED) + +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_sqp REQUIRED) +find_package(ocs2_ipm REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(ocs2_centroidal_model REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(ocs2_msgs REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_legged_robot REQUIRED) + +find_package(pinocchio REQUIRED) +find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -set(CATKIN_PACKAGE_DEPENDENCIES - roslib - cmake_modules - tf +add_library(${PROJECT_NAME} + src/gait/GaitKeyboardPublisher.cpp + src/gait/GaitReceiver.cpp + src/visualization/LeggedRobotVisualizer.cpp +) +target_link_libraries(${PROJECT_NAME} + pinocchio::pinocchio +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${pinocchio_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + rclcpp + tf2_ros + sensor_msgs + geometry_msgs + visualization_msgs urdf kdl_parser robot_state_publisher @@ -25,206 +70,106 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_ros_interfaces ocs2_legged_robot ) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_PINOCCHIO_FLAGS}) -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - -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 - Boost - pinocchio -) - -########### -## Build ## -########### - -include_directories( - include - ${pinocchio_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -link_directories( - ${pinocchio_LIBRARY_DIRS} -) - -# Main library -add_library(${PROJECT_NAME} - src/gait/GaitKeyboardPublisher.cpp - src/gait/GaitReceiver.cpp - src/visualization/LeggedRobotVisualizer.cpp -) -add_dependencies(${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} -) -target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) - -# DDP-MPC node for legged robot add_executable(legged_robot_ddp_mpc src/LeggedRobotDdpMpcNode.cpp ) -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} -) -target_compile_options(legged_robot_ddp_mpc PRIVATE ${OCS2_CXX_FLAGS}) +target_link_libraries(legged_robot_ddp_mpc ${PROJECT_NAME}) +target_compile_options(legged_robot_ddp_mpc PRIVATE ${OCS2_PINOCCHIO_FLAGS}) -## SQP-MPC node for legged robot add_executable(legged_robot_sqp_mpc src/LeggedRobotSqpMpcNode.cpp ) -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_link_libraries(legged_robot_sqp_mpc ${PROJECT_NAME}) +target_compile_options(legged_robot_sqp_mpc PRIVATE ${OCS2_PINOCCHIO_FLAGS}) -## IPM-MPC node for legged robot add_executable(legged_robot_ipm_mpc src/LeggedRobotIpmMpcNode.cpp ) -add_dependencies(legged_robot_ipm_mpc - ${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(legged_robot_ipm_mpc - ${PROJECT_NAME} - ${catkin_LIBRARIES} -) -target_compile_options(legged_robot_ipm_mpc PRIVATE ${OCS2_CXX_FLAGS}) +target_link_libraries(legged_robot_ipm_mpc ${PROJECT_NAME}) +target_compile_options(legged_robot_ipm_mpc PRIVATE ${OCS2_PINOCCHIO_FLAGS}) -# Dummy node add_executable(legged_robot_dummy src/LeggedRobotDummyNode.cpp ) -add_dependencies(legged_robot_dummy - ${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(legged_robot_dummy - ${PROJECT_NAME} - ${catkin_LIBRARIES} -) -target_compile_options(legged_robot_dummy PRIVATE ${OCS2_CXX_FLAGS}) +target_link_libraries(legged_robot_dummy ${PROJECT_NAME}) +target_compile_options(legged_robot_dummy PRIVATE ${OCS2_PINOCCHIO_FLAGS}) -# Keyboard command node for target trajectories add_executable(legged_robot_target src/LeggedRobotPoseCommandNode.cpp ) -add_dependencies(legged_robot_target - ${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(legged_robot_target - ${PROJECT_NAME} - ${catkin_LIBRARIES} -) -target_compile_options(legged_robot_target PRIVATE ${OCS2_CXX_FLAGS}) +target_link_libraries(legged_robot_target ${PROJECT_NAME}) +target_compile_options(legged_robot_target PRIVATE ${OCS2_PINOCCHIO_FLAGS}) -# Keyboard command node for gait add_executable(legged_robot_gait_command src/LeggedRobotGaitCommandNode.cpp ) -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} -) -target_compile_options(legged_robot_gait_command PRIVATE ${OCS2_CXX_FLAGS}) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target ocs2_legged_robot_ros") - add_clang_tooling( - TARGETS - ${PROJECT_NAME} - legged_robot_ddp_mpc - legged_robot_sqp_mpc - legged_robot_ipm_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 -) -endif(cmake_clang_tools_FOUND) +target_link_libraries(legged_robot_gait_command ${PROJECT_NAME}) +target_compile_options(legged_robot_gait_command PRIVATE ${OCS2_PINOCCHIO_FLAGS}) ############# ## Install ## ############# + +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(TARGETS + legged_robot_ddp_mpc + legged_robot_sqp_mpc + legged_robot_ipm_mpc + legged_robot_dummy + legged_robot_target + legged_robot_gait_command + RUNTIME DESTINATION lib/${PROJECT_NAME} +) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} ) -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(DIRECTORY launch rviz + DESTINATION share/${PROJECT_NAME} ) + install( - TARGETS - legged_robot_ddp_mpc - legged_robot_sqp_mpc - legged_robot_ipm_mpc - legged_robot_dummy - legged_robot_target - legged_robot_gait_command - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -install(DIRECTORY launch rviz - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + +ament_export_include_directories( + include + ${pinocchio_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + rclcpp + tf2_ros + sensor_msgs + geometry_msgs + visualization_msgs + urdf + kdl_parser + robot_state_publisher + ocs2_core + ocs2_oc + ocs2_ddp + ocs2_mpc + ocs2_sqp + ocs2_ipm + ocs2_robotic_tools + ocs2_pinocchio_interface + ocs2_centroidal_model + ocs2_robotic_assets + ocs2_msgs + ocs2_ros_interfaces + ocs2_legged_robot + pinocchio + Eigen3 + Boost ) -############# -## Testing ## -############# +ament_package() 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..81067de1a 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 @@ -29,23 +29,27 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include + +#include +#include #include #include -#include -#include - -#include +#include "rclcpp/rclcpp.hpp" namespace ocs2 { namespace legged_robot { -/** This class implements ModeSequence communication using ROS. */ +/** This class implements mode_sequence communication using ROS. */ class GaitKeyboardPublisher { public: - GaitKeyboardPublisher(ros::NodeHandle nodeHandle, const std::string& gaitFile, const std::string& robotName, bool verbose = false); + GaitKeyboardPublisher(const rclcpp::Node::SharedPtr& node, + 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. */ + /** Prints the command line interface and responds to user input. Function + * returns after one user input. */ void getKeyboardCommand(); private: @@ -55,7 +59,8 @@ 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 d69946deb..c803fc7b6 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 @@ -29,36 +29,40 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include - -#include - #include -#include -#include - #include #include #include +#include + +#include +#include + +#include "rclcpp/rclcpp.hpp" namespace ocs2 { namespace legged_robot { class GaitReceiver : public SolverSynchronizedModule { public: - GaitReceiver(::ros::NodeHandle nodeHandle, std::shared_ptr gaitSchedulePtr, const std::string& robotName); + GaitReceiver(const rclcpp::Node::SharedPtr& node, + std::shared_ptr gaitSchedulePtr, + const std::string& robotName); - void preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, + void preSolverRun(scalar_t initTime, scalar_t finalTime, + const vector_t& currentState, const ReferenceManagerInterface& referenceManager) override; void postSolverRun(const PrimalSolution& primalSolution) override{}; private: - void mpcModeSequenceCallback(const ocs2_msgs::mode_schedule::ConstPtr& msg); + void mpcModeSequenceCallback( + const ocs2_msgs::msg::ModeSchedule::ConstSharedPtr& 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..9b859e224 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 @@ -29,27 +29,34 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include - -#include - #include +#include +#include + 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..f27b44ab7 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 @@ -28,17 +28,19 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ #pragma once - -#include -#include -#include - #include #include #include #include #include #include +#include + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" namespace ocs2 { namespace legged_robot { @@ -46,15 +48,19 @@ namespace legged_robot { class LeggedRobotVisualizer : public DummyObserver { public: /** Visualization settings (publicly available) */ - std::string frameId_ = "odom"; // Frame name all messages are published in - scalar_t footMarkerDiameter_ = 0.03; // Size of the spheres at the feet - scalar_t footAlphaWhenLifted_ = 0.3; // Alpha value when a foot is lifted. - scalar_t forceScale_ = 1000.0; // Vector scale in N/m - scalar_t velScale_ = 5.0; // Vector scale in m/s - scalar_t copMarkerDiameter_ = 0.03; // Size of the sphere at the center of pressure - scalar_t supportPolygonLineWidth_ = 0.005; // LineThickness for the support polygon - scalar_t trajectoryLineWidth_ = 0.01; // LineThickness for trajectories - std::vector feetColorMap_ = {Color::blue, Color::orange, Color::yellow, Color::purple}; // Colors for markers per feet + std::string frameId_ = "odom"; // Frame name all messages are published in + scalar_t footMarkerDiameter_ = 0.03; // Size of the spheres at the feet + scalar_t footAlphaWhenLifted_ = 0.3; // Alpha value when a foot is lifted. + scalar_t forceScale_ = 1000.0; // Vector scale in N/m + scalar_t velScale_ = 5.0; // Vector scale in m/s + scalar_t copMarkerDiameter_ = + 0.03; // Size of the sphere at the center of pressure + scalar_t supportPolygonLineWidth_ = + 0.005; // LineThickness for the support polygon + scalar_t trajectoryLineWidth_ = 0.01; // LineThickness for trajectories + std::vector feetColorMap_ = { + Color::blue, Color::orange, Color::yellow, + Color::purple}; // Colors for markers per feet /** * @@ -62,45 +68,63 @@ class LeggedRobotVisualizer : public DummyObserver { * @param n * @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); + LeggedRobotVisualizer( + PinocchioInterface pinocchioInterface, + CentroidalModelInfo centroidalModelInfo, + const PinocchioEndEffectorKinematics& endEffectorKinematics, + const rclcpp::Node::SharedPtr& node, scalar_t maxUpdateFrequency = 100.0); ~LeggedRobotVisualizer() override = default; - void update(const SystemObservation& observation, const PrimalSolution& primalSolution, const CommandData& command) override; + void update(const SystemObservation& observation, + const PrimalSolution& primalSolution, + const CommandData& command) override; - void launchVisualizerNode(ros::NodeHandle& nodeHandle); + void publishTrajectory( + const std::vector& system_observation_array, + scalar_t speed = 1.0); - void publishTrajectory(const std::vector& system_observation_array, scalar_t speed = 1.0); + void publishObservation(rclcpp::Time timeStamp, + const SystemObservation& observation); - void publishObservation(ros::Time timeStamp, const SystemObservation& observation); + void publishDesiredTrajectory(rclcpp::Time timeStamp, + const TargetTrajectories& targetTrajectories); - void publishDesiredTrajectory(ros::Time timeStamp, const TargetTrajectories& targetTrajectories); + void publishOptimizedStateTrajectory(rclcpp::Time timeStamp, + const scalar_array_t& mpcTimeTrajectory, + const vector_array_t& mpcStateTrajectory, + const ModeSchedule& modeSchedule); - void publishOptimizedStateTrajectory(ros::Time timeStamp, const scalar_array_t& mpcTimeTrajectory, - const vector_array_t& mpcStateTrajectory, const ModeSchedule& modeSchedule); + protected: + rclcpp::Node::SharedPtr node_; 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) const; + 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_; + tf2_ros::TransformBroadcaster tfBroadcaster_; + rclcpp::Publisher::SharedPtr jointPublisher_; - 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..f7c3c027a --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch.py @@ -0,0 +1,128 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + rviz_config_file = get_package_share_directory('ocs2_legged_robot_ros') + "/rviz/legged_robot.rviz" + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='description_name', + default_value='legged_robot_description' + ), + launch.actions.DeclareLaunchArgument( + name='multiplot', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value=get_package_share_directory( + 'ocs2_legged_robot') + '/config/mpc/task.info' + ), + launch.actions.DeclareLaunchArgument( + name='referenceFile', + default_value=get_package_share_directory( + 'ocs2_legged_robot') + '/config/command/reference.info' + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory( + 'ocs2_robotic_assets') + '/resources/anymal_c/urdf/anymal.urdf' + ), + launch.actions.DeclareLaunchArgument( + name='gaitCommandFile', + default_value=get_package_share_directory( + 'ocs2_legged_robot') + '/config/command/gait.info' + ), + launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + arguments=[launch.substitutions.LaunchConfiguration("urdfFile")], + ), + launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=["-d", rviz_config_file], + condition=launch.conditions.IfCondition( + launch.substitutions.LaunchConfiguration('rviz')) + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_ddp_mpc', + name='legged_robot_ddp_mpc', + prefix= "", + output='screen', + parameters=[ + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_dummy', + name='legged_robot_dummy', + output='screen', + prefix=launch.substitutions.LaunchConfiguration('terminal_prefix'), + parameters=[ + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_target', + name='legged_robot_target', + output='screen', + prefix=launch.substitutions.LaunchConfiguration('terminal_prefix'), + parameters=[ + { + 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_gait_command', + name='legged_robot_gait_command', + output='screen', + prefix=launch.substitutions.LaunchConfiguration('terminal_prefix'), + parameters=[ + { + 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') + } + ] + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch.py b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch.py new file mode 100644 index 000000000..2f8369674 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch.py @@ -0,0 +1,131 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + rviz_config_file = get_package_share_directory('ocs2_legged_robot_ros') + "/rviz/legged_robot.rviz" + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='description_name', + default_value='legged_robot_description' + ), + launch.actions.DeclareLaunchArgument( + name='multiplot', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value=get_package_share_directory( + 'ocs2_legged_robot') + '/config/mpc/task.info' + ), + launch.actions.DeclareLaunchArgument( + name='referenceFile', + default_value=get_package_share_directory( + 'ocs2_legged_robot') + '/config/command/reference.info' + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory( + 'ocs2_robotic_assets') + '/resources/anymal_c/urdf/anymal.urdf' + ), + launch.actions.DeclareLaunchArgument( + name='gaitCommandFile', + default_value=get_package_share_directory( + 'ocs2_legged_robot') + '/config/command/gait.info' + ), + launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + arguments=[launch.substitutions.LaunchConfiguration("urdfFile")], + ), + launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=["-d", rviz_config_file], + condition=launch.conditions.IfCondition( + launch.substitutions.LaunchConfiguration('rviz')) + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_ipm_mpc', + name='legged_robot_ipm_mpc', + output='screen', + prefix= "", + parameters=[ + { + 'multiplot': launch.substitutions.LaunchConfiguration('multiplot') + }, + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_dummy', + name='legged_robot_dummy', + output='screen', + prefix=launch.substitutions.LaunchConfiguration('terminal_prefix'), + parameters=[ + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_target', + name='legged_robot_target', + output='screen', + prefix=launch.substitutions.LaunchConfiguration('terminal_prefix'), + parameters=[ + { + 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_gait_command', + name='legged_robot_gait_command', + output='screen', + prefix=launch.substitutions.LaunchConfiguration('terminal_prefix'), + parameters=[ + { + 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') + } + ] + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch.py b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch.py new file mode 100644 index 000000000..c26bb730c --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch.py @@ -0,0 +1,131 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + rviz_config_file = get_package_share_directory('ocs2_legged_robot_ros') + "/rviz/legged_robot.rviz" + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='description_name', + default_value='legged_robot_description' + ), + launch.actions.DeclareLaunchArgument( + name='multiplot', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value=get_package_share_directory( + 'ocs2_legged_robot') + '/config/mpc/task.info' + ), + launch.actions.DeclareLaunchArgument( + name='referenceFile', + default_value=get_package_share_directory( + 'ocs2_legged_robot') + '/config/command/reference.info' + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory( + 'ocs2_robotic_assets') + '/resources/anymal_c/urdf/anymal.urdf' + ), + launch.actions.DeclareLaunchArgument( + name='gaitCommandFile', + default_value=get_package_share_directory( + 'ocs2_legged_robot') + '/config/command/gait.info' + ), + launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + arguments=[launch.substitutions.LaunchConfiguration("urdfFile")], + ), + launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=["-d", rviz_config_file], + condition=launch.conditions.IfCondition( + launch.substitutions.LaunchConfiguration('rviz')) + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_sqp_mpc', + name='legged_robot_sqp_mpc', + output='screen', + prefix= "", + parameters=[ + { + 'multiplot': launch.substitutions.LaunchConfiguration('multiplot') + }, + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_dummy', + name='legged_robot_dummy', + output='screen', + prefix=launch.substitutions.LaunchConfiguration('terminal_prefix'), + parameters=[ + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_target', + name='legged_robot_target', + output='screen', + prefix=launch.substitutions.LaunchConfiguration('terminal_prefix'), + parameters=[ + { + 'referenceFile': launch.substitutions.LaunchConfiguration('referenceFile') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_legged_robot_ros', + executable='legged_robot_gait_command', + name='legged_robot_gait_command', + output='screen', + prefix=launch.substitutions.LaunchConfiguration('terminal_prefix'), + parameters=[ + { + 'gaitCommandFile': launch.substitutions.LaunchConfiguration('gaitCommandFile') + } + ] + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch.py b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch.py new file mode 100644 index 000000000..55f562548 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch.py @@ -0,0 +1,36 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='metrics_config', + default_value=get_package_share_directory( + 'ocs2_legged_robot') + '/config/multiplot/zero_velocity.xml' + ), + launch_ros.actions.Node( + package='rqt_multiplot', + executable='rqt_multiplot', + name='mpc_metrics', + output='screen' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_ros_interfaces'), 'launch/performance_indices.launch.py') + ), + launch_arguments={ + 'mpc_policy_topic_name': 'legged_robot_mpc_policy' + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml b/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml index 7bc71c34e..e0046ae97 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml @@ -1,23 +1,25 @@ - + ocs2_legged_robot_ros - 0.0.1 + 0.0.0 The ocs2_legged_robot_ros package Farbod Farshidian + Jan Carius Ruben Grandia - Jean-Pierre Sleiman - BSD-3 + TODO - catkin + ament_cmake - roslib - tf + rclcpp + tf2_ros + sensor_msgs + geometry_msgs + visualization_msgs urdf kdl_parser robot_state_publisher - cmake_modules ocs2_core ocs2_oc @@ -30,9 +32,14 @@ ocs2_centroidal_model ocs2_robotic_assets ocs2_msgs - ocs2_legged_robot ocs2_ros_interfaces - + ocs2_legged_robot pinocchio + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/rviz/legged_robot.rviz b/ocs2_robotic_examples/ocs2_legged_robot_ros/rviz/legged_robot.rviz index f119a5ee4..78c8dbdda 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/rviz/legged_robot.rviz +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/rviz/legged_robot.rviz @@ -1,48 +1,44 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 138 Name: Displays Property Tree Widget: Expanded: + - /RobotModel1 - /TF1/Frames1 Splitter Ratio: 0.5 - Tree Height: 874 - - Class: rviz/Selection + Tree Height: 447 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 SyncSource: "" - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 138 Name: Displays Property Tree Widget: Expanded: ~ Splitter Ratio: 0.8294117450714111 Tree Height: 303 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -59,8 +55,16 @@ Visualization Manager: Reference Frame: odom Value: true - Alpha: 0.4000000059604645 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description Enabled: true Links: All Links Enabled: true @@ -426,13 +430,15 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false + Mass Properties: + Inertia: false + Mass: false Name: RobotModel - Robot Description: legged_robot_description TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true - - Class: rviz/TF + - Class: rviz_default_plugins/TF Enabled: true Frame Timeout: 15 Frames: @@ -595,7 +601,6 @@ Visualization Manager: Value: false wide_angle_camera_rear_camera_parent: Value: false - Marker Alpha: 1 Marker Scale: 1 Name: TF Show Arrows: true @@ -707,70 +712,69 @@ Visualization Manager: {} Update Interval: 0 Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true - Marker Topic: /legged_robot/optimizedStateTrajectory Name: Optimized State Trajectory Namespaces: - CoM Trajectory: true - EE Trajectories: true - Queue Size: 100 + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true - Marker Topic: /legged_robot/currentState Name: Current State Namespaces: - Center of Pressure: true - EE Forces: true - EE Positions: true - Support Polygon: true - Queue Size: 100 + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array Value: true - - Class: rviz/Group + - Class: rviz_common/Group Displays: - - Class: rviz/Group + - Class: rviz_common/Group Displays: - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /legged_robot/desiredFeetTrajectory/LF Name: Marker - Namespaces: - {} + Namespaces: ~ Queue Size: 100 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /legged_robot/desiredFeetTrajectory/LH Name: Marker - Namespaces: - {} + Namespaces: ~ Queue Size: 100 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /legged_robot/desiredFeetTrajectory/RF Name: Marker - Namespaces: - {} + Namespaces: ~ Queue Size: 100 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /legged_robot/desiredFeetTrajectory/RH Name: Marker - Namespaces: - {} + Namespaces: ~ Queue Size: 100 Value: true Enabled: false Name: Target Feet Trajectories - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /legged_robot/desiredBaseTrajectory Name: Target Base Trajectory - Namespaces: - {} + Namespaces: ~ Queue Size: 100 Value: true Enabled: true @@ -792,43 +796,65 @@ Visualization Manager: Min Intensity: 0 Name: GridMap Show Grid Lines: true - Topic: /raisim_heightmap - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /raisim_heightmap Use Rainbow: true Value: true Enabled: true Global Options: Background Color: 238; 238; 238 - Default Light: true Fixed Frame: odom Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/Select - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit + Class: rviz_default_plugins/Orbit Distance: 2.5402400493621826 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 Focal Point: X: -0.17801520228385925 Y: 0.39354705810546875 @@ -840,15 +866,16 @@ Visualization Manager: Near Clip Distance: 0.009999999776482582 Pitch: 0.3903983533382416 Target Frame: + Value: Orbit (rviz_default_plugins) Yaw: 2.2036001682281494 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1163 + Height: 736 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000431fc020000000afb0000001200530065006c0065006300740069006f006e000000006e000000b00000005c00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000006e000004770000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000431000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000fb000000100044006900730070006c006100790073000000006e00000247000000c900ffffff000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d0065000000000000000640000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005dc0000043100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000015600000286fc020000000afb0000001200530065006c0065006300740069006f006e000000006e000000b00000005c00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000006e000004770000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000286000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000fb000000100044006900730070006c006100790073000000006e00000247000000c900ffffff000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d0065000000000000000640000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000035e0000028600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -857,6 +884,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1848 - X: 1992 - Y: 0 + Width: 1210 + X: 70 + Y: 27 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 355ba0234..7caedc43f 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp @@ -27,16 +27,16 @@ 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 #include #include +#include + #include "ocs2_legged_robot_ros/gait/GaitReceiver.h" +#include "rclcpp/rclcpp.hpp" using namespace ocs2; using namespace legged_robot; @@ -45,51 +45,67 @@ int main(int argc, char** argv) { const std::string robotName = "legged_robot"; // Initialize ros node - ::ros::init(argc, argv, robotName + "_mpc"); - ::ros::NodeHandle nodeHandle; - // Get node parameters - bool multiplot = false; - std::string taskFile, urdfFile, referenceFile; - nodeHandle.getParam("/multiplot", multiplot); - nodeHandle.getParam("/taskFile", taskFile); - nodeHandle.getParam("/referenceFile", referenceFile); - nodeHandle.getParam("/urdfFile", urdfFile); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_mpc"); + + const bool multiplot = node->declare_parameter("multiplot", false); + const std::string taskFile = node->declare_parameter("taskFile", ""); + const std::string urdfFile = node->declare_parameter("urdfFile", ""); + const std::string referenceFile = + node->declare_parameter("referenceFile", ""); + if (taskFile.empty() || urdfFile.empty() || referenceFile.empty()) { + throw std::runtime_error( + "[LeggedRobotDdpMpcNode] Parameters 'taskFile', 'urdfFile', and " + "'referenceFile' are required."); + } // Robot interface LeggedRobotInterface interface(taskFile, urdfFile, referenceFile); // Gait receiver - auto gaitReceiverPtr = - std::make_shared(nodeHandle, interface.getSwitchedModelReferenceManagerPtr()->getGaitSchedule(), robotName); + auto gaitReceiverPtr = std::make_shared( + node, interface.getSwitchedModelReferenceManagerPtr()->getGaitSchedule(), + robotName); // ROS ReferenceManager - auto rosReferenceManagerPtr = std::make_shared(robotName, interface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); + auto rosReferenceManagerPtr = std::make_shared( + robotName, interface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(node); // MPC - GaussNewtonDDP_MPC mpc(interface.mpcSettings(), interface.ddpSettings(), interface.getRollout(), interface.getOptimalControlProblem(), - interface.getInitializer()); + GaussNewtonDDP_MPC mpc( + interface.mpcSettings(), interface.ddpSettings(), interface.getRollout(), + interface.getOptimalControlProblem(), interface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); - // observer for zero velocity constraints (only add this for debugging as it slows down the solver) + // observer for zero velocity constraints (only add this for debugging as it + // slows down the solver) if (multiplot) { auto createStateInputBoundsObserver = [&](const std::string& termName) { const ocs2::scalar_array_t observingTimePoints{0.0}; - const std::vector topicNames{"metrics/" + termName + "/0MsLookAhead"}; - auto callback = ocs2::ros::createConstraintCallback(nodeHandle, {0.0}, topicNames, - ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); - return ocs2::SolverObserver::ConstraintTermObserver(ocs2::SolverObserver::Type::Intermediate, termName, std::move(callback)); + const std::vector topicNames{"metrics/" + termName + + "/MsLookAhead0"}; + auto callback = ocs2::ros::createConstraintCallback( + node, {0.0}, topicNames, + ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + return ocs2::SolverObserver::ConstraintTermObserver( + ocs2::SolverObserver::Type::Intermediate, termName, + std::move(callback)); }; - for (size_t i = 0; i < interface.getCentroidalModelInfo().numThreeDofContacts; i++) { - const std::string& footName = interface.modelSettings().contactNames3DoF[i]; - mpc.getSolverPtr()->addSolverObserver(createStateInputBoundsObserver(footName + "_zeroVelocity")); + for (size_t i = 0; + i < interface.getCentroidalModelInfo().numThreeDofContacts; i++) { + const std::string& footName = + interface.modelSettings().contactNames3DoF[i]; + mpc.getSolverPtr()->addSolverObserver( + createStateInputBoundsObserver(footName + "_zeroVelocity")); } } // Launch MPC ROS node MPC_ROS_Interface mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); // Successful exit return 0; 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 85a47e4f3..4a55d56fc 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp @@ -27,16 +27,16 @@ 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 #include #include +#include + #include "ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h" +#include "rclcpp/rclcpp.hpp" using namespace ocs2; using namespace legged_robot; @@ -45,13 +45,21 @@ int main(int argc, char** argv) { const std::string robotName = "legged_robot"; // Initialize ros node - ros::init(argc, argv, robotName + "_mrt"); - ros::NodeHandle nodeHandle; - // Get node parameters - std::string taskFile, urdfFile, referenceFile; - nodeHandle.getParam("/taskFile", taskFile); - nodeHandle.getParam("/urdfFile", urdfFile); - nodeHandle.getParam("/referenceFile", referenceFile); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_mrt"); + + const std::string taskFile = + node->declare_parameter("taskFile", ""); + const std::string urdfFile = + node->declare_parameter("urdfFile", ""); + const std::string referenceFile = + node->declare_parameter("referenceFile", ""); + if (taskFile.empty() || urdfFile.empty() || referenceFile.empty()) { + throw std::runtime_error( + "[LeggedRobotDummyNode] Parameters 'taskFile', 'urdfFile', and " + "'referenceFile' are required."); + } // Robot interface LeggedRobotInterface interface(taskFile, urdfFile, referenceFile); @@ -59,28 +67,34 @@ int main(int argc, char** argv) { // MRT MRT_ROS_Interface mrt(robotName); mrt.initRollout(&interface.getRollout()); - mrt.launchNodes(nodeHandle); + mrt.launchNodes(node); // Visualization - CentroidalModelPinocchioMapping pinocchioMapping(interface.getCentroidalModelInfo()); - PinocchioEndEffectorKinematics endEffectorKinematics(interface.getPinocchioInterface(), pinocchioMapping, - interface.modelSettings().contactNames3DoF); + CentroidalModelPinocchioMapping pinocchioMapping( + interface.getCentroidalModelInfo()); + PinocchioEndEffectorKinematics endEffectorKinematics( + interface.getPinocchioInterface(), pinocchioMapping, + interface.modelSettings().contactNames3DoF); auto leggedRobotVisualizer = std::make_shared( - interface.getPinocchioInterface(), interface.getCentroidalModelInfo(), endEffectorKinematics, nodeHandle); + interface.getPinocchioInterface(), interface.getCentroidalModelInfo(), + endEffectorKinematics, node); // Dummy legged robot - MRT_ROS_Dummy_Loop leggedRobotDummySimulator(mrt, interface.mpcSettings().mrtDesiredFrequency_, - interface.mpcSettings().mpcDesiredFrequency_); + MRT_ROS_Dummy_Loop leggedRobotDummySimulator( + mrt, interface.mpcSettings().mrtDesiredFrequency_, + interface.mpcSettings().mpcDesiredFrequency_); leggedRobotDummySimulator.subscribeObservers({leggedRobotVisualizer}); // Initial state SystemObservation initObservation; initObservation.state = interface.getInitialState(); - initObservation.input = vector_t::Zero(interface.getCentroidalModelInfo().inputDim); + initObservation.input = + vector_t::Zero(interface.getCentroidalModelInfo().inputDim); initObservation.mode = ModeNumber::STANCE; // Initial command - TargetTrajectories initTargetTrajectories({0.0}, {initObservation.state}, {initObservation.input}); + TargetTrajectories initTargetTrajectories({0.0}, {initObservation.state}, + {initObservation.input}); // run dummy leggedRobotDummySimulator.run(initObservation, initTargetTrajectories); 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..296301bbc 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotGaitCommandNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotGaitCommandNode.cpp @@ -27,10 +27,10 @@ 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 "ocs2_legged_robot_ros/gait/GaitKeyboardPublisher.h" +#include "rclcpp/rclcpp.hpp" + +#include using namespace ocs2; using namespace legged_robot; @@ -39,16 +39,21 @@ 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; - // Get node parameters - std::string gaitCommandFile; - nodeHandle.getParam("/gaitCommandFile", gaitCommandFile); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_mpc_mode_schedule"); + + const std::string gaitCommandFile = + node->declare_parameter("gaitCommandFile", ""); + if (gaitCommandFile.empty()) { + throw std::runtime_error( + "[LeggedRobotGaitCommandNode] Parameter 'gaitCommandFile' is required."); + } std::cerr << "Loading gait file: " << gaitCommandFile << std::endl; - GaitKeyboardPublisher gaitCommand(nodeHandle, gaitCommandFile, robotName, true); + GaitKeyboardPublisher gaitCommand(node, 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/LeggedRobotIpmMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp index e754012b9..6568de796 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp @@ -27,16 +27,16 @@ 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 #include #include +#include + #include "ocs2_legged_robot_ros/gait/GaitReceiver.h" +#include "rclcpp/rclcpp.hpp" using namespace ocs2; using namespace legged_robot; @@ -45,51 +45,68 @@ int main(int argc, char** argv) { const std::string robotName = "legged_robot"; // Initialize ros node - ::ros::init(argc, argv, robotName + "_mpc"); - ::ros::NodeHandle nodeHandle; - // Get node parameters - bool multiplot = false; - std::string taskFile, urdfFile, referenceFile; - nodeHandle.getParam("/multiplot", multiplot); - nodeHandle.getParam("/taskFile", taskFile); - nodeHandle.getParam("/urdfFile", urdfFile); - nodeHandle.getParam("/referenceFile", referenceFile); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_mpc"); + + const bool multiplot = node->declare_parameter("multiplot", false); + const std::string taskFile = node->declare_parameter("taskFile", ""); + const std::string urdfFile = node->declare_parameter("urdfFile", ""); + const std::string referenceFile = + node->declare_parameter("referenceFile", ""); + if (taskFile.empty() || urdfFile.empty() || referenceFile.empty()) { + throw std::runtime_error( + "[LeggedRobotIpmMpcNode] Parameters 'taskFile', 'urdfFile', and " + "'referenceFile' are required."); + } // Robot interface constexpr bool useHardFrictionConeConstraint = true; - LeggedRobotInterface interface(taskFile, urdfFile, referenceFile, useHardFrictionConeConstraint); + LeggedRobotInterface interface(taskFile, urdfFile, referenceFile, + useHardFrictionConeConstraint); // Gait receiver - auto gaitReceiverPtr = - std::make_shared(nodeHandle, interface.getSwitchedModelReferenceManagerPtr()->getGaitSchedule(), robotName); + auto gaitReceiverPtr = std::make_shared( + node, interface.getSwitchedModelReferenceManagerPtr()->getGaitSchedule(), + robotName); // ROS ReferenceManager - auto rosReferenceManagerPtr = std::make_shared(robotName, interface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); + auto rosReferenceManagerPtr = std::make_shared( + robotName, interface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(node); // MPC - IpmMpc mpc(interface.mpcSettings(), interface.ipmSettings(), interface.getOptimalControlProblem(), interface.getInitializer()); + IpmMpc mpc(interface.mpcSettings(), interface.ipmSettings(), + interface.getOptimalControlProblem(), interface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); - // observer for friction cone constraints (only add this for debugging as it slows down the solver) + // observer for friction cone constraints (only add this for debugging as it + // slows down the solver) if (multiplot) { auto createStateInputBoundsObserver = [&](const std::string& termName) { const ocs2::scalar_array_t observingTimePoints{0.0}; - const std::vector topicNames{"metrics/" + termName + "/0MsLookAhead"}; - auto callback = ocs2::ros::createConstraintCallback(nodeHandle, {0.0}, topicNames, - ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); - return ocs2::SolverObserver::ConstraintTermObserver(ocs2::SolverObserver::Type::Intermediate, termName, std::move(callback)); + const std::vector topicNames{"metrics/" + termName + + "/MsLookAhead0"}; + auto callback = ocs2::ros::createConstraintCallback( + node, {0.0}, topicNames, + ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + return ocs2::SolverObserver::ConstraintTermObserver( + ocs2::SolverObserver::Type::Intermediate, termName, + std::move(callback)); }; - for (size_t i = 0; i < interface.getCentroidalModelInfo().numThreeDofContacts; i++) { - const std::string& footName = interface.modelSettings().contactNames3DoF[i]; - mpc.getSolverPtr()->addSolverObserver(createStateInputBoundsObserver(footName + "_frictionCone")); + for (size_t i = 0; + i < interface.getCentroidalModelInfo().numThreeDofContacts; i++) { + const std::string& footName = + interface.modelSettings().contactNames3DoF[i]; + mpc.getSolverPtr()->addSolverObserver( + createStateInputBoundsObserver(footName + "_frictionCone")); } } // Launch MPC ROS node MPC_ROS_Interface mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); // Successful exit return 0; 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..b46329b62 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPoseCommandNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPoseCommandNode.cpp @@ -27,15 +27,15 @@ 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 #include +#include +#include + +#include "rclcpp/rclcpp.hpp" + using namespace ocs2; namespace { @@ -60,7 +60,8 @@ scalar_t estimateTimeToTarget(const vector_t& desiredBaseDisplacement) { * @param [in] commadLineTarget : [deltaX, deltaY, deltaZ, deltaYaw] * @param [in] observation : the current observation */ -TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTarget, const SystemObservation& observation) { +TargetTrajectories commandLineToTargetTrajectories( + const vector_t& commadLineTarget, const SystemObservation& observation) { const vector_t currentPose = observation.state.segment<6>(6); const vector_t targetPose = [&]() { vector_t target(6); @@ -78,7 +79,8 @@ TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTar }(); // target reaching duration - const scalar_t targetReachingTime = observation.time + estimateTimeToTarget(targetPose - currentPose); + const scalar_t targetReachingTime = + observation.time + estimateTimeToTarget(targetPose - currentPose); // desired time trajectory const scalar_array_t timeTrajectory{observation.time, targetReachingTime}; @@ -89,7 +91,8 @@ TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTar stateTrajectory[1] << vector_t::Zero(6), targetPose, defaultJointState; // desired input trajectory (just right dimensions, they are not used) - const vector_array_t inputTrajectory(2, vector_t::Zero(observation.input.size())); + const vector_array_t inputTrajectory( + 2, vector_t::Zero(observation.input.size())); return {timeTrajectory, stateTrajectory, inputTrajectory}; } @@ -98,22 +101,33 @@ int main(int argc, char* argv[]) { const std::string robotName = "legged_robot"; // Initialize ros node - ::ros::init(argc, argv, robotName + "_target"); - ::ros::NodeHandle nodeHandle; - // Get node parameters - std::string referenceFile; - nodeHandle.getParam("/referenceFile", referenceFile); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_target"); + + const std::string referenceFile = + node->declare_parameter("referenceFile", ""); + if (referenceFile.empty()) { + throw std::runtime_error( + "[LeggedRobotPoseCommandNode] Parameter 'referenceFile' is required."); + } loadData::loadCppDataType(referenceFile, "comHeight", comHeight); - loadData::loadEigenMatrix(referenceFile, "defaultJointState", defaultJointState); - loadData::loadCppDataType(referenceFile, "targetRotationVelocity", targetRotationVelocity); - loadData::loadCppDataType(referenceFile, "targetDisplacementVelocity", targetDisplacementVelocity); + loadData::loadEigenMatrix(referenceFile, "defaultJointState", + defaultJointState); + loadData::loadCppDataType(referenceFile, "targetRotationVelocity", + targetRotationVelocity); + loadData::loadCppDataType(referenceFile, "targetDisplacementVelocity", + targetDisplacementVelocity); // goalPose: [deltaX, deltaY, deltaZ, deltaYaw] const scalar_array_t relativeBaseLimit{10.0, 10.0, 0.2, 360.0}; - TargetTrajectoriesKeyboardPublisher targetPoseCommand(nodeHandle, robotName, relativeBaseLimit, &commandLineToTargetTrajectories); + TargetTrajectoriesKeyboardPublisher targetPoseCommand( + node, robotName, relativeBaseLimit, &commandLineToTargetTrajectories); - const std::string commandMsg = "Enter XYZ and Yaw (deg) displacements for the TORSO, separated by spaces"; + const std::string commandMsg = + "Enter XYZ and Yaw (deg) displacements for the TORSO, separated by " + "spaces"; targetPoseCommand.publishKeyboardCommand(commandMsg); // Successful exit 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 9b3e51641..ec3547bd0 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp @@ -27,16 +27,16 @@ 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 #include #include +#include + #include "ocs2_legged_robot_ros/gait/GaitReceiver.h" +#include "rclcpp/rclcpp.hpp" using namespace ocs2; using namespace legged_robot; @@ -45,50 +45,66 @@ int main(int argc, char** argv) { const std::string robotName = "legged_robot"; // Initialize ros node - ::ros::init(argc, argv, robotName + "_mpc"); - ::ros::NodeHandle nodeHandle; - // Get node parameters - bool multiplot = false; - std::string taskFile, urdfFile, referenceFile; - nodeHandle.getParam("/multiplot", multiplot); - nodeHandle.getParam("/taskFile", taskFile); - nodeHandle.getParam("/urdfFile", urdfFile); - nodeHandle.getParam("/referenceFile", referenceFile); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_mpc"); + + const bool multiplot = node->declare_parameter("multiplot", false); + const std::string taskFile = node->declare_parameter("taskFile", ""); + const std::string urdfFile = node->declare_parameter("urdfFile", ""); + const std::string referenceFile = + node->declare_parameter("referenceFile", ""); + if (taskFile.empty() || urdfFile.empty() || referenceFile.empty()) { + throw std::runtime_error( + "[LeggedRobotSqpMpcNode] Parameters 'taskFile', 'urdfFile', and " + "'referenceFile' are required."); + } // Robot interface LeggedRobotInterface interface(taskFile, urdfFile, referenceFile); // Gait receiver - auto gaitReceiverPtr = - std::make_shared(nodeHandle, interface.getSwitchedModelReferenceManagerPtr()->getGaitSchedule(), robotName); + auto gaitReceiverPtr = std::make_shared( + node, interface.getSwitchedModelReferenceManagerPtr()->getGaitSchedule(), + robotName); // ROS ReferenceManager - auto rosReferenceManagerPtr = std::make_shared(robotName, interface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); + auto rosReferenceManagerPtr = std::make_shared( + robotName, interface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(node); // MPC - SqpMpc mpc(interface.mpcSettings(), interface.sqpSettings(), interface.getOptimalControlProblem(), interface.getInitializer()); + SqpMpc mpc(interface.mpcSettings(), interface.sqpSettings(), + interface.getOptimalControlProblem(), interface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); - // observer for zero velocity constraints (only add this for debugging as it slows down the solver) + // observer for zero velocity constraints (only add this for debugging as it + // slows down the solver) if (multiplot) { auto createStateInputBoundsObserver = [&](const std::string& termName) { const ocs2::scalar_array_t observingTimePoints{0.0}; - const std::vector topicNames{"metrics/" + termName + "/0MsLookAhead"}; - auto callback = ocs2::ros::createConstraintCallback(nodeHandle, {0.0}, topicNames, - ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); - return ocs2::SolverObserver::ConstraintTermObserver(ocs2::SolverObserver::Type::Intermediate, termName, std::move(callback)); + const std::vector topicNames{"metrics/" + termName + + "/MsLookAhead0"}; + auto callback = ocs2::ros::createConstraintCallback( + node, {0.0}, topicNames, + ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + return ocs2::SolverObserver::ConstraintTermObserver( + ocs2::SolverObserver::Type::Intermediate, termName, + std::move(callback)); }; - for (size_t i = 0; i < interface.getCentroidalModelInfo().numThreeDofContacts; i++) { - const std::string& footName = interface.modelSettings().contactNames3DoF[i]; - mpc.getSolverPtr()->addSolverObserver(createStateInputBoundsObserver(footName + "_zeroVelocity")); + for (size_t i = 0; + i < interface.getCentroidalModelInfo().numThreeDofContacts; i++) { + const std::string& footName = + interface.modelSettings().contactNames3DoF[i]; + mpc.getSolverPtr()->addSolverObserver( + createStateInputBoundsObserver(footName + "_zeroVelocity")); } } // Launch MPC ROS node MPC_ROS_Interface mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); // Successful exit return 0; 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..e3a5424e1 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 @@ -29,11 +29,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_legged_robot_ros/gait/GaitKeyboardPublisher.h" -#include - #include #include -#include + +#include +#include #include "ocs2_legged_robot_ros/gait/ModeSequenceTemplateRos.h" @@ -43,28 +43,35 @@ namespace legged_robot { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -GaitKeyboardPublisher::GaitKeyboardPublisher(ros::NodeHandle nodeHandle, const std::string& gaitFile, const std::string& robotName, - bool verbose) { - ROS_INFO_STREAM(robotName + "_mpc_mode_schedule node is setting up ..."); +GaitKeyboardPublisher::GaitKeyboardPublisher( + const rclcpp::Node::SharedPtr& node, const std::string& gaitFile, + const std::string& robotName, bool verbose) { + RCLCPP_INFO_STREAM(node->get_logger(), + robotName + "_mpc_mode_schedule node is setting up ..."); loadData::loadStdVector(gaitFile, "list", gaitList_, verbose); - modeSequenceTemplatePublisher_ = nodeHandle.advertise(robotName + "_mpc_mode_schedule", 1, true); + modeSequenceTemplatePublisher_ = + node->create_publisher( + robotName + "_mpc_mode_schedule", 1); gaitMap_.clear(); for (const auto& gaitName : gaitList_) { - gaitMap_.insert({gaitName, loadModeSequenceTemplate(gaitFile, gaitName, verbose)}); + gaitMap_.insert( + {gaitName, loadModeSequenceTemplate(gaitFile, gaitName, verbose)}); } - ROS_INFO_STREAM(robotName + "_mpc_mode_schedule command node is ready."); + RCLCPP_INFO_STREAM(node->get_logger(), + robotName + "_mpc_mode_schedule command node is ready."); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ void GaitKeyboardPublisher::getKeyboardCommand() { - const std::string commadMsg = "Enter the desired gait, for the list of available gait enter \"list\""; + 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()) { @@ -78,7 +85,8 @@ void GaitKeyboardPublisher::getKeyboardCommand() { // lower case transform auto gaitCommand = commandLine.front(); - std::transform(gaitCommand.begin(), gaitCommand.end(), gaitCommand.begin(), ::tolower); + std::transform(gaitCommand.begin(), gaitCommand.end(), gaitCommand.begin(), + ::tolower); if (gaitCommand == "list") { printGaitList(gaitList_); @@ -86,8 +94,9 @@ void GaitKeyboardPublisher::getKeyboardCommand() { } try { - ModeSequenceTemplate modeSequenceTemplate = gaitMap_.at(gaitCommand); - modeSequenceTemplatePublisher_.publish(createModeSequenceTemplateMsg(modeSequenceTemplate)); + ModeSequenceTemplate ModeSequenceTemplate = gaitMap_.at(gaitCommand); + modeSequenceTemplatePublisher_->publish( + createModeSequenceTemplateMsg(ModeSequenceTemplate)); } catch (const std::out_of_range& e) { std::cout << "Gait \"" << gaitCommand << "\" not found.\n"; printGaitList(gaitList_); @@ -97,7 +106,8 @@ void GaitKeyboardPublisher::getKeyboardCommand() { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void GaitKeyboardPublisher::printGaitList(const std::vector& gaitList) const { +void GaitKeyboardPublisher::printGaitList( + const std::vector& gaitList) const { std::cout << "List of available gaits:\n"; size_t itr = 0; for (const auto& s : 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 cd98a3b2d..76c48f8ee 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,23 +37,33 @@ namespace legged_robot { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -GaitReceiver::GaitReceiver(::ros::NodeHandle 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()); +GaitReceiver::GaitReceiver(const rclcpp::Node::SharedPtr& node, + std::shared_ptr gaitSchedulePtr, + const std::string& robotName) + : gaitSchedulePtr_(std::move(gaitSchedulePtr)), + receivedGait_({0.0, 1.0}, {ModeNumber::STANCE}), + gaitUpdated_(false) { + mpcModeSequenceSubscriber_ = + node->create_subscription( + robotName + "_mpc_mode_schedule", 1, + std::bind(&GaitReceiver::mpcModeSequenceCallback, this, + std::placeholders::_1)); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void GaitReceiver::preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, - const ReferenceManagerInterface& referenceManager) { +void GaitReceiver::preSolverRun( + scalar_t initTime, scalar_t finalTime, const vector_t& currentState, + const ReferenceManagerInterface& referenceManager) { if (gaitUpdated_) { std::lock_guard lock(receivedGaitMutex_); - std::cerr << "[GaitReceiver]: Setting new gait after time " << finalTime << "\n"; + std::cerr << "[GaitReceiver]: Setting new gait after time " << finalTime + << "\n"; std::cerr << receivedGait_; const auto timeHorizon = finalTime - initTime; - gaitSchedulePtr_->insertModeSequenceTemplate(receivedGait_, finalTime, timeHorizon); + gaitSchedulePtr_->insertModeSequenceTemplate(receivedGait_, finalTime, + timeHorizon); gaitUpdated_ = false; } } @@ -61,7 +71,8 @@ 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::ConstSharedPtr& 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..cbedfc177 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 @@ -41,14 +41,16 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include + #include "ocs2_legged_robot/gait/MotionPhaseDefinition.h" // Additional messages not in the helpers file -#include -#include +#include +#include // URDF related #include + #include namespace ocs2 { @@ -57,58 +59,66 @@ namespace legged_robot { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -LeggedRobotVisualizer::LeggedRobotVisualizer(PinocchioInterface pinocchioInterface, CentroidalModelInfo centroidalModelInfo, - const PinocchioEndEffectorKinematics& endEffectorKinematics, ros::NodeHandle& nodeHandle, - scalar_t maxUpdateFrequency) - : pinocchioInterface_(std::move(pinocchioInterface)), +LeggedRobotVisualizer::LeggedRobotVisualizer( + PinocchioInterface pinocchioInterface, + CentroidalModelInfo centroidalModelInfo, + const PinocchioEndEffectorKinematics& endEffectorKinematics, + const rclcpp::Node::SharedPtr& node, scalar_t maxUpdateFrequency) + : node_(node), + pinocchioInterface_(std::move(pinocchioInterface)), centroidalModelInfo_(std::move(centroidalModelInfo)), endEffectorKinematicsPtr_(endEffectorKinematics.clone()), + tfBroadcaster_(node), lastTime_(std::numeric_limits::lowest()), minPublishTimeDifference_(1.0 / maxUpdateFrequency) { endEffectorKinematicsPtr_->setPinocchioInterface(pinocchioInterface_); - launchVisualizerNode(nodeHandle); + costDesiredBasePositionPublisher_ = + node->create_publisher( + "/legged_robot/desiredBaseTrajectory", 1); + costDesiredFeetPositionPublishers_.resize( + centroidalModelInfo_.numThreeDofContacts); + costDesiredFeetPositionPublishers_[0] = + node->create_publisher( + "/legged_robot/desiredFeetTrajectory/LF", 1); + costDesiredFeetPositionPublishers_[1] = + node->create_publisher( + "/legged_robot/desiredFeetTrajectory/RF", 1); + costDesiredFeetPositionPublishers_[2] = + node->create_publisher( + "/legged_robot/desiredFeetTrajectory/LH", 1); + costDesiredFeetPositionPublishers_[3] = + node->create_publisher( + "/legged_robot/desiredFeetTrajectory/RH", 1); + stateOptimizedPublisher_ = + node->create_publisher( + "/legged_robot/optimizedStateTrajectory", 1); + currentStatePublisher_ = + node->create_publisher( + "/legged_robot/currentState", 1); + + jointPublisher_ = + node_->create_publisher("joint_states", 1); }; /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void LeggedRobotVisualizer::launchVisualizerNode(ros::NodeHandle& nodeHandle) { - costDesiredBasePositionPublisher_ = nodeHandle.advertise("/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); - - // 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); - } -} - -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void LeggedRobotVisualizer::update(const SystemObservation& observation, const PrimalSolution& primalSolution, const CommandData& command) { +void LeggedRobotVisualizer::update(const SystemObservation& observation, + const PrimalSolution& primalSolution, + const CommandData& command) { if (observation.time - lastTime_ > minPublishTimeDifference_) { const auto& model = pinocchioInterface_.getModel(); auto& data = pinocchioInterface_.getData(); - pinocchio::forwardKinematics(model, data, centroidal_model::getGeneralizedCoordinates(observation.state, centroidalModelInfo_)); + pinocchio::forwardKinematics(model, data, + centroidal_model::getGeneralizedCoordinates( + observation.state, centroidalModelInfo_)); pinocchio::updateFramePlacements(model, data); - const auto timeStamp = ros::Time::now(); + const auto timeStamp = node_->get_clock()->now(); publishObservation(timeStamp, observation); publishDesiredTrajectory(timeStamp, command.mpcTargetTrajectories_); - publishOptimizedStateTrajectory(timeStamp, primalSolution.timeTrajectory_, primalSolution.stateTrajectory_, + publishOptimizedStateTrajectory(timeStamp, primalSolution.timeTrajectory_, + primalSolution.stateTrajectory_, primalSolution.modeSchedule_); lastTime_ = observation.time; } @@ -117,49 +127,83 @@ 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_); + const auto basePose = + centroidal_model::getBasePose(observation.state, centroidalModelInfo_); + const auto qJoints = + centroidal_model::getJointAngles(observation.state, centroidalModelInfo_); // Compute cartesian state and inputs - const auto feetPositions = endEffectorKinematicsPtr_->getPosition(observation.state); + const auto feetPositions = + endEffectorKinematicsPtr_->getPosition(observation.state); std::vector feetForces(centroidalModelInfo_.numThreeDofContacts); for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) { - feetForces[i] = centroidal_model::getContactForces(observation.input, i, centroidalModelInfo_); + feetForces[i] = centroidal_model::getContactForces(observation.input, i, + centroidalModelInfo_); } // Publish publishJointTransforms(timeStamp, qJoints); publishBaseTransform(timeStamp, basePose); - publishCartesianMarkers(timeStamp, modeNumber2StanceLeg(observation.mode), feetPositions, feetForces); + publishCartesianMarkers(timeStamp, modeNumber2StanceLeg(observation.mode), + feetPositions, feetForces); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -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) const { + if (jointPublisher_ != nullptr) { + sensor_msgs::msg::JointState joint_state; + joint_state.header.stamp = node_->get_clock()->now(); + joint_state.name.resize(12); + joint_state.position.resize(12); + joint_state.name[0] = "LF_HAA"; + joint_state.name[1] = "LF_HFE"; + joint_state.name[2] = "LF_KFE"; + joint_state.name[3] = "LH_HAA"; + joint_state.name[4] = "LH_HFE"; + joint_state.name[5] = "LH_KFE"; + joint_state.name[6] = "RF_HAA"; + joint_state.name[7] = "RF_HFE"; + joint_state.name[8] = "RF_KFE"; + joint_state.name[9] = "RH_HAA"; + joint_state.name[10] = "RH_HFE"; + joint_state.name[11] = "RH_KFE"; + joint_state.position[0] = jointAngles[0]; + joint_state.position[1] = jointAngles[1]; + joint_state.position[2] = jointAngles[2]; + joint_state.position[3] = jointAngles[3]; + joint_state.position[4] = jointAngles[4]; + joint_state.position[5] = jointAngles[5]; + joint_state.position[6] = jointAngles[6]; + joint_state.position[7] = jointAngles[7]; + joint_state.position[8] = jointAngles[8]; + joint_state.position[9] = jointAngles[9]; + joint_state.position[10] = jointAngles[10]; + joint_state.position[11] = jointAngles[11]; + jointPublisher_->publish(joint_state); } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void LeggedRobotVisualizer::publishBaseTransform(ros::Time timeStamp, const vector_t& basePose) { - if (robotStatePublisherPtr_ != nullptr) { - geometry_msgs::TransformStamped baseToWorldTransform; +void LeggedRobotVisualizer::publishBaseTransform(rclcpp::Time timeStamp, + const vector_t& basePose) { + if (jointPublisher_ != nullptr) { + 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>())); + 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>()); + baseToWorldTransform.transform.translation = + getVectorMsg(basePose.head<3>()); tfBroadcaster_.sendTransform(baseToWorldTransform); } } @@ -167,12 +211,20 @@ void LeggedRobotVisualizer::publishBaseTransform(ros::Time timeStamp, const vect /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void LeggedRobotVisualizer::publishTrajectory(const std::vector& system_observation_array, scalar_t speed) { +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 frameDuration = speed * (system_observation_array[k + 1].time - + system_observation_array[k].time); + scalar_t publishDuration = timedExecutionInSeconds([&]() { + publishObservation(node_->get_clock()->now(), + system_observation_array[k]); + }); if (frameDuration > publishDuration) { - ros::WallDuration(frameDuration - publishDuration).sleep(); + const rclcpp::Duration duration = + rclcpp::Duration::from_seconds(frameDuration - publishDuration); + rclcpp::sleep_for((std::chrono::nanoseconds(duration.nanoseconds()))); } } } @@ -180,50 +232,58 @@ void LeggedRobotVisualizer::publishTrajectory(const std::vector& feetPositions, - const std::vector& feetForces) const { +void LeggedRobotVisualizer::publishCartesianMarkers( + rclcpp::Time timeStamp, const contact_flag_t& contactFlags, + 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 for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; ++i) { markerArray.markers.emplace_back( - getFootMarker(feetPositions[i], contactFlags[i], feetColorMap_[i], footMarkerDiameter_, footAlphaWhenLifted_)); - markerArray.markers.emplace_back(getForceMarker(feetForces[i], feetPositions[i], contactFlags[i], Color::green, forceScale_)); + getFootMarker(feetPositions[i], contactFlags[i], feetColorMap_[i], + footMarkerDiameter_, footAlphaWhenLifted_)); + markerArray.markers.emplace_back( + getForceMarker(feetForces[i], feetPositions[i], contactFlags[i], + Color::green, forceScale_)); } // Center of pressure - markerArray.markers.emplace_back(getCenterOfPressureMarker(feetForces.begin(), feetForces.end(), feetPositions.begin(), - contactFlags.begin(), Color::green, copMarkerDiameter_)); + markerArray.markers.emplace_back(getCenterOfPressureMarker( + feetForces.begin(), feetForces.end(), feetPositions.begin(), + contactFlags.begin(), Color::green, copMarkerDiameter_)); // Support polygon - markerArray.markers.emplace_back( - getSupportPolygonMarker(feetPositions.begin(), feetPositions.end(), contactFlags.begin(), Color::black, supportPolygonLineWidth_)); + markerArray.markers.emplace_back(getSupportPolygonMarker( + feetPositions.begin(), feetPositions.end(), contactFlags.begin(), + Color::black, supportPolygonLineWidth_)); // Give markers an id and a frame - assignHeader(markerArray.markers.begin(), markerArray.markers.end(), getHeaderMsg(frameId_, timeStamp)); + assignHeader(markerArray.markers.begin(), markerArray.markers.end(), + getHeaderMsg(frameId_, timeStamp)); 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()); } @@ -238,8 +298,9 @@ void LeggedRobotVisualizer::publishDesiredTrajectory(ros::Time timeStamp, const } // Construct base pose msg - const auto basePose = centroidal_model::getBasePose(state, centroidalModelInfo_); - geometry_msgs::Pose pose; + const auto basePose = + centroidal_model::getBasePose(state, centroidalModelInfo_); + geometry_msgs::msg::Pose pose; pose.position = getPointMsg(basePose.head<3>()); // Fill message containers @@ -248,85 +309,104 @@ void LeggedRobotVisualizer::publishDesiredTrajectory(ros::Time timeStamp, const // Fill feet msgs const auto& model = pinocchioInterface_.getModel(); auto& data = pinocchioInterface_.getData(); - pinocchio::forwardKinematics(model, data, centroidal_model::getGeneralizedCoordinates(state, centroidalModelInfo_)); + pinocchio::forwardKinematics(model, data, + centroidal_model::getGeneralizedCoordinates( + state, centroidalModelInfo_)); pinocchio::updateFramePlacements(model, data); 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); } } // Headers - auto comLineMsg = getLineMsg(std::move(desiredBasePositionMsg), Color::green, trajectoryLineWidth_); + auto comLineMsg = getLineMsg(std::move(desiredBasePositionMsg), Color::green, + trajectoryLineWidth_); comLineMsg.header = getHeaderMsg(frameId_, timeStamp); 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_); + 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, - const vector_array_t& mpcStateTrajectory, const ModeSchedule& modeSchedule) { +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 - std::for_each(mpcStateTrajectory.begin(), mpcStateTrajectory.end(), [&](const vector_t& state) { - const auto basePose = centroidal_model::getBasePose(state, centroidalModelInfo_); - - // Fill com position and pose msgs - geometry_msgs::Pose pose; - pose.position = getPointMsg(basePose.head<3>()); - mpcComPositionMsgs.push_back(pose.position); - - // Fill feet msgs - const auto& model = pinocchioInterface_.getModel(); - auto& data = pinocchioInterface_.getData(); - pinocchio::forwardKinematics(model, data, centroidal_model::getGeneralizedCoordinates(state, centroidalModelInfo_)); - pinocchio::updateFramePlacements(model, data); - - const auto feetPositions = endEffectorKinematicsPtr_->getPosition(state); - for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) { - const auto position = getPointMsg(feetPositions[i]); - feetMsgs[i].push_back(position); - } - }); + std::for_each( + mpcStateTrajectory.begin(), mpcStateTrajectory.end(), + [&](const vector_t& state) { + const auto basePose = + centroidal_model::getBasePose(state, centroidalModelInfo_); + + // Fill com position and pose msgs + geometry_msgs::msg::Pose pose; + pose.position = getPointMsg(basePose.head<3>()); + mpcComPositionMsgs.push_back(pose.position); + + // Fill feet msgs + const auto& model = pinocchioInterface_.getModel(); + auto& data = pinocchioInterface_.getData(); + pinocchio::forwardKinematics( + model, data, + centroidal_model::getGeneralizedCoordinates(state, + centroidalModelInfo_)); + pinocchio::updateFramePlacements(model, data); + + const auto feetPositions = + endEffectorKinematicsPtr_->getPosition(state); + for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) { + const auto position = getPointMsg(feetPositions[i]); + feetMsgs[i].push_back(position); + } + }); // 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 + 2); // 1 trajectory per foot + 1 for the future + // footholds + 1 for the com trajectory for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) { - markerArray.markers.emplace_back(getLineMsg(std::move(feetMsgs[i]), feetColorMap_[i], trajectoryLineWidth_)); + markerArray.markers.emplace_back(getLineMsg( + std::move(feetMsgs[i]), feetColorMap_[i], trajectoryLineWidth_)); markerArray.markers.back().ns = "EE Trajectories"; } - markerArray.markers.emplace_back(getLineMsg(std::move(mpcComPositionMsgs), Color::red, trajectoryLineWidth_)); + markerArray.markers.emplace_back(getLineMsg( + std::move(mpcComPositionMsgs), Color::red, trajectoryLineWidth_)); 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_; @@ -337,19 +417,29 @@ void LeggedRobotVisualizer::publishOptimizedStateTrajectory(ros::Time timeStamp, const auto tStart = mpcTimeTrajectory.front(); const auto tEnd = mpcTimeTrajectory.back(); for (size_t event = 0; event < eventTimes.size(); ++event) { - if (tStart < eventTimes[event] && eventTimes[event] < tEnd) { // Only publish future footholds within the optimized horizon - const auto preEventContactFlags = modeNumber2StanceLeg(subsystemSequence[event]); - const auto postEventContactFlags = modeNumber2StanceLeg(subsystemSequence[event + 1]); - const auto postEventState = LinearInterpolation::interpolate(eventTimes[event], mpcTimeTrajectory, mpcStateTrajectory); + if (tStart < eventTimes[event] && + eventTimes[event] < tEnd) { // Only publish future footholds within the + // optimized horizon + const auto preEventContactFlags = + modeNumber2StanceLeg(subsystemSequence[event]); + const auto postEventContactFlags = + modeNumber2StanceLeg(subsystemSequence[event + 1]); + const auto postEventState = LinearInterpolation::interpolate( + eventTimes[event], mpcTimeTrajectory, mpcStateTrajectory); const auto& model = pinocchioInterface_.getModel(); auto& data = pinocchioInterface_.getData(); - pinocchio::forwardKinematics(model, data, centroidal_model::getGeneralizedCoordinates(postEventState, centroidalModelInfo_)); + pinocchio::forwardKinematics(model, data, + centroidal_model::getGeneralizedCoordinates( + postEventState, centroidalModelInfo_)); pinocchio::updateFramePlacements(model, data); - const auto feetPosition = endEffectorKinematicsPtr_->getPosition(postEventState); + const auto feetPosition = + endEffectorKinematicsPtr_->getPosition(postEventState); for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) { - if (!preEventContactFlags[i] && postEventContactFlags[i]) { // If a foot lands, a marker is added at that location. + if (!preEventContactFlags[i] && + postEventContactFlags[i]) { // If a foot lands, a marker is added + // at that location. sphereList.points.emplace_back(getPointMsg(feetPosition[i])); sphereList.colors.push_back(getColor(feetColorMap_[i])); } @@ -359,10 +449,11 @@ void LeggedRobotVisualizer::publishOptimizedStateTrajectory(ros::Time timeStamp, markerArray.markers.push_back(std::move(sphereList)); // Add headers and Id - assignHeader(markerArray.markers.begin(), markerArray.markers.end(), getHeaderMsg(frameId_, 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..95b259729 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/CMakeLists.txt @@ -1,82 +1,28 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_mobile_manipulator) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_ddp - ocs2_mpc - ocs2_robotic_tools - ocs2_robotic_assets - ocs2_pinocchio_interface - ocs2_self_collision -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(pinocchio REQUIRED pinocchio) - +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(ocs2_self_collision REQUIRED) +find_package(pinocchio 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 - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost - pinocchio -) - -########### -## Build ## -########### - -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 -) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) # Resolve for the package path at compile time. -configure_file ( +configure_file( "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h.in" - "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY -) - -# Add directories for all targets -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -link_directories( - ${pinocchio_LIBRARY_DIRS} + "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" + @ONLY ) -# mobile maniulator interface library add_library(${PROJECT_NAME} src/FactoryFunctions.cpp src/MobileManipulatorPreComputation.cpp @@ -88,67 +34,96 @@ add_library(${PROJECT_NAME} src/dynamics/FullyActuatedFloatingArmManipulatorDynamics.cpp src/MobileManipulatorInterface.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(${PROJECT_NAME} - ${Boost_LIBRARIES} - ${catkin_LIBRARIES} - ${pinocchio_LIBRARIES} + pinocchio::pinocchio + Boost::filesystem + Boost::system ) -target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) - -#################### -## Clang tooling ### -#################### - -find_package(cmake_clang_tools QUIET) -if (cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif (cmake_clang_tools_FOUND) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${pinocchio_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_ddp + ocs2_mpc + ocs2_robotic_tools + ocs2_robotic_assets + ocs2_pinocchio_interface + ocs2_self_collision +) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_PINOCCHIO_FLAGS}) ############# ## Install ## ############# 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} + DESTINATION include/${PROJECT_NAME} ) install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) -############# -## Testing ## -############# +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) -# Helper macro for adding target applications -macro(add_ocs2_test APP_NAME APP_SRC) - catkin_add_gtest(${APP_NAME} - ${APP_SRC} - ) - target_include_directories(${APP_NAME} PRIVATE - ${PROJECT_BINARY_DIR}/include - ) - target_link_libraries(${APP_NAME} - gtest_main - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ) -endmacro() +ament_export_include_directories( + include + ${PROJECT_BINARY_DIR}/include + ${pinocchio_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_ddp + ocs2_mpc + ocs2_robotic_tools + ocs2_robotic_assets + ocs2_pinocchio_interface + ocs2_self_collision + pinocchio + Eigen3 + Boost +) -add_ocs2_test(SelfCollisionTest test/testSelfCollision.cpp) -add_ocs2_test(EndEffectorConstraintTest test/testEndEffectorConstraint.cpp) -add_ocs2_test(DummyMobileManipulatorTest test/testDummyMobileManipulator.cpp) +############ +# Testing ## +############ + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + macro(add_ocs2_test APP_NAME APP_SRC) + ament_add_gtest(${APP_NAME} + ${APP_SRC} + ) + target_include_directories(${APP_NAME} PRIVATE + ${PROJECT_BINARY_DIR}/include + ) + target_link_libraries(${APP_NAME} + gtest_main + ${PROJECT_NAME} + ) + endmacro() + + add_ocs2_test(SelfCollisionTest test/testSelfCollision.cpp) + add_ocs2_test(EndEffectorConstraintTest test/testEndEffectorConstraint.cpp) + add_ocs2_test(DummyMobileManipulatorTest test/testDummyMobileManipulator.cpp) + set_tests_properties(DummyMobileManipulatorTest PROPERTIES TIMEOUT 600) +endif() + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/package.xml b/ocs2_robotic_examples/ocs2_mobile_manipulator/package.xml index 0d1ff99af..d108f8ff4 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/package.xml +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/package.xml @@ -1,5 +1,5 @@ - + ocs2_mobile_manipulator 0.0.0 The ocs2_mobile_manipulator package @@ -10,8 +10,7 @@ TODO - catkin - cmake_clang_tools + ament_cmake ocs2_core ocs2_ddp @@ -21,5 +20,13 @@ ocs2_pinocchio_interface ocs2_self_collision pinocchio + eigen3_cmake_module + boost - \ No newline at end of file + ament_cmake_gtest + + + ament_cmake + + + diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/CMakeLists.txt index 6831c0606..3430a839a 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/CMakeLists.txt @@ -1,17 +1,43 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_mobile_manipulator_ros) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -set(CATKIN_PACKAGE_DEPENDENCIES - roslib - tf +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(urdf REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(robot_state_publisher REQUIRED) + +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(ocs2_self_collision REQUIRED) +find_package(ocs2_self_collision_visualization REQUIRED) +find_package(ocs2_mobile_manipulator REQUIRED) + +find_package(pinocchio REQUIRED) +find_package(hpp-fcl REQUIRED) +find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) + +set(dependencies + rclcpp + tf2_ros + sensor_msgs + geometry_msgs + visualization_msgs urdf kdl_parser robot_state_publisher - visualization_msgs - geometry_msgs ocs2_ros_interfaces ocs2_core ocs2_ddp @@ -24,139 +50,61 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_mobile_manipulator ) -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(pinocchio REQUIRED pinocchio) - -find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost - pinocchio -) - -########### -## Build ## -########### - -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} - ${Boost_INCLUDE_DIRS} - ${pinocchio_INCLUDE_DIRS} -) - -link_directories( - ${pinocchio_LIBRARY_DIRS} -) - -# MPC node add_executable(mobile_manipulator_mpc_node src/MobileManipulatorMpcNode.cpp ) -add_dependencies(mobile_manipulator_mpc_node - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(mobile_manipulator_mpc_node - ${catkin_LIBRARIES} +target_include_directories(mobile_manipulator_mpc_node PRIVATE + $ ) -target_compile_options(mobile_manipulator_mpc_node PUBLIC ${FLAGS}) +ament_target_dependencies(mobile_manipulator_mpc_node ${dependencies}) +target_compile_options(mobile_manipulator_mpc_node PRIVATE ${OCS2_PINOCCHIO_FLAGS}) -# DistanceVisualization node add_executable(mobile_manipulator_distance_visualization src/MobileManipulatorDistanceVisualization.cpp ) -add_dependencies(mobile_manipulator_distance_visualization - ${catkin_EXPORTED_TARGETS} +target_include_directories(mobile_manipulator_distance_visualization PRIVATE + $ ) -target_link_libraries(mobile_manipulator_distance_visualization - ${catkin_LIBRARIES} -) -target_compile_options(mobile_manipulator_distance_visualization PUBLIC ${FLAGS}) +ament_target_dependencies(mobile_manipulator_distance_visualization ${dependencies}) +target_compile_options(mobile_manipulator_distance_visualization PRIVATE ${OCS2_PINOCCHIO_FLAGS}) -# Dummy node add_executable(mobile_manipulator_dummy_mrt_node src/MobileManipulatorDummyMRT.cpp src/MobileManipulatorDummyVisualization.cpp ) -add_dependencies(mobile_manipulator_dummy_mrt_node - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(mobile_manipulator_dummy_mrt_node - ${catkin_LIBRARIES} +target_include_directories(mobile_manipulator_dummy_mrt_node PRIVATE + $ ) -target_compile_options(mobile_manipulator_dummy_mrt_node PUBLIC ${FLAGS}) +ament_target_dependencies(mobile_manipulator_dummy_mrt_node ${dependencies}) +target_compile_options(mobile_manipulator_dummy_mrt_node PRIVATE ${OCS2_PINOCCHIO_FLAGS}) -# Target node add_executable(mobile_manipulator_target src/MobileManipulatorTarget.cpp ) -add_dependencies(mobile_manipulator_target - ${catkin_EXPORTED_TARGETS} +target_include_directories(mobile_manipulator_target PRIVATE + $ ) -target_link_libraries(mobile_manipulator_target - ${catkin_LIBRARIES} -) -target_compile_options(mobile_manipulator_target PUBLIC ${FLAGS}) - -#################### -## Clang tooling ### -#################### - -find_package(cmake_clang_tools QUIET) -if (cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS mobile_manipulator_mpc_node mobile_manipulator_dummy_mrt_node - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif (cmake_clang_tools_FOUND) +ament_target_dependencies(mobile_manipulator_target ${dependencies}) +target_compile_options(mobile_manipulator_target PRIVATE ${OCS2_PINOCCHIO_FLAGS}) ############# ## Install ## ############# -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(TARGETS + mobile_manipulator_mpc_node + mobile_manipulator_distance_visualization + mobile_manipulator_dummy_mrt_node + mobile_manipulator_target + RUNTIME DESTINATION lib/${PROJECT_NAME} ) -install( - TARGETS - mobile_manipulator_mpc_node - mobile_manipulator_distance_visualization - mobile_manipulator_dummy_mrt_node - mobile_manipulator_target - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} ) + install(DIRECTORY launch rviz - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/include/ocs2_mobile_manipulator_ros/MobileManipulatorDummyVisualization.h b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/include/ocs2_mobile_manipulator_ros/MobileManipulatorDummyVisualization.h index aeb71df61..17c56aa1c 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/include/ocs2_mobile_manipulator_ros/MobileManipulatorDummyVisualization.h +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/include/ocs2_mobile_manipulator_ros/MobileManipulatorDummyVisualization.h @@ -29,45 +29,58 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include -#include - -#include - #include #include +#include #include +#include + +#include +#include namespace ocs2 { namespace mobile_manipulator { class MobileManipulatorDummyVisualization final : public DummyObserver { public: - MobileManipulatorDummyVisualization(ros::NodeHandle& nodeHandle, const MobileManipulatorInterface& interface) - : pinocchioInterface_(interface.getPinocchioInterface()), modelInfo_(interface.getManipulatorModelInfo()) { - launchVisualizerNode(nodeHandle); + MobileManipulatorDummyVisualization( + const rclcpp::Node::SharedPtr& node, + const MobileManipulatorInterface& interface) + : node_(node), + pinocchioInterface_(interface.getPinocchioInterface()), + modelInfo_(interface.getManipulatorModelInfo()), + tfBroadcaster_(node) { + launchVisualizerNode(); } ~MobileManipulatorDummyVisualization() override = default; - void update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) override; + void update(const SystemObservation& observation, + const PrimalSolution& policy, + const CommandData& command) override; private: - void launchVisualizerNode(ros::NodeHandle& nodeHandle); + void launchVisualizerNode(); - void publishObservation(const ros::Time& timeStamp, const SystemObservation& observation); - void publishTargetTrajectories(const ros::Time& timeStamp, const TargetTrajectories& targetTrajectories); - void publishOptimizedTrajectory(const ros::Time& timeStamp, const PrimalSolution& policy); + void publishObservation(const rclcpp::Time& timeStamp, + const SystemObservation& observation); + void publishTargetTrajectories(const rclcpp::Time& timeStamp, + const TargetTrajectories& targetTrajectories); + void publishOptimizedTrajectory(const rclcpp::Time& timeStamp, + const PrimalSolution& policy); + rclcpp::Node::SharedPtr node_; PinocchioInterface pinocchioInterface_; const ManipulatorModelInfo modelInfo_; std::vector removeJointNames_; - std::unique_ptr robotStatePublisherPtr_; - tf::TransformBroadcaster tfBroadcaster_; + rclcpp::Publisher::SharedPtr jointPublisher_; + tf2_ros::TransformBroadcaster tfBroadcaster_; - ros::Publisher stateOptimizedPublisher_; - ros::Publisher stateOptimizedPosePublisher_; + rclcpp::Publisher::SharedPtr + stateOptimizedPublisher_; + rclcpp::Publisher::SharedPtr + stateOptimizedPosePublisher_; std::unique_ptr geometryVisualization_; }; diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator.launch.py new file mode 100644 index 000000000..69c49d079 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator.launch.py @@ -0,0 +1,122 @@ +import os +from launch.substitutions import LaunchConfiguration + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + default_task_file = get_package_share_directory('ocs2_mobile_manipulator') + '/config/franka/task.info' + default_urdf_file = get_package_share_directory('ocs2_robotic_assets') + '/resources/mobile_manipulator/franka/urdf/panda.urdf' + default_lib_folder = '/tmp/ocs2_mobile_manipulator_auto_generated' + + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='debug', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='gdb_prefix', + default_value='gdb -ex run --args' + ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value=default_task_file + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=default_urdf_file + ), + launch.actions.DeclareLaunchArgument( + name='libFolder', + default_value=default_lib_folder + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_mobile_manipulator_ros'), 'launch/include/visualize.launch.py') + ), + launch_arguments={ + 'urdfFile': LaunchConfiguration('urdfFile'), + 'rviz': LaunchConfiguration('rviz') + }.items() + ), + launch_ros.actions.Node( + package='ocs2_mobile_manipulator_ros', + executable='mobile_manipulator_mpc_node', + name='mobile_manipulator_mpc', + prefix=LaunchConfiguration('gdb_prefix'), + condition=launch.conditions.IfCondition(LaunchConfiguration("debug")), + output='screen', + parameters=[ + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + }, + { + 'libFolder': launch.substitutions.LaunchConfiguration('libFolder') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_mobile_manipulator_ros', + executable='mobile_manipulator_mpc_node', + name='mobile_manipulator_mpc', + prefix=LaunchConfiguration('terminal_prefix'), + condition=launch.conditions.UnlessCondition(LaunchConfiguration("debug")), + output='screen', + parameters=[ + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + }, + { + 'libFolder': launch.substitutions.LaunchConfiguration('libFolder') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_mobile_manipulator_ros', + executable='mobile_manipulator_dummy_mrt_node', + name='mobile_manipulator_dummy_mrt_node', + prefix=LaunchConfiguration('terminal_prefix'), + output='screen', + parameters=[ + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + }, + { + 'libFolder': launch.substitutions.LaunchConfiguration('libFolder') + } + ] + ), + launch_ros.actions.Node( + package='ocs2_mobile_manipulator_ros', + executable='mobile_manipulator_target', + name='mobile_manipulator_target', + prefix=LaunchConfiguration('terminal_prefix'), + output='screen', + parameters=[] + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator_distance.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator_distance.launch.py new file mode 100644 index 000000000..8eb33307c --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator_distance.launch.py @@ -0,0 +1,64 @@ +import os +from launch.substitutions import LaunchConfiguration + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + default_task_file = get_package_share_directory('ocs2_mobile_manipulator') + '/config/franka/task.info' + default_urdf_file = get_package_share_directory('ocs2_robotic_assets') + '/resources/mobile_manipulator/franka/urdf/panda.urdf' + + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=default_urdf_file + ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value=default_task_file + ), + launch.actions.DeclareLaunchArgument( + name='rvizconfig', + default_value=get_package_share_directory('ocs2_mobile_manipulator_ros') + "/rviz/mobile_manipulator_distance.rviz" + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_mobile_manipulator_ros'), 'launch/include/visualize.launch.py') + ), + launch_arguments={ + 'urdfFile': LaunchConfiguration('urdfFile'), + 'rvizconfig': LaunchConfiguration('rvizconfig') + }.items() + ), + launch_ros.actions.Node( + package='joint_state_publisher_gui', + executable='joint_state_publisher_gui', + name='mobile_manipulator_joint_state_publisher', + parameters=[ + { + 'rate': "100" + } + ] + ), + launch_ros.actions.Node( + package='ocs2_mobile_manipulator_ros', + executable='mobile_manipulator_distance_visualization', + name='mobile_manipulator_distance_visualization', + parameters=[ + { + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile') + }, + { + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile') + } + ] + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/visualize.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/visualize.launch.py new file mode 100644 index 000000000..998678323 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/visualize.launch.py @@ -0,0 +1,52 @@ +import os +import launch +import launch_ros.actions +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory('ocs2_robotic_assets') + "/resources/mobile_manipulator/franka/urdf/panda.urdf" + ), + launch.actions.DeclareLaunchArgument( + name='test', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='rvizconfig', + default_value=get_package_share_directory('ocs2_mobile_manipulator_ros') + "/rviz/mobile_manipulator.rviz" + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + arguments=[LaunchConfiguration("urdfFile")], + ), + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + arguments=[LaunchConfiguration("urdfFile")], + condition=IfCondition(LaunchConfiguration("test")), + ), + Node( + package='rviz2', + executable='rviz2', + name='mobile_manipulator', + output='screen', + condition=IfCondition(LaunchConfiguration("rviz")), + arguments=["-d", LaunchConfiguration("rvizconfig")] + ) + ]) + return ld + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_franka.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_franka.launch.py new file mode 100644 index 000000000..c1a2e4479 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_franka.launch.py @@ -0,0 +1,51 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='debug', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory( + 'ocs2_robotic_assets') + '/resources/mobile_manipulator/franka/urdf/panda.urdf' + ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value=get_package_share_directory( + 'ocs2_mobile_manipulator') + '/config/franka/task.info' + ), + launch.actions.DeclareLaunchArgument( + name='libFolder', + default_value='/tmp/ocs2_mobile_manipulator_auto_generated/franka' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_mobile_manipulator_ros'), 'launch/include/mobile_manipulator.launch.py') + ), + launch_arguments={ + 'rviz': launch.substitutions.LaunchConfiguration('rviz'), + 'debug': launch.substitutions.LaunchConfiguration('debug'), + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile'), + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile'), + 'libFolder': launch.substitutions.LaunchConfiguration('libFolder') + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n6.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n6.launch.py new file mode 100644 index 000000000..20d24ae64 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n6.launch.py @@ -0,0 +1,51 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='debug', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory( + 'ocs2_robotic_assets') + '/resources/mobile_manipulator/kinova/urdf/j2n6s300.urdf' + ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value=get_package_share_directory( + 'ocs2_mobile_manipulator') + '/config/kinova/task_j2n6.info' + ), + launch.actions.DeclareLaunchArgument( + name='libFolder', + default_value='/tmp/ocs2_mobile_manipulator_auto_generated/kinova/j2n6' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_mobile_manipulator_ros'), 'launch/include/mobile_manipulator.launch.py') + ), + launch_arguments={ + 'rviz': launch.substitutions.LaunchConfiguration('rviz'), + 'debug': launch.substitutions.LaunchConfiguration('debug'), + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile'), + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile'), + 'libFolder': launch.substitutions.LaunchConfiguration('libFolder') + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n7.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n7.launch.py new file mode 100644 index 000000000..01722e724 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n7.launch.py @@ -0,0 +1,51 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='debug', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory( + 'ocs2_robotic_assets') + '/resources/mobile_manipulator/kinova/urdf/j2n7s300.urdf' + ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value=get_package_share_directory( + 'ocs2_mobile_manipulator') + '/config/kinova/task_j2n7.info' + ), + launch.actions.DeclareLaunchArgument( + name='libFolder', + default_value='/tmp/ocs2_mobile_manipulator_auto_generated/kinova/j2n7' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_mobile_manipulator_ros'), 'launch/include/mobile_manipulator.launch.py') + ), + launch_arguments={ + 'rviz': launch.substitutions.LaunchConfiguration('rviz'), + 'debug': launch.substitutions.LaunchConfiguration('debug'), + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile'), + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile'), + 'libFolder': launch.substitutions.LaunchConfiguration('libFolder') + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_mabi_mobile.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_mabi_mobile.launch.py new file mode 100644 index 000000000..dbc403a58 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_mabi_mobile.launch.py @@ -0,0 +1,51 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='debug', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory( + 'ocs2_robotic_assets') + '/resources/mobile_manipulator/mabi_mobile/urdf/mabi_mobile.urdf' + ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value=get_package_share_directory( + 'ocs2_mobile_manipulator') + '/config/mabi_mobile/task.info' + ), + launch.actions.DeclareLaunchArgument( + name='libFolder', + default_value='/tmp/ocs2_mobile_manipulator_auto_generated/mabi_mobile' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_mobile_manipulator_ros'), 'launch/include/mobile_manipulator.launch.py') + ), + launch_arguments={ + 'rviz': launch.substitutions.LaunchConfiguration('rviz'), + 'debug': launch.substitutions.LaunchConfiguration('debug'), + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile'), + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile'), + 'libFolder': launch.substitutions.LaunchConfiguration('libFolder') + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_pr2.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_pr2.launch.py new file mode 100644 index 000000000..4ffc2b3fc --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_pr2.launch.py @@ -0,0 +1,51 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='debug', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory( + 'ocs2_robotic_assets') + '/resources/mobile_manipulator/pr2/urdf/pr2.urdf' + ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value=get_package_share_directory( + 'ocs2_mobile_manipulator') + '/config/pr2/task.info' + ), + launch.actions.DeclareLaunchArgument( + name='libFolder', + default_value='/tmp/ocs2_mobile_manipulator_auto_generated/pr2' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_mobile_manipulator_ros'), 'launch/include/mobile_manipulator.launch.py') + ), + launch_arguments={ + 'rviz': launch.substitutions.LaunchConfiguration('rviz'), + 'debug': launch.substitutions.LaunchConfiguration('debug'), + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile'), + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile'), + 'libFolder': launch.substitutions.LaunchConfiguration('libFolder') + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_ridgeback_ur5.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_ridgeback_ur5.launch.py new file mode 100644 index 000000000..5fac41125 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_ridgeback_ur5.launch.py @@ -0,0 +1,51 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='debug', + default_value='false' + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory( + 'ocs2_robotic_assets') + '/resources/mobile_manipulator/ridgeback_ur5/urdf/ridgeback_ur5.urdf' + ), + launch.actions.DeclareLaunchArgument( + name='taskFile', + default_value=get_package_share_directory( + 'ocs2_mobile_manipulator') + '/config/ridgeback_ur5/task.info' + ), + launch.actions.DeclareLaunchArgument( + name='libFolder', + default_value='/tmp/ocs2_mobile_manipulator_auto_generated/ridgeback_ur5' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_mobile_manipulator_ros'), 'launch/include/mobile_manipulator.launch.py') + ), + launch_arguments={ + 'rviz': launch.substitutions.LaunchConfiguration('rviz'), + 'debug': launch.substitutions.LaunchConfiguration('debug'), + 'urdfFile': launch.substitutions.LaunchConfiguration('urdfFile'), + 'taskFile': launch.substitutions.LaunchConfiguration('taskFile'), + 'libFolder': launch.substitutions.LaunchConfiguration('libFolder') + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/package.xml b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/package.xml index 3a68339d1..2910227af 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/package.xml @@ -1,5 +1,5 @@ - + ocs2_mobile_manipulator_ros 0.0.0 The ocs2_mobile_manipulator_ros package @@ -11,26 +11,34 @@ BSD3 - catkin - cmake_clang_tools + ament_cmake - roslib - tf + rclcpp + tf2_ros + sensor_msgs + geometry_msgs + visualization_msgs urdf kdl_parser robot_state_publisher - visualization_msgs - geometry_msgs + + ocs2_ros_interfaces ocs2_core ocs2_ddp ocs2_mpc ocs2_robotic_tools ocs2_robotic_assets - ocs2_ros_interfaces ocs2_pinocchio_interface ocs2_self_collision ocs2_self_collision_visualization ocs2_mobile_manipulator pinocchio + hpp-fcl + + eigen3_cmake_module + boost + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/rviz/mobile_manipulator.rviz b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/rviz/mobile_manipulator.rviz index 2de59a852..1c7a75667 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/rviz/mobile_manipulator.rviz +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/rviz/mobile_manipulator.rviz @@ -1,41 +1,37 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 + - /RobotModel1 Splitter Ratio: 0.5 - Tree Height: 555 - - Class: rviz/Selection + Tree Height: 439 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -52,92 +48,136 @@ Visualization Manager: Reference Frame: Value: true - Alpha: 0.699999988079071 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description Enabled: true Links: - ARM: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: Alpha: 1 Show Axes: false Show Trail: false Value: true - All Links Enabled: true - ELBOW: + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_leftfinger: Alpha: 1 Show Axes: false Show Trail: false Value: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - FOREARM: + panda_link0: Alpha: 1 Show Axes: false Show Trail: false Value: true - Link Tree Style: Links in Alphabetic Order - SHOULDER: + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: Alpha: 1 Show Axes: false Show Trail: false Value: true - WRIST_1: + panda_link3: Alpha: 1 Show Axes: false Show Trail: false Value: true - WRIST_2: + panda_link4: Alpha: 1 Show Axes: false Show Trail: false Value: true - arm_base: + panda_link5: Alpha: 1 Show Axes: false Show Trail: false Value: true - arm_base_inertia: + panda_link6: Alpha: 1 Show Axes: false Show Trail: false - base: + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: Alpha: 1 Show Axes: false Show Trail: false + Value: true + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + root: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false Name: RobotModel - Robot Description: robot_description TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true - - Class: rviz/TF + - Class: rviz_default_plugins/TF Enabled: true Frame Timeout: 15 Frames: - ARM: - Value: true All Enabled: true - ELBOW: + command: Value: true - FOREARM: + panda_hand: Value: true - SHOULDER: + panda_hand_tcp: Value: true - WRIST_1: + panda_leftfinger: Value: true - WRIST_2: + panda_link0: Value: true - arm_base: + panda_link1: Value: true - arm_base_inertia: + panda_link2: Value: true - base: + panda_link3: Value: true - command: + panda_link4: + Value: true + panda_link5: + Value: true + panda_link6: + Value: true + panda_link7: + Value: true + panda_link8: + Value: true + panda_rightfinger: + Value: true + root: Value: true world: Value: true - Marker Alpha: 1 Marker Scale: 1 Name: TF Show Arrows: true @@ -145,104 +185,137 @@ Visualization Manager: Show Names: true Tree: world: - base: - arm_base: - SHOULDER: - ARM: - ELBOW: - FOREARM: - WRIST_1: - WRIST_2: - {} - arm_base_inertia: - {} command: {} + root: + panda_link0: + panda_link1: + panda_link2: + panda_link3: + panda_link4: + panda_link5: + panda_link6: + panda_link7: + panda_link8: + panda_hand: + panda_hand_tcp: + {} + panda_leftfinger: + {} + panda_rightfinger: + {} Update Interval: 0 Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true - Marker Topic: /mobile_manipulator/optimizedStateTrajectory Name: MarkerArray Namespaces: - Base Trajectory: true - EE Trajectory: true - Queue Size: 100 + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array Value: true - Alpha: 1 Arrow Length: 0.30000001192092896 Axes Length: 0.10000000149011612 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 Head Radius: 0.029999999329447746 Name: PoseArray - Queue Size: 10 Shaft Length: 0.23000000417232513 Shaft Radius: 0.009999999776482582 Shape: Axes - Topic: /mobile_manipulator/optimizedPoseTrajectory - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /mobile_manipulator/optimizedPoseTrajectory Value: true - - Class: rviz/InteractiveMarkers + - Class: rviz_default_plugins/InteractiveMarkers Enable Transparency: true Enabled: true + Interactive Markers Namespace: "" Name: InteractiveMarkers Show Axes: false Show Descriptions: true Show Visual Aids: false - Update Topic: /simple_marker/update Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true - Marker Topic: /distance_markers Name: MarkerArray Namespaces: - 1 - 4: true - 1 - 6: true - Queue Size: 100 + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array Value: true Enabled: true Global Options: Background Color: 186; 189; 182 - Default Light: true Fixed Frame: world Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit - Distance: 5.996953964233398 + Class: rviz_default_plugins/Orbit + Distance: 1.6701574325561523 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 Focal Point: - X: 0 - Y: 0 - Z: 0 + X: -0.038962461054325104 + Y: -0.20587436854839325 + Z: 0.2906208038330078 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false @@ -250,15 +323,16 @@ Visualization Manager: Near Clip Distance: 0.009999999776482582 Pitch: 0.4003985524177551 Target Frame: + Value: Orbit (rviz_default_plugins) Yaw: 0.44039851427078247 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 846 + Height: 736 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000015600000242fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000242000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004ba0000003efc0100000002fb0000000800540069006d00650100000000000004ba000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000035e0000024200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -267,6 +341,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1200 - X: 720 - Y: 280 + Width: 1210 + X: 70 + Y: 27 diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/rviz/mobile_manipulator_distance.rviz b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/rviz/mobile_manipulator_distance.rviz index 79a61901c..3eb710c43 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/rviz/mobile_manipulator_distance.rviz +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/rviz/mobile_manipulator_distance.rviz @@ -1,5 +1,5 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: @@ -8,21 +8,21 @@ Panels: - /MarkerArray1 Splitter Ratio: 0.5 Tree Height: 732 - - Class: rviz/Selection + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 @@ -36,7 +36,7 @@ Visualization Manager: Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -53,7 +53,7 @@ Visualization Manager: Reference Frame: Value: true - Alpha: 0.699999988079071 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: true Enabled: true Links: @@ -111,7 +111,7 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: false - - Class: rviz/TF + - Class: rviz_default_plugins/TF Enabled: true Frame Timeout: 15 Frames: @@ -153,7 +153,7 @@ Visualization Manager: {} Update Interval: 0 Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: /distance_markers Name: MarkerArray @@ -165,7 +165,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.10000000149011612 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -185,26 +185,26 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + - Class: rviz_default_plugins/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - - Class: rviz/SetGoal + - Class: rviz_default_plugins/SetGoal Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: - Class: rviz/Orbit + Class: rviz_default_plugins/Orbit Distance: 1.469738483428955 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDistanceVisualization.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDistanceVisualization.cpp index 570a42606..f62375e30 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDistanceVisualization.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDistanceVisualization.cpp @@ -28,22 +28,21 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ // needs to be included before boost -#include - #include #include +#include +#include +#include #include - #include #include -#include -#include -#include +#include +#include -#include -#include -#include +#include + +#include "rclcpp/rclcpp.hpp" using namespace ocs2; using namespace mobile_manipulator; @@ -52,11 +51,9 @@ std::unique_ptr pInterface; std::shared_ptr gInterface; std::unique_ptr vInterface; -sensor_msgs::JointState lastMsg; - -std::unique_ptr pub; +sensor_msgs::msg::JointState lastMsg; -void jointStateCallback(sensor_msgs::JointStateConstPtr msg) { +void jointStateCallback(sensor_msgs::msg::JointState::ConstSharedPtr msg) { if (lastMsg.position == msg->position) { return; } @@ -73,31 +70,46 @@ void jointStateCallback(sensor_msgs::JointStateConstPtr msg) { int main(int argc, char** argv) { // Initialize ros node - ros::init(argc, argv, "distance_visualization"); - ros::NodeHandle nodeHandle; - // Get ROS parameters - std::string urdfPath, taskFile; - nodeHandle.getParam("/taskFile", taskFile); - nodeHandle.getParam("/urdfFile", urdfPath); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared("distance_visualization"); + + const std::string taskFile = + node->declare_parameter("taskFile", ""); + const std::string urdfPath = + node->declare_parameter("urdfFile", ""); + if (taskFile.empty() || urdfPath.empty()) { + throw std::runtime_error( + "[MobileManipulatorDistanceVisualization] Parameters 'taskFile' and " + "'urdfFile' are required."); + } // read the task file boost::property_tree::ptree pt; boost::property_tree::read_info(taskFile, pt); // read manipulator type - ManipulatorModelType modelType = mobile_manipulator::loadManipulatorType(taskFile, "model_information.manipulatorModelType"); + ManipulatorModelType modelType = mobile_manipulator::loadManipulatorType( + taskFile, "model_information.manipulatorModelType"); // read the joints to make fixed std::vector removeJointNames; - loadData::loadStdVector(taskFile, "model_information.removeJoints", removeJointNames, true); + loadData::loadStdVector( + taskFile, "model_information.removeJoints", removeJointNames, true); // read the frame names std::string baseFrame; - loadData::loadPtreeValue(pt, baseFrame, "model_information.baseFrame", false); + loadData::loadPtreeValue(pt, baseFrame, + "model_information.baseFrame", false); // create pinocchio interface - pInterface.reset(new PinocchioInterface(::ocs2::mobile_manipulator::createPinocchioInterface(urdfPath, modelType))); + pInterface.reset(new PinocchioInterface( + ::ocs2::mobile_manipulator::createPinocchioInterface(urdfPath, + modelType))); std::cerr << "\n #### Model Information:"; - std::cerr << "\n #### =============================================================================\n"; - std::cerr << "\n #### model_information.manipulatorModelType: " << static_cast(modelType); + std::cerr << "\n #### " + "===============================================================" + "==============\n"; + std::cerr << "\n #### model_information.manipulatorModelType: " + << static_cast(modelType); std::cerr << "\n #### model_information.removeJoints: "; for (const auto& name : removeJointNames) { std::cerr << "\"" << name << "\" "; @@ -106,8 +118,10 @@ int main(int argc, char** argv) { std::vector> selfCollisionObjectPairs; std::vector> selfCollisionLinkPairs; - loadData::loadStdVectorOfPair(taskFile, "selfCollision.collisionObjectPairs", selfCollisionObjectPairs); - loadData::loadStdVectorOfPair(taskFile, "selfCollision.collisionLinkPairs", selfCollisionLinkPairs); + loadData::loadStdVectorOfPair(taskFile, "selfCollision.collisionObjectPairs", + selfCollisionObjectPairs); + loadData::loadStdVectorOfPair(taskFile, "selfCollision.collisionLinkPairs", + selfCollisionLinkPairs); for (const auto& element : selfCollisionObjectPairs) { std::cerr << "[" << element.first << ", " << element.second << "]; "; } @@ -118,13 +132,16 @@ int main(int argc, char** argv) { } std::cerr << std::endl; - gInterface.reset(new PinocchioGeometryInterface(*pInterface, selfCollisionLinkPairs, selfCollisionObjectPairs)); + gInterface.reset(new PinocchioGeometryInterface( + *pInterface, selfCollisionLinkPairs, selfCollisionObjectPairs)); - vInterface.reset(new GeometryInterfaceVisualization(*pInterface, *gInterface, nodeHandle, baseFrame)); + vInterface.reset( + new GeometryInterfaceVisualization(*pInterface, *gInterface, baseFrame)); - ros::Subscriber sub = nodeHandle.subscribe("joint_states", 1, &jointStateCallback); + auto sub = node->create_subscription( + "joint_states", 1, &jointStateCallback); - ros::spin(); + rclcpp::spin(node); return 0; } diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp index b472ae59b..e951057cd 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp @@ -28,15 +28,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ #include - #include - #include #include #include -#include -#include +#include + +#include "rclcpp/rclcpp.hpp" using namespace ocs2; using namespace mobile_manipulator; @@ -45,29 +44,41 @@ int main(int argc, char** argv) { const std::string robotName = "mobile_manipulator"; // Initialize ros node - ros::init(argc, argv, robotName + "_mrt"); - ros::NodeHandle nodeHandle; - // Get node parameters - std::string taskFile, libFolder, urdfFile; - nodeHandle.getParam("/taskFile", taskFile); - nodeHandle.getParam("/libFolder", libFolder); - nodeHandle.getParam("/urdfFile", urdfFile); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_mrt"); + + const std::string taskFile = + node->declare_parameter("taskFile", ""); + const std::string libFolder = + node->declare_parameter("libFolder", ""); + const std::string urdfFile = + node->declare_parameter("urdfFile", ""); + if (taskFile.empty() || libFolder.empty() || urdfFile.empty()) { + throw std::runtime_error( + "[MobileManipulatorDummyMRT] Parameters 'taskFile', 'libFolder', and " + "'urdfFile' are required."); + } std::cerr << "Loading task file: " << taskFile << std::endl; std::cerr << "Loading library folder: " << libFolder << std::endl; std::cerr << "Loading urdf file: " << urdfFile << std::endl; // Robot Interface - mobile_manipulator::MobileManipulatorInterface interface(taskFile, libFolder, urdfFile); + mobile_manipulator::MobileManipulatorInterface interface(taskFile, libFolder, + urdfFile); // MRT MRT_ROS_Interface mrt(robotName); mrt.initRollout(&interface.getRollout()); - mrt.launchNodes(nodeHandle); + mrt.launchNodes(node); // Visualization - auto dummyVisualization = std::make_shared(nodeHandle, interface); + auto dummyVisualization = + std::make_shared( + node, interface); // Dummy MRT - MRT_ROS_Dummy_Loop dummy(mrt, interface.mpcSettings().mrtDesiredFrequency_, interface.mpcSettings().mpcDesiredFrequency_); + MRT_ROS_Dummy_Loop dummy(mrt, interface.mpcSettings().mrtDesiredFrequency_, + interface.mpcSettings().mpcDesiredFrequency_); dummy.subscribeObservers({dummyVisualization}); // initial state @@ -80,8 +91,10 @@ int main(int argc, char** argv) { vector_t initTarget(7); initTarget.head(3) << 1, 0, 1; initTarget.tail(4) << Eigen::Quaternion(1, 0, 0, 0).coeffs(); - const vector_t zeroInput = vector_t::Zero(interface.getManipulatorModelInfo().inputDim); - const TargetTrajectories initTargetTrajectories({initObservation.time}, {initTarget}, {zeroInput}); + const vector_t zeroInput = + vector_t::Zero(interface.getManipulatorModelInfo().inputDim); + const TargetTrajectories initTargetTrajectories({initObservation.time}, + {initTarget}, {zeroInput}); // Run dummy (loops while ros is ok) dummy.run(initObservation, initTargetTrajectories); diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyVisualization.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyVisualization.cpp index 9a1d6dcee..90b688cf6 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyVisualization.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyVisualization.cpp @@ -27,28 +27,26 @@ 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 -#include -#include - -#include -#include - #include #include -#include - #include #include #include #include #include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" namespace ocs2 { namespace mobile_manipulator { @@ -57,7 +55,7 @@ namespace mobile_manipulator { /******************************************************************************************************/ /******************************************************************************************************/ 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; } @@ -76,54 +74,55 @@ void assignIncreasingId(It firstIt, It lastIt, int startId = 0) { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MobileManipulatorDummyVisualization::launchVisualizerNode(ros::NodeHandle& nodeHandle) { - // load a kdl-tree from the urdf robot description and initialize the robot state publisher - const std::string urdfName = "robot_description"; - urdf::Model model; - if (!model.initParam(urdfName)) { - ROS_ERROR("URDF model load was NOT successful"); - } - KDL::Tree tree; - if (!kdl_parser::treeFromUrdfModel(model, tree)) { - ROS_ERROR("Failed to extract kdl tree from xml robot description"); - } - - robotStatePublisherPtr_.reset(new robot_state_publisher::RobotStatePublisher(tree)); - robotStatePublisherPtr_->publishFixedTransforms(true); - - stateOptimizedPublisher_ = nodeHandle.advertise("/mobile_manipulator/optimizedStateTrajectory", 1); - stateOptimizedPosePublisher_ = nodeHandle.advertise("/mobile_manipulator/optimizedPoseTrajectory", 1); +void MobileManipulatorDummyVisualization::launchVisualizerNode() { + jointPublisher_ = + node_->create_publisher("joint_states", 1); + stateOptimizedPublisher_ = + node_->create_publisher( + "/mobile_manipulator/optimizedStateTrajectory", 1); + stateOptimizedPosePublisher_ = + node_->create_publisher( + "/mobile_manipulator/optimizedPoseTrajectory", 1); // Get ROS parameter - std::string urdfFile, taskFile; - nodeHandle.getParam("/urdfFile", urdfFile); - nodeHandle.getParam("/taskFile", taskFile); + std::string taskFile = node_->get_parameter("taskFile").as_string(); + std::string urdfFile = node_->get_parameter("urdfFile").as_string(); // read manipulator type - ManipulatorModelType modelType = mobile_manipulator::loadManipulatorType(taskFile, "model_information.manipulatorModelType"); + ManipulatorModelType modelType = mobile_manipulator::loadManipulatorType( + taskFile, "model_information.manipulatorModelType"); // read the joints to make fixed - loadData::loadStdVector(taskFile, "model_information.removeJoints", removeJointNames_, false); + loadData::loadStdVector( + taskFile, "model_information.removeJoints", removeJointNames_, false); // read if self-collision checking active boost::property_tree::ptree pt; boost::property_tree::read_info(taskFile, pt); bool activateSelfCollision = true; - loadData::loadPtreeValue(pt, activateSelfCollision, "selfCollision.activate", true); + loadData::loadPtreeValue(pt, activateSelfCollision, "selfCollision.activate", + true); // create pinocchio interface - PinocchioInterface pinocchioInterface(mobile_manipulator::createPinocchioInterface(urdfFile, modelType, removeJointNames_)); + PinocchioInterface pinocchioInterface( + mobile_manipulator::createPinocchioInterface(urdfFile, modelType, + removeJointNames_)); // activate markers for self-collision visualization if (activateSelfCollision) { std::vector> collisionObjectPairs; - loadData::loadStdVectorOfPair(taskFile, "selfCollision.collisionObjectPairs", collisionObjectPairs, true); - PinocchioGeometryInterface geomInterface(pinocchioInterface, collisionObjectPairs); + loadData::loadStdVectorOfPair(taskFile, + "selfCollision.collisionObjectPairs", + collisionObjectPairs, true); + PinocchioGeometryInterface geomInterface(pinocchioInterface, + collisionObjectPairs); // set geometry visualization markers - geometryVisualization_.reset(new GeometryInterfaceVisualization(std::move(pinocchioInterface), geomInterface, nodeHandle)); + geometryVisualization_.reset(new GeometryInterfaceVisualization( + std::move(pinocchioInterface), geomInterface)); } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MobileManipulatorDummyVisualization::update(const SystemObservation& observation, const PrimalSolution& policy, - const CommandData& command) { - const ros::Time timeStamp = ros::Time::now(); +void MobileManipulatorDummyVisualization::update( + const SystemObservation& observation, const PrimalSolution& policy, + const CommandData& command) { + const rclcpp::Time timeStamp = node_->get_clock()->now(); publishObservation(timeStamp, observation); publishTargetTrajectories(timeStamp, command.mpcTargetTrajectories_); @@ -136,12 +135,14 @@ void MobileManipulatorDummyVisualization::update(const SystemObservation& observ /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MobileManipulatorDummyVisualization::publishObservation(const ros::Time& timeStamp, const SystemObservation& observation) { +void MobileManipulatorDummyVisualization::publishObservation( + const rclcpp::Time& timeStamp, const SystemObservation& observation) { // publish world -> base transform const auto r_world_base = getBasePosition(observation.state, modelInfo_); - const Eigen::Quaternion q_world_base = getBaseOrientation(observation.state, modelInfo_); + const Eigen::Quaternion q_world_base = + getBaseOrientation(observation.state, modelInfo_); - geometry_msgs::TransformStamped base_tf; + geometry_msgs::msg::TransformStamped base_tf; base_tf.header.stamp = timeStamp; base_tf.header.frame_id = "world"; base_tf.child_frame_id = modelInfo_.baseFrame; @@ -151,91 +152,115 @@ void MobileManipulatorDummyVisualization::publishObservation(const ros::Time& ti // publish joints transforms const auto j_arm = getArmJointAngles(observation.state, modelInfo_); - std::map jointPositions; - for (size_t i = 0; i < modelInfo_.dofNames.size(); i++) { - jointPositions[modelInfo_.dofNames[i]] = j_arm(i); + sensor_msgs::msg::JointState joint_state; + joint_state.header.stamp = node_->get_clock()->now(); + const auto dofNames_count = modelInfo_.dofNames.size(); + const auto joint_count = dofNames_count + removeJointNames_.size(); + joint_state.name.resize(joint_count); + joint_state.position.resize(joint_count); + for (size_t i = 0; i < dofNames_count; i++) { + joint_state.name[i] = modelInfo_.dofNames[i]; + joint_state.position[i] = j_arm(i); } + + auto joint_state_index = dofNames_count; for (const auto& name : removeJointNames_) { - jointPositions[name] = 0.0; + joint_state.name[joint_state_index] = name; + joint_state.position[joint_state_index] = 0.0; + joint_state_index++; } - robotStatePublisherPtr_->publishTransforms(jointPositions, timeStamp); + jointPublisher_->publish(joint_state); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MobileManipulatorDummyVisualization::publishTargetTrajectories(const ros::Time& timeStamp, - const TargetTrajectories& targetTrajectories) { +void MobileManipulatorDummyVisualization::publishTargetTrajectories( + const rclcpp::Time& timeStamp, + const TargetTrajectories& targetTrajectories) { // publish command transform - const Eigen::Vector3d eeDesiredPosition = targetTrajectories.stateTrajectory.back().head(3); + const Eigen::Vector3d eeDesiredPosition = + targetTrajectories.stateTrajectory.back().head(3); Eigen::Quaterniond eeDesiredOrientation; - eeDesiredOrientation.coeffs() = targetTrajectories.stateTrajectory.back().tail(4); - geometry_msgs::TransformStamped command_tf; + eeDesiredOrientation.coeffs() = + targetTrajectories.stateTrajectory.back().tail(4); + geometry_msgs::msg::TransformStamped command_tf; command_tf.header.stamp = timeStamp; command_tf.header.frame_id = "world"; command_tf.child_frame_id = "command"; - command_tf.transform.translation = ros_msg_helpers::getVectorMsg(eeDesiredPosition); - command_tf.transform.rotation = ros_msg_helpers::getOrientationMsg(eeDesiredOrientation); + command_tf.transform.translation = + ros_msg_helpers::getVectorMsg(eeDesiredPosition); + command_tf.transform.rotation = + ros_msg_helpers::getOrientationMsg(eeDesiredOrientation); tfBroadcaster_.sendTransform(command_tf); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MobileManipulatorDummyVisualization::publishOptimizedTrajectory(const ros::Time& timeStamp, const PrimalSolution& policy) { +void MobileManipulatorDummyVisualization::publishOptimizedTrajectory( + const rclcpp::Time& timeStamp, const PrimalSolution& policy) { const scalar_t TRAJECTORYLINEWIDTH = 0.005; const std::array red{0.6350, 0.0780, 0.1840}; const std::array blue{0, 0.4470, 0.7410}; const auto& mpcStateTrajectory = policy.stateTrajectory_; - visualization_msgs::MarkerArray markerArray; + visualization_msgs::msg::MarkerArray markerArray; // Base trajectory - std::vector baseTrajectory; + std::vector baseTrajectory; baseTrajectory.reserve(mpcStateTrajectory.size()); - geometry_msgs::PoseArray poseArray; + geometry_msgs::msg::PoseArray poseArray; poseArray.poses.reserve(mpcStateTrajectory.size()); // End effector trajectory const auto& model = pinocchioInterface_.getModel(); auto& data = pinocchioInterface_.getData(); - std::vector endEffectorTrajectory; + std::vector endEffectorTrajectory; endEffectorTrajectory.reserve(mpcStateTrajectory.size()); - std::for_each(mpcStateTrajectory.begin(), mpcStateTrajectory.end(), [&](const Eigen::VectorXd& state) { - pinocchio::forwardKinematics(model, data, state); - pinocchio::updateFramePlacements(model, data); - const auto eeIndex = model.getBodyId(modelInfo_.eeFrame); - const vector_t eePosition = data.oMf[eeIndex].translation(); - endEffectorTrajectory.push_back(ros_msg_helpers::getPointMsg(eePosition)); - }); - - markerArray.markers.emplace_back(ros_msg_helpers::getLineMsg(std::move(endEffectorTrajectory), blue, TRAJECTORYLINEWIDTH)); + std::for_each(mpcStateTrajectory.begin(), mpcStateTrajectory.end(), + [&](const Eigen::VectorXd& state) { + pinocchio::forwardKinematics(model, data, state); + pinocchio::updateFramePlacements(model, data); + const auto eeIndex = model.getBodyId(modelInfo_.eeFrame); + const vector_t eePosition = data.oMf[eeIndex].translation(); + endEffectorTrajectory.push_back( + ros_msg_helpers::getPointMsg(eePosition)); + }); + + markerArray.markers.emplace_back(ros_msg_helpers::getLineMsg( + std::move(endEffectorTrajectory), blue, TRAJECTORYLINEWIDTH)); markerArray.markers.back().ns = "EE Trajectory"; // Extract base pose from state - std::for_each(mpcStateTrajectory.begin(), mpcStateTrajectory.end(), [&](const vector_t& state) { - // extract from observation - const auto r_world_base = getBasePosition(state, modelInfo_); - const Eigen::Quaternion q_world_base = getBaseOrientation(state, modelInfo_); - - // convert to ros message - geometry_msgs::Pose pose; - pose.position = ros_msg_helpers::getPointMsg(r_world_base); - pose.orientation = ros_msg_helpers::getOrientationMsg(q_world_base); - baseTrajectory.push_back(pose.position); - poseArray.poses.push_back(std::move(pose)); - }); - - markerArray.markers.emplace_back(ros_msg_helpers::getLineMsg(std::move(baseTrajectory), red, TRAJECTORYLINEWIDTH)); + std::for_each(mpcStateTrajectory.begin(), mpcStateTrajectory.end(), + [&](const vector_t& state) { + // extract from observation + const auto r_world_base = getBasePosition(state, modelInfo_); + const Eigen::Quaternion q_world_base = + getBaseOrientation(state, modelInfo_); + + // convert to ros message + geometry_msgs::msg::Pose pose; + pose.position = ros_msg_helpers::getPointMsg(r_world_base); + pose.orientation = + ros_msg_helpers::getOrientationMsg(q_world_base); + baseTrajectory.push_back(pose.position); + poseArray.poses.push_back(std::move(pose)); + }); + + markerArray.markers.emplace_back(ros_msg_helpers::getLineMsg( + std::move(baseTrajectory), red, TRAJECTORYLINEWIDTH)); markerArray.markers.back().ns = "Base Trajectory"; - assignHeader(markerArray.markers.begin(), markerArray.markers.end(), ros_msg_helpers::getHeaderMsg("world", timeStamp)); + assignHeader(markerArray.markers.begin(), markerArray.markers.end(), + ros_msg_helpers::getHeaderMsg("world", timeStamp)); assignIncreasingId(markerArray.markers.begin(), markerArray.markers.end()); poseArray.header = ros_msg_helpers::getHeaderMsg("world", timeStamp); - stateOptimizedPublisher_.publish(markerArray); - stateOptimizedPosePublisher_.publish(poseArray); + stateOptimizedPublisher_->publish(markerArray); + stateOptimizedPosePublisher_->publish(poseArray); } } // namespace mobile_manipulator diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp index 64ebb951f..26a67e4e7 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp @@ -27,14 +27,14 @@ 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 #include -#include +#include + +#include "rclcpp/rclcpp.hpp" using namespace ocs2; using namespace mobile_manipulator; @@ -43,13 +43,21 @@ int main(int argc, char** argv) { const std::string robotName = "mobile_manipulator"; // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; - // Get node parameters - std::string taskFile, libFolder, urdfFile; - nodeHandle.getParam("/taskFile", taskFile); - nodeHandle.getParam("/libFolder", libFolder); - nodeHandle.getParam("/urdfFile", urdfFile); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_mpc"); + + const std::string taskFile = + node->declare_parameter("taskFile", ""); + const std::string libFolder = + node->declare_parameter("libFolder", ""); + const std::string urdfFile = + node->declare_parameter("urdfFile", ""); + if (taskFile.empty() || libFolder.empty() || urdfFile.empty()) { + throw std::runtime_error( + "[MobileManipulatorMpcNode] Parameters 'taskFile', 'libFolder', and " + "'urdfFile' are required."); + } std::cerr << "Loading task file: " << taskFile << std::endl; std::cerr << "Loading library folder: " << libFolder << std::endl; std::cerr << "Loading urdf file: " << urdfFile << std::endl; @@ -57,17 +65,19 @@ int main(int argc, char** argv) { MobileManipulatorInterface interface(taskFile, libFolder, urdfFile); // ROS ReferenceManager - auto rosReferenceManagerPtr = std::make_shared(robotName, interface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); + auto rosReferenceManagerPtr = std::make_shared( + robotName, interface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(node); // MPC - ocs2::GaussNewtonDDP_MPC mpc(interface.mpcSettings(), interface.ddpSettings(), interface.getRollout(), - interface.getOptimalControlProblem(), interface.getInitializer()); + ocs2::GaussNewtonDDP_MPC mpc( + interface.mpcSettings(), interface.ddpSettings(), interface.getRollout(), + interface.getOptimalControlProblem(), interface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // Launch MPC ROS node MPC_ROS_Interface mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); // Successful exit return 0; diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorTarget.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorTarget.cpp index 371d16958..6d622d5fa 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorTarget.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorTarget.cpp @@ -34,25 +34,31 @@ using namespace ocs2; /** * Converts the pose of the interactive marker to TargetTrajectories. */ -TargetTrajectories goalPoseToTargetTrajectories(const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation, - const SystemObservation& observation) { +TargetTrajectories goalPoseToTargetTrajectories( + const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation, + const SystemObservation& observation) { // time trajectory const scalar_array_t timeTrajectory{observation.time}; - // state trajectory: 3 + 4 for desired position vector and orientation quaternion - const vector_t target = (vector_t(7) << position, orientation.coeffs()).finished(); + // state trajectory: 3 + 4 for desired position vector and orientation + // quaternion + const vector_t target = + (vector_t(7) << position, orientation.coeffs()).finished(); const vector_array_t stateTrajectory{target}; // input trajectory - const vector_array_t inputTrajectory{vector_t::Zero(observation.input.size())}; + const vector_array_t inputTrajectory{ + vector_t::Zero(observation.input.size())}; return {timeTrajectory, stateTrajectory, inputTrajectory}; } int main(int argc, char* argv[]) { const std::string robotName = "mobile_manipulator"; - ::ros::init(argc, argv, robotName + "_target"); - ::ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_target"); - TargetTrajectoriesInteractiveMarker targetPoseCommand(nodeHandle, robotName, &goalPoseToTargetTrajectories); + TargetTrajectoriesInteractiveMarker targetPoseCommand( + node, robotName, &goalPoseToTargetTrajectories); targetPoseCommand.publishInteractiveMarker(); // Successful exit diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal/CMakeLists.txt index a6ffc9f67..46241b65f 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal/CMakeLists.txt @@ -1,4 +1,5 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) +set(CMAKE_CXX_STANDARD 17) project(ocs2_anymal) -find_package(catkin REQUIRED) -catkin_metapackage() +find_package(ament_cmake REQUIRED) +ament_package() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal/package.xml b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal/package.xml index 49817d0fd..3c4528e26 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal/package.xml +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal/package.xml @@ -10,13 +10,12 @@ TODO - catkin + ament_cmake ocs2_anymal_mpc ocs2_anymal_loopshaping_mpc - - + + ament_cmake - diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/CMakeLists.txt index 2a627d59c..c0ea356a5 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/CMakeLists.txt @@ -1,40 +1,35 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) +set(CMAKE_CXX_STANDARD 17) project(ocs2_anymal_commands) -find_package(catkin REQUIRED COMPONENTS - roslib +set(dependencies + rclcpp ocs2_ros_interfaces ocs2_robotic_tools ocs2_switched_model_interface grid_map_filters_rsl + Boost ) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_switched_model_interface REQUIRED) +find_package(grid_map_filters_rsl REQUIRED) + ## Eigen3 find_package(Eigen3 3.3 REQUIRED NO_MODULE) find_package(Boost REQUIRED COMPONENTS + system filesystem + log_setup + log ) -# 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_ros_interfaces - ocs2_robotic_tools - ocs2_switched_model_interface - DEPENDS - Boost -) + ########### ## Build ## @@ -43,7 +38,6 @@ catkin_package( include_directories( include ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} ) # Declare a C++ library @@ -57,85 +51,90 @@ add_library(${PROJECT_NAME} src/ReferenceExtrapolation.cpp src/TerrainAdaptation.cpp ) -add_dependencies(${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) # Declare a C++ Executable add_executable(target_command_node src/AnymalPoseCommandNode.cpp - ) -add_dependencies(target_command_node - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ) -target_link_libraries(target_command_node +) +ament_target_dependencies(target_command_node + ${dependencies} +) +target_link_libraries(target_command_node ${PROJECT_NAME} - ${catkin_LIBRARIES} - ) +) target_compile_options(target_command_node PRIVATE ${OCS2_CXX_FLAGS}) add_executable(gait_command_node src/AnymalGaitNode.cpp ) -add_dependencies(gait_command_node - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} +ament_target_dependencies(gait_command_node + ${dependencies} ) -target_link_libraries(gait_command_node +target_link_libraries(gait_command_node ${PROJECT_NAME} - ${catkin_LIBRARIES} ) target_compile_options(gait_command_node PRIVATE ${OCS2_CXX_FLAGS}) add_executable(motion_command_node src/AnymalMotionCommandNode.cpp ) -add_dependencies(motion_command_node - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} +ament_target_dependencies(motion_command_node + ${dependencies} ) -target_link_libraries(motion_command_node +target_link_libraries(motion_command_node ${PROJECT_NAME} - ${catkin_LIBRARIES} - ) +) target_compile_options(motion_command_node PRIVATE ${OCS2_CXX_FLAGS}) ############# ## Install ## ############# -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include/${PROJECT_NAME} ) -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) -install(TARGETS target_command_node gait_command_node motion_command_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) + +install(TARGETS + target_command_node + gait_command_node + motion_command_node + DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME}/ ) ############# ## Testing ## ############# +find_package(ament_lint_auto REQUIRED) +ament_lint_auto_find_test_dependencies() +find_package(ament_cmake_gtest) -catkin_add_gtest(test_${PROJECT_NAME} +ament_add_gtest(test_${PROJECT_NAME} test/testLoadMotions.cpp test/testReferenceExtrapolation.cpp test/testTerrainAdaptation.cpp ) +ament_target_dependencies(test_${PROJECT_NAME} + ${dependencies} +) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} -lstdc++fs - gtest_main ) + +ament_export_dependencies(${dependencies}) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/ModeSequenceKeyboard.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/ModeSequenceKeyboard.h index 56d6e9765..315968d45 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/ModeSequenceKeyboard.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/ModeSequenceKeyboard.h @@ -1,5 +1,5 @@ /* - * ModeSequence_Keyboard_Quadruped.h + * modeSequence_Keyboard_Quadruped.h * * Created on: Oct 11, 2018 * Author: farbod @@ -7,22 +7,25 @@ #pragma once +#include +#include + #include #include -#include - -#include -#include +#include "rclcpp/rclcpp.hpp" namespace switched_model { -/** This class implements ModeSequence communication using ROS. */ +/** This class implements modeSequence communication using ROS. */ class ModeSequenceKeyboard { public: - ModeSequenceKeyboard(ros::NodeHandle nodeHandle, const std::string& gaitFile, const std::string& robotName, bool verbose = false); + ModeSequenceKeyboard(const rclcpp::Node::SharedPtr& node, + 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. */ + /** Prints the command line interface and responds to user input. Function + * returns after one user input. */ void getKeyboardCommand(); private: @@ -35,7 +38,8 @@ class ModeSequenceKeyboard { std::vector gaitList_; std::map gaitMap_; - ros::Publisher modeSequenceTemplatePublisher_; + rclcpp::Publisher::SharedPtr + modeSequenceTemplatePublisher_; }; } // end of namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandController.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandController.h index c6e25b26d..f9a1b5be2 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandController.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandController.h @@ -4,26 +4,32 @@ #pragma once -#include "ocs2_anymal_commands/MotionCommandInterface.h" - -#include - #include -#include #include +#include +#include +#include + +#include "ocs2_anymal_commands/MotionCommandInterface.h" + namespace switched_model { class MotionCommandController : public MotionCommandInterface { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - MotionCommandController(ros::NodeHandle& nodeHandle, const std::string& configFile, const std::string& controllerName); + MotionCommandController(const rclcpp::Node::SharedPtr& node, + const std::string& configFile, + const std::string& controllerName); ~MotionCommandController() override = default; - void publishMotion(const std::pair& motion) override; + void publishMotion( + const std::pair& motion) override; private: - ros::ServiceClient client; + rclcpp::Node::SharedPtr node_; + rclcpp::Client::SharedPtr + client; }; } // namespace switched_model \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandDummy.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandDummy.h index e8310b3f6..b9dcc7685 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandDummy.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandDummy.h @@ -4,37 +4,50 @@ #pragma once -#include "ocs2_anymal_commands/MotionCommandInterface.h" - -#include - #include -#include #include +#include +#include +#include +#include + +#include "ocs2_anymal_commands/MotionCommandInterface.h" +#include "rclcpp/rclcpp.hpp" + namespace switched_model { class MotionCommandDummy : public MotionCommandInterface { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - MotionCommandDummy(ros::NodeHandle& nodeHandle, const std::string& configFile, const std::string& robotName); + MotionCommandDummy(const rclcpp::Node::SharedPtr& node, + const std::string& configFile, + const std::string& robotName); ~MotionCommandDummy() override = default; - void publishMotion(const std::pair& motion) override; + void publishMotion( + const std::pair& motion) override; private: - void observationCallback(const ocs2_msgs::mpc_observation::ConstPtr& msg); + void observationCallback( + const ocs2_msgs::msg::MpcObservation::ConstSharedPtr& msg); - void terrainCallback(const visualization_msgs::Marker::ConstPtr& msg); + void terrainCallback( + const visualization_msgs::msg::Marker::ConstSharedPtr& msg); - ros::Publisher targetTrajectoryPublisher_; - ros::Publisher gaitSequencePublisher_; + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr + targetTrajectoryPublisher_; + rclcpp::Publisher:: + SharedPtr gaitSequencePublisher_; - ros::Subscriber observationSubscriber_; + rclcpp::Subscription::SharedPtr + observationSubscriber_; std::mutex observationMutex_; std::unique_ptr observationPtr_; - ros::Subscriber terrainSubscriber_; + rclcpp::Subscription::SharedPtr + terrainSubscriber_; std::mutex terrainMutex_; TerrainPlane localTerrain_; }; diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandInterface.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandInterface.h index 1fe9b8f67..ece888dcc 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandInterface.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/MotionCommandInterface.h @@ -7,7 +7,7 @@ #include #include -#include +#include "rclcpp/rclcpp.hpp" #include #include diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/PoseCommandToCostDesiredRos.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/PoseCommandToCostDesiredRos.h index f97c9fbdb..7dc57de28 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/PoseCommandToCostDesiredRos.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/include/ocs2_anymal_commands/PoseCommandToCostDesiredRos.h @@ -6,8 +6,8 @@ #include -#include -#include +#include "rclcpp/rclcpp.hpp" +#include #include #include @@ -15,27 +15,29 @@ #include #include -namespace switched_model { +namespace switched_model +{ -class PoseCommandToCostDesiredRos { - public: - scalar_t targetDisplacementVelocity; - scalar_t targetRotationVelocity; - scalar_t comHeight; - joint_coordinate_t defaultJointState; + class PoseCommandToCostDesiredRos + { + public: + scalar_t targetDisplacementVelocity; + scalar_t targetRotationVelocity; + scalar_t comHeight; + joint_coordinate_t defaultJointState; - PoseCommandToCostDesiredRos(::ros::NodeHandle& nodeHandle, const std::string& configFile); + PoseCommandToCostDesiredRos(const rclcpp::Node::SharedPtr &node, const std::string &configFile); - ocs2::TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTarget, const ocs2::SystemObservation& observation) const; + ocs2::TargetTrajectories commandLineToTargetTrajectories(const vector_t &commadLineTarget, const ocs2::SystemObservation &observation) const; - private: - scalar_t desiredTimeToTarget(scalar_t dyaw, scalar_t dx, scalar_t dy) const; + private: + scalar_t desiredTimeToTarget(scalar_t dyaw, scalar_t dx, scalar_t dy) const; - void terrainCallback(const visualization_msgs::Marker::ConstPtr& msg); + void terrainCallback(const visualization_msgs::msg::Marker::ConstSharedPtr &msg); - TerrainPlane localTerrain_; - mutable std::mutex terrainMutex_; - ros::Subscriber terrainSubscriber_; -}; + TerrainPlane localTerrain_; + mutable std::mutex terrainMutex_; + rclcpp::Subscription::SharedPtr terrainSubscriber_; + }; } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/package.xml b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/package.xml index 861a2a4db..4d125d2c0 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/package.xml +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/package.xml @@ -10,12 +10,15 @@ TODO - catkin + ament_cmake - roslib + rclcpp ocs2_ros_interfaces ocs2_robotic_tools ocs2_switched_model_interface grid_map_filters_rsl + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalGaitNode.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalGaitNode.cpp index 53edffb85..bbad18548 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalGaitNode.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalGaitNode.cpp @@ -1,25 +1,30 @@ /* - * AnymalModeSequenceCommand.cpp + * AnymalmodeSequenceCommand.cpp * * Created on: Oct 11, 2018 * Author: farbod */ -#include +#include #include "ocs2_anymal_commands/ModeSequenceKeyboard.h" +#include "rclcpp/rclcpp.hpp" int main(int argc, char* argv[]) { const std::string robotName = "anymal"; - std::string gaitFile = ros::package::getPath("ocs2_anymal_commands") + "/config/gait.info"; + std::string gaitFile = + ament_index_cpp::get_package_share_directory("ocs2_anymal_commands") + + "/config/gait.info"; std::cerr << "Loading gait file: " << gaitFile << std::endl; - ros::init(argc, argv, robotName + "_mpc_mode_sequence"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_mpc_modeSequence"); - switched_model::ModeSequenceKeyboard modeSequenceCommand(nodeHandle, gaitFile, robotName, true); + switched_model::ModeSequenceKeyboard modeSequenceCommand(node, gaitFile, + robotName, true); - while (ros::ok() && ros::master::check()) { + while (rclcpp::ok()) { modeSequenceCommand.getKeyboardCommand(); } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalMotionCommandNode.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalMotionCommandNode.cpp index 102d6f6a7..2c0e06920 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalMotionCommandNode.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalMotionCommandNode.cpp @@ -1,35 +1,42 @@ -#include - +#include "rclcpp/rclcpp.hpp" +#include #include "ocs2_anymal_commands/MotionCommandController.h" #include "ocs2_anymal_commands/MotionCommandDummy.h" -int main(int argc, char* argv[]) { +int main(int argc, char *argv[]) +{ const std::string robotName = "anymal"; - std::string motionFile = ros::package::getPath("ocs2_anymal_commands") + "/config/motions.info"; + std::string motionFile = ament_index_cpp::get_package_share_directory("ocs2_anymal_commands") + "/config/motions.info"; std::cerr << "Loading motion file: " << motionFile << std::endl; - const std::string controllerName = [&] { - std::vector programArgs{}; - ros::removeROSArgs(argc, argv, programArgs); - if (programArgs.size() <= 1) { + const std::string controllerName = [&] + { + std::vector programArgs = rclcpp::remove_ros_arguments(argc, argv); + + if (programArgs.size() <= 1) + { throw std::runtime_error("No operation mode specified. Aborting."); } return programArgs[1]; }(); - ros::init(argc, argv, robotName + "_mpc_motion_command"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mpc_motion_command"); std::unique_ptr motionCommandInterface; - if (controllerName == "dummy") { - motionCommandInterface.reset(new switched_model::MotionCommandDummy(nodeHandle, motionFile, robotName)); - } else { - motionCommandInterface.reset(new switched_model::MotionCommandController(nodeHandle, motionFile, controllerName)); + if (controllerName == "dummy") + { + motionCommandInterface.reset(new switched_model::MotionCommandDummy(node, motionFile, robotName)); + } + else + { + motionCommandInterface.reset(new switched_model::MotionCommandController(node, motionFile, controllerName)); } - ros::Rate rate(10); - while (ros::ok() && ros::master::check()) { + rclcpp::Rate rate(10); + while (rclcpp::ok()) + { motionCommandInterface->getKeyboardCommand(); rate.sleep(); } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalPoseCommandNode.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalPoseCommandNode.cpp index 21681a770..7f9323185 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalPoseCommandNode.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/AnymalPoseCommandNode.cpp @@ -1,17 +1,18 @@ -#include - #include #include #include +#include + #include "ocs2_anymal_commands/PoseCommandToCostDesiredRos.h" int main(int argc, char* argv[]) { const std::string robotName = "anymal"; const std::string filename = [&] { - std::vector programArgs{}; - ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } @@ -19,21 +20,28 @@ int main(int argc, char* argv[]) { }(); // ros node handle - ::ros::init(argc, argv, robotName + "_mpc_pose_command_node"); - ::ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_mpc_pose_command_node"); // PoseCommand To TargetTrajectories - switched_model::PoseCommandToCostDesiredRos targetPoseCommand(nodeHandle, filename); - auto commandLineToTargetTrajectoriesFun = [&](const ocs2::vector_t& commandLineTarget, const ocs2::SystemObservation& observation) { - return targetPoseCommand.commandLineToTargetTrajectories(commandLineTarget, observation); - }; + switched_model::PoseCommandToCostDesiredRos targetPoseCommand(node, filename); + auto commandLineToTargetTrajectoriesFun = + [&](const ocs2::vector_t& commandLineTarget, + const ocs2::SystemObservation& observation) { + return targetPoseCommand.commandLineToTargetTrajectories( + commandLineTarget, observation); + }; // goalPose: [deltaX, deltaY, deltaZ, Roll, Pitch, deltaYaw] - const ocs2::scalar_array_t relativeBaseLimit{10.0, 10.0, 0.2, 45.0, 45.0, 360.0}; + const ocs2::scalar_array_t relativeBaseLimit{10.0, 10.0, 0.2, + 45.0, 45.0, 360.0}; ocs2::TargetTrajectoriesKeyboardPublisher targetTrajectoriesKeyboardPublisher( - nodeHandle, robotName, relativeBaseLimit, commandLineToTargetTrajectoriesFun); + node, robotName, relativeBaseLimit, commandLineToTargetTrajectoriesFun); - const std::string commandMsg = "Enter XYZ displacement and RollPitchYaw for the robot, separated by spaces"; + const std::string commandMsg = + "Enter XYZ displacement and RollPitchYaw for the robot, separated by " + "spaces"; targetTrajectoriesKeyboardPublisher.publishKeyboardCommand(commandMsg); // Successful exit diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/LoadMotions.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/LoadMotions.cpp index f60b62168..fdb71391c 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/LoadMotions.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/LoadMotions.cpp @@ -31,7 +31,8 @@ CsvData readCsv(const std::string& fileName) { // Extract each column name while (std::getline(ss, colname, ',')) { // Remove additional endline marker generated in python or windows. - colname.erase(std::remove(colname.begin(), colname.end(), '\r'), colname.end()); + colname.erase(std::remove(colname.begin(), colname.end(), '\r'), + colname.end()); result.header.push_back(colname); ss >> std::ws; // remove white space @@ -64,7 +65,8 @@ CsvData readCsv(const std::string& fileName) { return result; } -std::pair readMotion(const CsvData& csvData, scalar_t dt) { +std::pair readMotion(const CsvData& csvData, + scalar_t dt) { verifyHeader(csvData.header); const auto numDataPoints = csvData.data.size(); @@ -72,7 +74,8 @@ std::pair readMotion(const CsvData& csvData, sca const scalar_t duration = csvData.data.back()[0] - t0; const auto getMode = [](const vector_t dataLine) -> size_t { - const contact_flag_t contactFlags{dataLine[1] > 0.5, dataLine[2] > 0.5, dataLine[3] > 0.5, dataLine[4] > 0.5}; + const contact_flag_t contactFlags{dataLine[1] > 0.5, dataLine[2] > 0.5, + dataLine[3] > 0.5, dataLine[4] > 0.5}; return stanceLeg2ModeNumber(contactFlags); }; @@ -94,7 +97,8 @@ std::pair readMotion(const CsvData& csvData, sca gait.modeSequence.push_back(mode); } else { // Drop a point if dt is smaller than desired - if (!targetTrajectories.empty() && t < targetTrajectories.timeTrajectory.back() + dt) { + if (!targetTrajectories.empty() && + t < targetTrajectories.timeTrajectory.back() + dt) { continue; } } @@ -107,23 +111,31 @@ std::pair readMotion(const CsvData& csvData, sca const vector3_t basePositionInWorld = dataLine.segment(colId, 3); colId += 3; const Eigen::Quaterniond quaternion = - Eigen::Quaterniond(dataLine[colId], dataLine[colId + 1], dataLine[colId + 2], dataLine[colId + 3]).normalized(); + Eigen::Quaterniond(dataLine[colId], dataLine[colId + 1], + dataLine[colId + 2], dataLine[colId + 3]) + .normalized(); colId += 4; const vector3_t baseLinearvelocityInBase = dataLine.segment(colId, 3); colId += 3; const vector3_t baseAngularvelocityInBase = dataLine.segment(colId, 3); colId += 3; - const joint_coordinate_t jointPositions = dataLine.segment(colId, JOINT_COORDINATE_SIZE); + const joint_coordinate_t jointPositions = + dataLine.segment(colId, JOINT_COORDINATE_SIZE); colId += JOINT_COORDINATE_SIZE; - const joint_coordinate_t jointVelocities = dataLine.segment(colId, JOINT_COORDINATE_SIZE); + const joint_coordinate_t jointVelocities = + dataLine.segment(colId, JOINT_COORDINATE_SIZE); colId += JOINT_COORDINATE_SIZE; - const vector_t contactForces = dataLine.segment(colId, 3 * NUM_CONTACT_POINTS); + const vector_t contactForces = + dataLine.segment(colId, 3 * NUM_CONTACT_POINTS); // Convert orientation to ocs2 convention auto newOrientation = eulerAnglesFromQuaternionBaseToOrigin(quaternion); ocs2::makeEulerAnglesUnique(newOrientation); - const auto prevYaw = targetTrajectories.stateTrajectory.empty() ? 0.0 : targetTrajectories.stateTrajectory.back()[2]; - const auto newYaw = ocs2::moduloAngleWithReference(newOrientation[2], prevYaw); + const auto prevYaw = targetTrajectories.stateTrajectory.empty() + ? 0.0 + : targetTrajectories.stateTrajectory.back()[2]; + const auto newYaw = + ocs2::moduloAngleWithReference(newOrientation[2], prevYaw); const vector3_t eulerXYZ(newOrientation[0], newOrientation[1], newYaw); const matrix3_t o_R_b = rotationMatrixBaseToOrigin(eulerXYZ); @@ -131,80 +143,84 @@ std::pair readMotion(const CsvData& csvData, sca vector_t contactForcesInBase(3 * NUM_CONTACT_POINTS); for (size_t i = 0; i < NUM_CONTACT_POINTS; i++) { - contactForcesInBase.segment<3>(3 * i) = o_R_b.transpose() * contactForces.segment<3>(3 * i); + contactForcesInBase.segment<3>(3 * i) = + o_R_b.transpose() * contactForces.segment<3>(3 * i); } // State trajectory targetTrajectories.stateTrajectory.push_back(vector_t(STATE_DIM)); - targetTrajectories.stateTrajectory.back() << eulerXYZ, basePositionInWorld, baseAngularvelocityInBase, baseLinearvelocityInBase, - jointPositions; + targetTrajectories.stateTrajectory.back() << eulerXYZ, basePositionInWorld, + baseAngularvelocityInBase, baseLinearvelocityInBase, jointPositions; // Input trajectory targetTrajectories.inputTrajectory.push_back(vector_t(INPUT_DIM)); - targetTrajectories.inputTrajectory.back() << contactForcesInBase, jointVelocities; + targetTrajectories.inputTrajectory.back() << contactForcesInBase, + jointVelocities; } return {targetTrajectories, gait}; } void verifyHeader(const std::vector& provided) { - const std::vector expectedHeader = {"time", - "contactflag_LF", - "contactflag_RF", - "contactflag_LH", - "contactflag_RH", - "base_positionInWorld_x", - "base_positionInWorld_y", - "base_positionInWorld_z", - "base_quaternion_w", - "base_quaternion_x", - "base_quaternion_y", - "base_quaternion_z", - "base_linearvelocityInBase_x", - "base_linearvelocityInBase_y", - "base_linearvelocityInBase_z", - "base_angularvelocityInBase_x", - "base_angularvelocityInBase_y", - "base_angularvelocityInBase_z", - "jointAngle_LF_HAA", - "jointAngle_LF_HFE", - "jointAngle_LF_KFE", - "jointAngle_RF_HAA", - "jointAngle_RF_HFE", - "jointAngle_RF_KFE", - "jointAngle_LH_HAA", - "jointAngle_LH_HFE", - "jointAngle_LH_KFE", - "jointAngle_RH_HAA", - "jointAngle_RH_HFE", - "jointAngle_RH_KFE", - "jointVelocity_LF_HAA", - "jointVelocity_LF_HFE", - "jointVelocity_LF_KFE", - "jointVelocity_RF_HAA", - "jointVelocity_RF_HFE", - "jointVelocity_RF_KFE", - "jointVelocity_LH_HAA", - "jointVelocity_LH_HFE", - "jointVelocity_LH_KFE", - "jointVelocity_RH_HAA", - "jointVelocity_RH_HFE", - "jointVelocity_RH_KFE", - "contactForcesInWorld_LF_x", - "contactForcesInWorld_LF_y", - "contactForcesInWorld_LF_z", - "contactForcesInWorld_RF_x", - "contactForcesInWorld_RF_y", - "contactForcesInWorld_RF_z", - "contactForcesInWorld_LH_x", - "contactForcesInWorld_LH_y", - "contactForcesInWorld_LH_z", - "contactForcesInWorld_RH_x", - "contactForcesInWorld_RH_y", - "contactForcesInWorld_RH_z"}; + const std::vector expectedHeader = { + "time", + "contactflag_LF", + "contactflag_RF", + "contactflag_LH", + "contactflag_RH", + "base_positionInWorld_x", + "base_positionInWorld_y", + "base_positionInWorld_z", + "base_quaternion_w", + "base_quaternion_x", + "base_quaternion_y", + "base_quaternion_z", + "base_linearvelocityInBase_x", + "base_linearvelocityInBase_y", + "base_linearvelocityInBase_z", + "base_angularvelocityInBase_x", + "base_angularvelocityInBase_y", + "base_angularvelocityInBase_z", + "jointAngle_LF_HAA", + "jointAngle_LF_HFE", + "jointAngle_LF_KFE", + "jointAngle_RF_HAA", + "jointAngle_RF_HFE", + "jointAngle_RF_KFE", + "jointAngle_LH_HAA", + "jointAngle_LH_HFE", + "jointAngle_LH_KFE", + "jointAngle_RH_HAA", + "jointAngle_RH_HFE", + "jointAngle_RH_KFE", + "jointVelocity_LF_HAA", + "jointVelocity_LF_HFE", + "jointVelocity_LF_KFE", + "jointVelocity_RF_HAA", + "jointVelocity_RF_HFE", + "jointVelocity_RF_KFE", + "jointVelocity_LH_HAA", + "jointVelocity_LH_HFE", + "jointVelocity_LH_KFE", + "jointVelocity_RH_HAA", + "jointVelocity_RH_HFE", + "jointVelocity_RH_KFE", + "contactForcesInWorld_LF_x", + "contactForcesInWorld_LF_y", + "contactForcesInWorld_LF_z", + "contactForcesInWorld_RF_x", + "contactForcesInWorld_RF_y", + "contactForcesInWorld_RF_z", + "contactForcesInWorld_LH_x", + "contactForcesInWorld_LH_y", + "contactForcesInWorld_LH_z", + "contactForcesInWorld_RH_x", + "contactForcesInWorld_RH_y", + "contactForcesInWorld_RH_z"}; verifyHeaderImpl(expectedHeader, provided); } -std::pair readCartesianMotion(const CsvData& csvData, scalar_t dt) { +std::pair readCartesianMotion( + const CsvData& csvData, scalar_t dt) { verifyCartesianHeader(csvData.header); const auto numDataPoints = csvData.data.size(); @@ -212,7 +228,8 @@ std::pair readCartesianMotion(const CsvData& csv const scalar_t duration = csvData.data.back()[0] - t0; const auto getMode = [](const vector_t dataLine) -> size_t { - const contact_flag_t contactFlags{dataLine[1] > 0.5, dataLine[2] > 0.5, dataLine[3] > 0.5, dataLine[4] > 0.5}; + const contact_flag_t contactFlags{dataLine[1] > 0.5, dataLine[2] > 0.5, + dataLine[3] > 0.5, dataLine[4] > 0.5}; return stanceLeg2ModeNumber(contactFlags); }; @@ -234,7 +251,8 @@ std::pair readCartesianMotion(const CsvData& csv gait.modeSequence.push_back(mode); } else { // Drop a point if dt is smaller than desired - if (!targetTrajectories.empty() && t < targetTrajectories.timeTrajectory.back() + dt) { + if (!targetTrajectories.empty() && + t < targetTrajectories.timeTrajectory.back() + dt) { continue; } } @@ -247,7 +265,9 @@ std::pair readCartesianMotion(const CsvData& csv const vector3_t basePositionInWorld = dataLine.segment(colId, 3); colId += 3; const Eigen::Quaterniond quaternion = - Eigen::Quaterniond(dataLine[colId], dataLine[colId + 1], dataLine[colId + 2], dataLine[colId + 3]).normalized(); + Eigen::Quaterniond(dataLine[colId], dataLine[colId + 1], + dataLine[colId + 2], dataLine[colId + 3]) + .normalized(); colId += 4; const vector3_t baseLinearvelocityInBase = dataLine.segment(colId, 3); colId += 3; @@ -257,13 +277,17 @@ std::pair readCartesianMotion(const CsvData& csv colId += 3 * NUM_CONTACT_POINTS; const auto feetVelocities = dataLine.segment(colId, 3 * NUM_CONTACT_POINTS); colId += 3 * NUM_CONTACT_POINTS; - const vector_t contactForces = dataLine.segment(colId, 3 * NUM_CONTACT_POINTS); + const vector_t contactForces = + dataLine.segment(colId, 3 * NUM_CONTACT_POINTS); // Convert orientation to ocs2 convention auto newOrientation = eulerAnglesFromQuaternionBaseToOrigin(quaternion); ocs2::makeEulerAnglesUnique(newOrientation); - const auto prevYaw = targetTrajectories.stateTrajectory.empty() ? 0.0 : targetTrajectories.stateTrajectory.back()[2]; - const auto newYaw = ocs2::moduloAngleWithReference(newOrientation[2], prevYaw); + const auto prevYaw = targetTrajectories.stateTrajectory.empty() + ? 0.0 + : targetTrajectories.stateTrajectory.back()[2]; + const auto newYaw = + ocs2::moduloAngleWithReference(newOrientation[2], prevYaw); const vector3_t eulerXYZ(newOrientation[0], newOrientation[1], newYaw); const matrix3_t o_R_b = rotationMatrixBaseToOrigin(eulerXYZ); @@ -271,89 +295,97 @@ std::pair readCartesianMotion(const CsvData& csv vector_t contactForcesInBase(3 * NUM_CONTACT_POINTS); for (size_t i = 0; i < NUM_CONTACT_POINTS; i++) { - contactForcesInBase.segment<3>(3 * i) = o_R_b.transpose() * contactForces.segment<3>(3 * i); + contactForcesInBase.segment<3>(3 * i) = + o_R_b.transpose() * contactForces.segment<3>(3 * i); } // State trajectory - targetTrajectories.stateTrajectory.push_back(vector_t(2 * BASE_COORDINATE_SIZE + 6 * NUM_CONTACT_POINTS)); - targetTrajectories.stateTrajectory.back() << eulerXYZ, basePositionInWorld, baseAngularvelocityInBase, baseLinearvelocityInBase, - feetPositions, feetVelocities; + targetTrajectories.stateTrajectory.push_back( + vector_t(2 * BASE_COORDINATE_SIZE + 6 * NUM_CONTACT_POINTS)); + targetTrajectories.stateTrajectory.back() << eulerXYZ, basePositionInWorld, + baseAngularvelocityInBase, baseLinearvelocityInBase, feetPositions, + feetVelocities; // Input trajectory - targetTrajectories.inputTrajectory.push_back(vector_t(3 * NUM_CONTACT_POINTS)); + targetTrajectories.inputTrajectory.push_back( + vector_t(3 * NUM_CONTACT_POINTS)); targetTrajectories.inputTrajectory.back() << contactForcesInBase; } return {targetTrajectories, gait}; } void verifyCartesianHeader(const std::vector& provided) { - const std::vector expectedHeader = {"time", - "contactflag_LF", - "contactflag_RF", - "contactflag_LH", - "contactflag_RH", - "base_positionInWorld_x", - "base_positionInWorld_y", - "base_positionInWorld_z", - "base_quaternion_w", - "base_quaternion_x", - "base_quaternion_y", - "base_quaternion_z", - "base_linearvelocityInBase_x", - "base_linearvelocityInBase_y", - "base_linearvelocityInBase_z", - "base_angularvelocityInBase_x", - "base_angularvelocityInBase_y", - "base_angularvelocityInBase_z", - "footPositionInWorld_LF_x", - "footPositionInWorld_LF_y", - "footPositionInWorld_LF_z", - "footPositionInWorld_RF_x", - "footPositionInWorld_RF_y", - "footPositionInWorld_RF_z", - "footPositionInWorld_LH_x", - "footPositionInWorld_LH_y", - "footPositionInWorld_LH_z", - "footPositionInWorld_RH_x", - "footPositionInWorld_RH_y", - "footPositionInWorld_RH_z", - "footVelocityInWorld_LF_x", - "footVelocityInWorld_LF_y", - "footVelocityInWorld_LF_z", - "footVelocityInWorld_RF_x", - "footVelocityInWorld_RF_y", - "footVelocityInWorld_RF_z", - "footVelocityInWorld_LH_x", - "footVelocityInWorld_LH_y", - "footVelocityInWorld_LH_z", - "footVelocityInWorld_RH_x", - "footVelocityInWorld_RH_y", - "footVelocityInWorld_RH_z", - "contactForcesInWorld_LF_x", - "contactForcesInWorld_LF_y", - "contactForcesInWorld_LF_z", - "contactForcesInWorld_RF_x", - "contactForcesInWorld_RF_y", - "contactForcesInWorld_RF_z", - "contactForcesInWorld_LH_x", - "contactForcesInWorld_LH_y", - "contactForcesInWorld_LH_z", - "contactForcesInWorld_RH_x", - "contactForcesInWorld_RH_y", - "contactForcesInWorld_RH_z"}; + const std::vector expectedHeader = { + "time", + "contactflag_LF", + "contactflag_RF", + "contactflag_LH", + "contactflag_RH", + "base_positionInWorld_x", + "base_positionInWorld_y", + "base_positionInWorld_z", + "base_quaternion_w", + "base_quaternion_x", + "base_quaternion_y", + "base_quaternion_z", + "base_linearvelocityInBase_x", + "base_linearvelocityInBase_y", + "base_linearvelocityInBase_z", + "base_angularvelocityInBase_x", + "base_angularvelocityInBase_y", + "base_angularvelocityInBase_z", + "footPositionInWorld_LF_x", + "footPositionInWorld_LF_y", + "footPositionInWorld_LF_z", + "footPositionInWorld_RF_x", + "footPositionInWorld_RF_y", + "footPositionInWorld_RF_z", + "footPositionInWorld_LH_x", + "footPositionInWorld_LH_y", + "footPositionInWorld_LH_z", + "footPositionInWorld_RH_x", + "footPositionInWorld_RH_y", + "footPositionInWorld_RH_z", + "footVelocityInWorld_LF_x", + "footVelocityInWorld_LF_y", + "footVelocityInWorld_LF_z", + "footVelocityInWorld_RF_x", + "footVelocityInWorld_RF_y", + "footVelocityInWorld_RF_z", + "footVelocityInWorld_LH_x", + "footVelocityInWorld_LH_y", + "footVelocityInWorld_LH_z", + "footVelocityInWorld_RH_x", + "footVelocityInWorld_RH_y", + "footVelocityInWorld_RH_z", + "contactForcesInWorld_LF_x", + "contactForcesInWorld_LF_y", + "contactForcesInWorld_LF_z", + "contactForcesInWorld_RF_x", + "contactForcesInWorld_RF_y", + "contactForcesInWorld_RF_z", + "contactForcesInWorld_LH_x", + "contactForcesInWorld_LH_y", + "contactForcesInWorld_LH_z", + "contactForcesInWorld_RH_x", + "contactForcesInWorld_RH_y", + "contactForcesInWorld_RH_z"}; verifyHeaderImpl(expectedHeader, provided); } -void verifyHeaderImpl(const std::vector& expected, const std::vector& provided) { +void verifyHeaderImpl(const std::vector& expected, + const std::vector& provided) { // Check header if (provided.size() != expected.size()) { - throw std::runtime_error("Incorrect amount of columns. Expected: " + std::to_string(expected.size()) + ", but got " + + throw std::runtime_error("Incorrect amount of columns. Expected: " + + std::to_string(expected.size()) + ", but got " + std::to_string(provided.size())); } for (size_t i = 0; i < expected.size(); ++i) { if (provided[i] != expected[i]) { - throw std::runtime_error("Incorrect header of column " + std::to_string(i) + ", expected: " + expected[i] + ", but got " + - provided[i]); + throw std::runtime_error( + "Incorrect header of column " + std::to_string(i) + + ", expected: " + expected[i] + ", but got " + provided[i]); } } } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/ModeSequenceKeyboard.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/ModeSequenceKeyboard.cpp index ba37e3e70..70b589b44 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/ModeSequenceKeyboard.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/ModeSequenceKeyboard.cpp @@ -7,29 +7,37 @@ #include #include -#include +#include namespace switched_model { -ModeSequenceKeyboard::ModeSequenceKeyboard(ros::NodeHandle nodeHandle, const std::string& gaitFile, const std::string& robotName, +ModeSequenceKeyboard::ModeSequenceKeyboard(const rclcpp::Node::SharedPtr& node, + const std::string& gaitFile, + const std::string& robotName, bool verbose) { - ROS_INFO_STREAM(robotName + "_mpc_mode_schedule node is setting up ..."); + RCLCPP_INFO_STREAM(node->get_logger(), + robotName + "_mpc_mode_schedule node is setting up ..."); ocs2::loadData::loadStdVector(gaitFile, "list", gaitList_, verbose); - modeSequenceTemplatePublisher_ = nodeHandle.advertise(robotName + "_mpc_mode_schedule", 1, true); + modeSequenceTemplatePublisher_ = + node->create_publisher( + robotName + "_mpc_mode_schedule", 1); gaitMap_.clear(); for (const auto& gaitName : gaitList_) { - gaitMap_.insert({gaitName, loadModeSequenceTemplate(gaitFile, gaitName, verbose)}); + gaitMap_.insert( + {gaitName, loadModeSequenceTemplate(gaitFile, gaitName, verbose)}); } - ROS_INFO_STREAM(robotName + "_mpc_mode_schedule command node is ready."); + RCLCPP_INFO_STREAM(node->get_logger(), + robotName + "_mpc_mode_schedule command node is ready."); } void ModeSequenceKeyboard::getKeyboardCommand() { - const std::string commadMsg = "Enter the desired gait, for the list of available gait enter \"list\""; + 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 gaitCommand = ocs2::getCommandLineString(shouldTerminate); if (gaitCommand.empty()) { @@ -42,15 +50,16 @@ void ModeSequenceKeyboard::getKeyboardCommand() { } try { - ModeSequenceTemplate modeSequenceTemplate = gaitMap_.at(gaitCommand); - publishModeSequenceTemplate(modeSequenceTemplate); + ModeSequenceTemplate ModeSequenceTemplate = gaitMap_.at(gaitCommand); + publishModeSequenceTemplate(ModeSequenceTemplate); } catch (const std::out_of_range& e) { std::cout << "Gait \"" << gaitCommand << "\" not found.\n"; printGaitList(gaitList_); } } -void ModeSequenceKeyboard::printGaitList(const std::vector& gaitList) const { +void ModeSequenceKeyboard::printGaitList( + const std::vector& gaitList) const { std::cout << "List of available gaits:\n"; size_t itr = 0; for (const auto& s : gaitList) { @@ -59,8 +68,10 @@ void ModeSequenceKeyboard::printGaitList(const std::vector& gaitLis std::cout << std::endl; } -void ModeSequenceKeyboard::publishModeSequenceTemplate(const ModeSequenceTemplate& modeSchedule) { - modeSequenceTemplatePublisher_.publish(createModeSequenceTemplateMsg(modeSchedule)); +void ModeSequenceKeyboard::publishModeSequenceTemplate( + const ModeSequenceTemplate& modeSchedule) { + modeSequenceTemplatePublisher_->publish( + createModeSequenceTemplateMsg(modeSchedule)); } } // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandController.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandController.cpp index 3eabc2b8e..e65fd6395 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandController.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandController.cpp @@ -6,26 +6,56 @@ #include #include -#include -#include + +#include + +#include namespace switched_model { -MotionCommandController::MotionCommandController(ros::NodeHandle& nodeHandle, const std::string& configFile, - const std::string& controllerName) +MotionCommandController::MotionCommandController( + const rclcpp::Node::SharedPtr& node, const std::string& configFile, + const std::string& controllerName) : MotionCommandInterface(configFile), - client(nodeHandle.serviceClient(controllerName + "/trajectory_request")) {} + node_(node), + client( + node->create_client( + controllerName + "/trajectory_request")) {} + +void MotionCommandController::publishMotion( + const std::pair& motion) { + if (!node_) { + throw std::runtime_error( + "[MotionCommandController::publishMotion] Node is not initialized."); + } + if (!client) { + throw std::runtime_error( + "[MotionCommandController::publishMotion] Service client is not initialized."); + } -void MotionCommandController::publishMotion(const std::pair& motion) { Gait stance; stance.duration = 1.0; stance.modeSequence = {15}; - ocs2_switched_model_msgs::trajectory_request srv; - srv.request.trajectory = ocs2::ros_msg_conversions::createTargetTrajectoriesMsg(motion.first); - srv.request.gaitSequence = switched_model::ros_msg_conversions::toMessage({motion.second, stance}); - - client.call(srv); + auto srv = std::make_shared< + ocs2_switched_model_msgs::srv::TrajectoryRequest::Request>(); + srv->trajectory = + ocs2::ros_msg_conversions::createTargetTrajectoriesMsg(motion.first); + srv->gait_sequence = + switched_model::ros_msg_conversions::toMessage({motion.second, stance}); + + if (!client->wait_for_service(std::chrono::seconds(5))) { + throw std::runtime_error( + "[MotionCommandController::publishMotion] TrajectoryRequest service not available."); + } + + auto future = client->async_send_request(srv); + const auto status = + rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + if (status != rclcpp::FutureReturnCode::SUCCESS) { + throw std::runtime_error( + "[MotionCommandController::publishMotion] TrajectoryRequest service call failed or timed out."); + } } -} // namespace switched_model \ No newline at end of file +} // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandDummy.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandDummy.cpp index 25f02c6b7..4a349f098 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandDummy.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandDummy.cpp @@ -4,69 +4,78 @@ #include "ocs2_anymal_commands/MotionCommandDummy.h" -#include - #include -#include +#include #include #include "ocs2_anymal_commands/TerrainAdaptation.h" -namespace switched_model { +namespace switched_model +{ -MotionCommandDummy::MotionCommandDummy(ros::NodeHandle& nodeHandle, const std::string& configFile, const std::string& robotName) - : MotionCommandInterface(configFile), localTerrain_() { - // Publishers - targetTrajectoryPublisher_ = nodeHandle.advertise(robotName + "_mpc_target", 1, false); - gaitSequencePublisher_ = - nodeHandle.advertise(robotName + "_mpc_gait_schedule", 1, true); + MotionCommandDummy::MotionCommandDummy(const rclcpp::Node::SharedPtr &node, const std::string &configFile, const std::string &robotName) + : MotionCommandInterface(configFile), node_(node), localTerrain_() + { + // Publishers + targetTrajectoryPublisher_ = node->create_publisher(robotName + "_mpc_target", 1); + gaitSequencePublisher_ = + node->create_publisher(robotName + "_mpc_gait_schedule", 1); - // Subsribers - observationSubscriber_ = nodeHandle.subscribe(robotName + "_mpc_observation", 1, &MotionCommandDummy::observationCallback, this); - terrainSubscriber_ = nodeHandle.subscribe("/ocs2_anymal/localTerrain", 1, &MotionCommandDummy::terrainCallback, this); -} + // Subsribers + observationSubscriber_ = node->create_subscription(robotName + "_mpc_observation", 1, + std::bind(&MotionCommandDummy::observationCallback, this, std::placeholders::_1)); + terrainSubscriber_ = node->create_subscription("/ocs2_anymal/localTerrain", 1, + std::bind(&MotionCommandDummy::terrainCallback, this, std::placeholders::_1)); + } -void MotionCommandDummy::observationCallback(const ocs2_msgs::mpc_observation::ConstPtr& msg) { - std::lock_guard lock(observationMutex_); - observationPtr_.reset(new ocs2::SystemObservation(ocs2::ros_msg_conversions::readObservationMsg(*msg))); -} + void MotionCommandDummy::observationCallback(const ocs2_msgs::msg::MpcObservation::ConstSharedPtr &msg) + { + std::lock_guard lock(observationMutex_); + observationPtr_.reset(new ocs2::SystemObservation(ocs2::ros_msg_conversions::readObservationMsg(*msg))); + } -void MotionCommandDummy::terrainCallback(const visualization_msgs::Marker::ConstPtr& msg) { - Eigen::Quaterniond orientationTerrainToWorld{msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, - msg->pose.orientation.z}; + void MotionCommandDummy::terrainCallback(const visualization_msgs::msg::Marker::ConstSharedPtr &msg) + { + Eigen::Quaterniond orientationTerrainToWorld{msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, + msg->pose.orientation.z}; - std::lock_guard lock(terrainMutex_); - localTerrain_.positionInWorld = {msg->pose.position.x, msg->pose.position.y, msg->pose.position.z}; - localTerrain_.orientationWorldToTerrain = orientationTerrainToWorld.toRotationMatrix().transpose(); -} + std::lock_guard lock(terrainMutex_); + localTerrain_.positionInWorld = {msg->pose.position.x, msg->pose.position.y, msg->pose.position.z}; + localTerrain_.orientationWorldToTerrain = orientationTerrainToWorld.toRotationMatrix().transpose(); + } -void MotionCommandDummy::publishMotion(const std::pair& motion) { - ros::spinOnce(); // Trigger callback - ocs2::SystemObservation observation; + void MotionCommandDummy::publishMotion(const std::pair &motion) { - std::lock_guard lock(observationMutex_); - if (observationPtr_) { - observation = *observationPtr_; - } else { - std::cout << "No observation is received from the MPC node. Make sure the MPC node is running!" - << "\n"; - return; + rclcpp::spin_some(node_); // Trigger callback + ocs2::SystemObservation observation; + { + std::lock_guard lock(observationMutex_); + if (observationPtr_) + { + observation = *observationPtr_; + } + else + { + std::cout << "No observation is received from the MPC node. Make sure the MPC node is running!" + << "\n"; + return; + } } - } - const scalar_t startTime = observation.time + 1.0; + const scalar_t startTime = observation.time + 1.0; - Gait stance; - stance.duration = 1.0; - stance.modeSequence = {15}; + Gait stance; + stance.duration = 1.0; + stance.modeSequence = {15}; - const auto gaitMessage = switched_model::ros_msg_conversions::toMessage(startTime, {motion.second, stance}); - auto mpcTargetTrajectoriesMsg = ocs2::ros_msg_conversions::createTargetTrajectoriesMsg(motion.first); - for (auto& t : mpcTargetTrajectoriesMsg.timeTrajectory) { - t += startTime; - } + const auto gaitMessage = switched_model::ros_msg_conversions::toMessage(startTime, {motion.second, stance}); + auto mpcTargetTrajectoriesMsg = ocs2::ros_msg_conversions::createTargetTrajectoriesMsg(motion.first); + for (auto &t : mpcTargetTrajectoriesMsg.time_trajectory) + { + t += startTime; + } - gaitSequencePublisher_.publish(gaitMessage); - targetTrajectoryPublisher_.publish(mpcTargetTrajectoriesMsg); -} + gaitSequencePublisher_->publish(gaitMessage); + targetTrajectoryPublisher_->publish(mpcTargetTrajectoriesMsg); + } -} // namespace switched_model \ No newline at end of file +} // namespace switched_model \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandInterface.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandInterface.cpp index 226367267..93eefae5d 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandInterface.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/MotionCommandInterface.cpp @@ -4,64 +4,76 @@ #include "ocs2_anymal_commands/MotionCommandInterface.h" -#include - +#include "rclcpp/rclcpp.hpp" +#include #include #include #include "ocs2_anymal_commands/LoadMotions.h" -namespace switched_model { - -MotionCommandInterface::MotionCommandInterface(const std::string& configFile) { - bool verbose = false; - - scalar_t dt = 0.0; - ocs2::loadData::loadCppDataType(configFile, "dt", dt); - - std::vector motionList; - ocs2::loadData::loadStdVector(configFile, "motionList", motionList, verbose); +namespace switched_model +{ - const std::string motionFilesPath = ros::package::getPath("ocs2_anymal_commands") + "/config/motions/"; - for (const auto& motionName : motionList) { - const auto csvData = readCsv(motionFilesPath + motionName + ".txt"); - motionData_.insert({motionName, readMotion(csvData, dt)}); - } - printAnimationList(); -} - -void MotionCommandInterface::getKeyboardCommand() { - const std::string commandMsg = "Enter the desired motion, for the list of available motions enter \"list\""; - std::cout << commandMsg << ": "; + MotionCommandInterface::MotionCommandInterface(const std::string &configFile) + { + bool verbose = false; - auto shouldTerminate = []() { return !ros::ok() || !ros::master::check(); }; - const std::string motionCommand = ocs2::getCommandLineString(shouldTerminate); + scalar_t dt = 0.0; + ocs2::loadData::loadCppDataType(configFile, "dt", dt); - if (motionCommand.empty()) { - return; - } + std::vector motionList; + ocs2::loadData::loadStdVector(configFile, "motionList", motionList, verbose); - if (motionCommand == "list") { + const std::string motionFilesPath = ament_index_cpp::get_package_share_directory("ocs2_anymal_commands") + "/config/motions/"; + for (const auto &motionName : motionList) + { + const auto csvData = readCsv(motionFilesPath + motionName + ".txt"); + motionData_.insert({motionName, readMotion(csvData, dt)}); + } printAnimationList(); - return; } - try { - const auto& motion = motionData_.at(motionCommand); - std::cout << "Executing \"" << motionCommand << "\" \n"; - publishMotion(motion); - } catch (const std::out_of_range& e) { - std::cout << "Motion \"" << motionCommand << "\" not found.\n"; - printAnimationList(); + void MotionCommandInterface::getKeyboardCommand() + { + const std::string commandMsg = "Enter the desired motion, for the list of available motions enter \"list\""; + std::cout << commandMsg << ": "; + + auto shouldTerminate = []() + { return !rclcpp::ok(); }; + const std::string motionCommand = ocs2::getCommandLineString(shouldTerminate); + + if (motionCommand.empty()) + { + return; + } + + if (motionCommand == "list") + { + printAnimationList(); + return; + } + + try + { + const auto &motion = motionData_.at(motionCommand); + std::cout << "Executing \"" << motionCommand << "\" \n"; + publishMotion(motion); + } + catch (const std::out_of_range &e) + { + std::cout << "Motion \"" << motionCommand << "\" not found.\n"; + printAnimationList(); + } } -} -void MotionCommandInterface::printAnimationList() const { - std::cout << "\nList of available motions:\n"; - for (const auto& s : motionData_) { - std::cout << " * " << s.first << "\n"; + void MotionCommandInterface::printAnimationList() const + { + std::cout << "\nList of available motions:\n"; + for (const auto &s : motionData_) + { + std::cout << " * " << s.first << "\n"; + } + std::cout << std::endl; } - std::cout << std::endl; -} -} // namespace switched_model \ No newline at end of file +} // namespace switched_model \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/PoseCommandToCostDesiredRos.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/PoseCommandToCostDesiredRos.cpp index 9607a84d2..9b0f34ab7 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/PoseCommandToCostDesiredRos.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_commands/src/PoseCommandToCostDesiredRos.cpp @@ -8,76 +8,82 @@ #include "ocs2_anymal_commands/TerrainAdaptation.h" -namespace switched_model { - -PoseCommandToCostDesiredRos::PoseCommandToCostDesiredRos(::ros::NodeHandle& nodeHandle, const std::string& configFile) { - boost::property_tree::ptree pt; - boost::property_tree::read_info(configFile, pt); - targetDisplacementVelocity = pt.get("targetDisplacementVelocity"); - targetRotationVelocity = pt.get("targetRotationVelocity"); - comHeight = pt.get("comHeight"); - ocs2::loadData::loadEigenMatrix(configFile, "defaultJointState", defaultJointState); - - // Setup ROS communication - terrainSubscriber_ = nodeHandle.subscribe("/ocs2_anymal/localTerrain", 1, &PoseCommandToCostDesiredRos::terrainCallback, this); -} - -ocs2::TargetTrajectories PoseCommandToCostDesiredRos::commandLineToTargetTrajectories(const vector_t& commadLineTarget, - const ocs2::SystemObservation& observation) const { - auto deg2rad = [](scalar_t deg) { return (deg * M_PI / 180.0); }; - - // Command to desired Base - // x, y are relative, z is relative to terrain + default offset; - vector3_t comPositionDesired{commadLineTarget(0) + observation.state[3], commadLineTarget(1) + observation.state[4], commadLineTarget(2) + comHeight}; - // Roll and pitch are absolute, yaw is relative - vector3_t comOrientationDesired{deg2rad(commadLineTarget(3)), deg2rad(commadLineTarget(4)), deg2rad(commadLineTarget(5)) + observation.state[2]}; - const auto desiredTime = - desiredTimeToTarget(comOrientationDesired.z() - observation.state[2], comPositionDesired.x() - observation.state[3], - comPositionDesired.y() - observation.state[4]); - - { // Terrain adaptation - std::lock_guard lock(terrainMutex_); - comPositionDesired = adaptDesiredPositionHeightToTerrain(comPositionDesired, localTerrain_, comPositionDesired.z()); - comOrientationDesired = alignDesiredOrientationToTerrain(comOrientationDesired, localTerrain_); +namespace switched_model +{ + + PoseCommandToCostDesiredRos::PoseCommandToCostDesiredRos(const rclcpp::Node::SharedPtr &node, const std::string &configFile) + { + boost::property_tree::ptree pt; + boost::property_tree::read_info(configFile, pt); + targetDisplacementVelocity = pt.get("targetDisplacementVelocity"); + targetRotationVelocity = pt.get("targetRotationVelocity"); + comHeight = pt.get("comHeight"); + ocs2::loadData::loadEigenMatrix(configFile, "defaultJointState", defaultJointState); + + // Setup ROS communication + terrainSubscriber_ = node->create_subscription("/ocs2_anymal/localTerrain", 1, std::bind(&PoseCommandToCostDesiredRos::terrainCallback, this, std::placeholders::_1)); } - // Desired time trajectory - const ocs2::scalar_array_t tDesiredTrajectory{observation.time, observation.time + desiredTime}; - - // Desired state trajectory - ocs2::vector_array_t xDesiredTrajectory(2, ocs2::vector_t::Zero(STATE_DIM)); - xDesiredTrajectory[0].segment<12>(0) = observation.state.segment<12>(0); - xDesiredTrajectory[0].segment<12>(12) = defaultJointState; - - xDesiredTrajectory[1].segment<3>(0) = comOrientationDesired; - // base x, y relative to current state - xDesiredTrajectory[1].segment<3>(3) = comPositionDesired; - // target velocities - xDesiredTrajectory[1].segment<6>(6).setZero(); - // joint angle from initialization - xDesiredTrajectory[1].segment<12>(12) = defaultJointState; - - // Desired input trajectory - const ocs2::vector_array_t uDesiredTrajectory(2, ocs2::vector_t::Zero(INPUT_DIM)); - - return {tDesiredTrajectory, xDesiredTrajectory, uDesiredTrajectory}; -} + ocs2::TargetTrajectories PoseCommandToCostDesiredRos::commandLineToTargetTrajectories(const vector_t &commadLineTarget, + const ocs2::SystemObservation &observation) const + { + auto deg2rad = [](scalar_t deg) + { return (deg * M_PI / 180.0); }; + + // Command to desired Base + // x, y are relative, z is relative to terrain + default offset; + vector3_t comPositionDesired{commadLineTarget(0) + observation.state[3], commadLineTarget(1) + observation.state[4], commadLineTarget(2) + comHeight}; + // Roll and pitch are absolute, yaw is relative + vector3_t comOrientationDesired{deg2rad(commadLineTarget(3)), deg2rad(commadLineTarget(4)), deg2rad(commadLineTarget(5)) + observation.state[2]}; + const auto desiredTime = + desiredTimeToTarget(comOrientationDesired.z() - observation.state[2], comPositionDesired.x() - observation.state[3], + comPositionDesired.y() - observation.state[4]); + + { // Terrain adaptation + std::lock_guard lock(terrainMutex_); + comPositionDesired = adaptDesiredPositionHeightToTerrain(comPositionDesired, localTerrain_, comPositionDesired.z()); + comOrientationDesired = alignDesiredOrientationToTerrain(comOrientationDesired, localTerrain_); + } + + // Desired time trajectory + const ocs2::scalar_array_t tDesiredTrajectory{observation.time, observation.time + desiredTime}; + + // Desired state trajectory + ocs2::vector_array_t xDesiredTrajectory(2, ocs2::vector_t::Zero(STATE_DIM)); + xDesiredTrajectory[0].segment<12>(0) = observation.state.segment<12>(0); + xDesiredTrajectory[0].segment<12>(12) = defaultJointState; + + xDesiredTrajectory[1].segment<3>(0) = comOrientationDesired; + // base x, y relative to current state + xDesiredTrajectory[1].segment<3>(3) = comPositionDesired; + // target velocities + xDesiredTrajectory[1].segment<6>(6).setZero(); + // joint angle from initialization + xDesiredTrajectory[1].segment<12>(12) = defaultJointState; + + // Desired input trajectory + const ocs2::vector_array_t uDesiredTrajectory(2, ocs2::vector_t::Zero(INPUT_DIM)); + + return {tDesiredTrajectory, xDesiredTrajectory, uDesiredTrajectory}; + } -void PoseCommandToCostDesiredRos::terrainCallback(const visualization_msgs::Marker::ConstPtr& msg) { - Eigen::Quaterniond orientationTerrainToWorld{msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, - msg->pose.orientation.z}; + void PoseCommandToCostDesiredRos::terrainCallback(const visualization_msgs::msg::Marker::ConstSharedPtr &msg) + { + Eigen::Quaterniond orientationTerrainToWorld{msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, + msg->pose.orientation.z}; - std::lock_guard lock(terrainMutex_); - localTerrain_.positionInWorld = {msg->pose.position.x, msg->pose.position.y, msg->pose.position.z}; - localTerrain_.orientationWorldToTerrain = orientationTerrainToWorld.toRotationMatrix().transpose(); -} + std::lock_guard lock(terrainMutex_); + localTerrain_.positionInWorld = {msg->pose.position.x, msg->pose.position.y, msg->pose.position.z}; + localTerrain_.orientationWorldToTerrain = orientationTerrainToWorld.toRotationMatrix().transpose(); + } -scalar_t PoseCommandToCostDesiredRos::desiredTimeToTarget(scalar_t dyaw, scalar_t dx, scalar_t dy) const { - scalar_t rotationTime = std::abs(dyaw) / targetRotationVelocity; - scalar_t displacement = std::sqrt(dx * dx + dy * dy); - scalar_t displacementTime = displacement / targetDisplacementVelocity; + scalar_t PoseCommandToCostDesiredRos::desiredTimeToTarget(scalar_t dyaw, scalar_t dx, scalar_t dy) const + { + scalar_t rotationTime = std::abs(dyaw) / targetRotationVelocity; + scalar_t displacement = std::sqrt(dx * dx + dy * dy); + scalar_t displacementTime = displacement / targetDisplacementVelocity; - return std::max(rotationTime, displacementTime); -} + return std::max(rotationTime, displacementTime); + } -} // namespace switched_model +} // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/CMakeLists.txt index b74047744..2425904e0 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/CMakeLists.txt @@ -1,33 +1,39 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) +set(CMAKE_CXX_STANDARD 17) project(ocs2_anymal_loopshaping_mpc) -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - roslib - ocs2_anymal_mpc - ocs2_quadruped_loopshaping_interface -) +## Find ament_cmake macros and libraries +find_package(ament_cmake REQUIRED) +find_package(convex_plane_decomposition REQUIRED) +find_package(convex_plane_decomposition_ros REQUIRED) +find_package(grid_map_ros REQUIRED) +find_package(rclcpp REQUIRED) +find_package(ocs2_anymal_mpc REQUIRED) +find_package(ocs2_quadruped_loopshaping_interface REQUIRED) +find_package(PCL REQUIRED) +find_package(segmented_planes_terrain_model REQUIRED) ## Eigen3 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 - roslib - ocs2_anymal_mpc - ocs2_quadruped_loopshaping_interface - DEPENDS +find_package(Boost REQUIRED COMPONENTS + system + filesystem + log_setup + log +) + + + + +set(dependencies + convex_plane_decomposition + convex_plane_decomposition_ros + grid_map_ros + rclcpp + ocs2_anymal_mpc + ocs2_quadruped_loopshaping_interface + segmented_planes_terrain_model ) ########### @@ -37,86 +43,94 @@ catkin_package( include_directories( include ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} ) add_library(${PROJECT_NAME} src/AnymalLoopshapingInterface.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) add_executable(${PROJECT_NAME}_mpc_node src/AnymalLoopshapingMpcNode.cpp ) +ament_target_dependencies(${PROJECT_NAME}_mpc_node + ${dependencies} +) target_link_libraries(${PROJECT_NAME}_mpc_node ${PROJECT_NAME} - ${catkin_LIBRARIES} ) add_executable(${PROJECT_NAME}_dummy_mrt_node src/AnymalLoopshapingDummyMrt.cpp ) +ament_target_dependencies(${PROJECT_NAME}_dummy_mrt_node + ${dependencies} +) target_link_libraries(${PROJECT_NAME}_dummy_mrt_node ${PROJECT_NAME} - ${catkin_LIBRARIES} ) add_executable(${PROJECT_NAME}_perceptive_demo src/PerceptiveMpcDemo.cpp ) +ament_target_dependencies(${PROJECT_NAME}_perceptive_demo + ${dependencies} +) target_link_libraries(${PROJECT_NAME}_perceptive_demo ${PROJECT_NAME} - ${catkin_LIBRARIES} - ) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if (cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_mpc_node ${PROJECT_NAME}_dummy_mrt_node - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -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(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include/${PROJECT_NAME} ) -install(TARGETS ${PROJECT_NAME}_mpc_node ${PROJECT_NAME}_dummy_mrt_node ${PROJECT_NAME}_perceptive_demo - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) +install(TARGETS + ${PROJECT_NAME}_mpc_node + ${PROJECT_NAME}_dummy_mrt_node + ${PROJECT_NAME}_perceptive_demo + DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY config launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME}/ ) ############# ## Testing ## ############# +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) -# Build unit tests -catkin_add_gtest(${PROJECT_NAME}_test - test/testProblemFormulation.cpp - test/testMotionTracking.cpp - test/testSensitivity.cpp -) -target_link_libraries(${PROJECT_NAME}_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main -) \ No newline at end of file + # Build unit tests + ament_add_gtest(${PROJECT_NAME}_test + test/testProblemFormulation.cpp + test/testMotionTracking.cpp + test/testSensitivity.cpp + ) + ament_target_dependencies(${PROJECT_NAME}_test + ${dependencies} + ) + target_link_libraries(${PROJECT_NAME}_test + ${PROJECT_NAME} + ) + set_tests_properties(${PROJECT_NAME}_test PROPERTIES TIMEOUT 3600) +endif() + +ament_export_dependencies(${dependencies}) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/config/rviz/demo_config.rviz b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/config/rviz/demo_config.rviz index 9a17623d9..95616547b 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/config/rviz/demo_config.rviz +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/config/rviz/demo_config.rviz @@ -1,5 +1,5 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 138 Name: Displays Property Tree Widget: @@ -8,16 +8,16 @@ Panels: - /Terrain1/FilteredMap1 Splitter Ratio: 0.5038759708404541 Tree Height: 727 - - Class: rviz/Selection + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 - /CoGTrajectory1 @@ -25,7 +25,7 @@ Panels: - /video_side1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 @@ -39,7 +39,7 @@ Visualization Manager: Displays: - Alpha: 0.5 Cell Size: 0.10000000149011612 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: false Line Style: @@ -56,7 +56,7 @@ Visualization Manager: Reference Frame: world Value: false - Alpha: 1 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false Enabled: true Links: @@ -394,9 +394,9 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true - - Class: rviz/Group + - Class: rviz_common/Group Displays: - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /ocs2_anymal/desiredBaseTrajectory Name: Base Position Trajectory @@ -404,7 +404,7 @@ Visualization Manager: "": true Queue Size: 1 Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredVelTrajectory Name: Base Velocity Trajectory @@ -412,7 +412,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredAngVelTrajectory Name: Base Angular Velocity Trajectory @@ -424,7 +424,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -437,9 +437,9 @@ Visualization Manager: Topic: ocs2_anymal/desiredPoseTrajectory Unreliable: false Value: true - - Class: rviz/Group + - Class: rviz_common/Group Displays: - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /ocs2_anymal/desiredFeetTrajectory/LF Name: LF @@ -447,7 +447,7 @@ Visualization Manager: {} Queue Size: 1 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /ocs2_anymal/desiredFeetTrajectory/RF Name: RF @@ -455,7 +455,7 @@ Visualization Manager: {} Queue Size: 1 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /ocs2_anymal/desiredFeetTrajectory/LH Name: LH @@ -463,7 +463,7 @@ Visualization Manager: {} Queue Size: 1 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /ocs2_anymal/desiredFeetTrajectory/RH Name: RH @@ -473,9 +473,9 @@ Visualization Manager: Value: true Enabled: false Name: Feet Position Trajectories - - Class: rviz/Group + - Class: rviz_common/Group Displays: - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredFeetVelTrajectory/LF Name: LF @@ -483,7 +483,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredFeetVelTrajectory/RF Name: RF @@ -491,7 +491,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredFeetVelTrajectory/LH Name: LH @@ -499,7 +499,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredFeetVelTrajectory/RH Name: RH @@ -509,13 +509,13 @@ Visualization Manager: Value: false Enabled: false Name: Feet Velocity Trajectories - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 1 Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -532,7 +532,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -549,7 +549,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -566,7 +566,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -583,13 +583,13 @@ Visualization Manager: Name: Feet Pose Trajectories Enabled: true Name: Desired Trajectory - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 1 Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: false Head Length: 0.07000000029802322 @@ -602,7 +602,7 @@ Visualization Manager: Topic: ocs2_anymal/optimizedPoseTrajectory Unreliable: false Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: /ocs2_anymal/optimizedStateTrajectory Name: State Trajectory @@ -612,13 +612,13 @@ Visualization Manager: Future footholds: true Queue Size: 1 Value: true - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 1 Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -635,7 +635,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -652,7 +652,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -669,7 +669,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -686,13 +686,13 @@ Visualization Manager: Name: Feet Pose Trajectories Enabled: true Name: Optimized Trajectory - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 1 Arrow Length: 0.30000001192092896 Axes Length: 0.10000000149011612 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -705,7 +705,7 @@ Visualization Manager: Topic: ocs2_anymal/currentPose Unreliable: false Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: /ocs2_anymal/currentState Name: State @@ -720,7 +720,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.10000000149011612 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: false Head Length: 0.07000000029802322 @@ -733,7 +733,7 @@ Visualization Manager: Topic: /ocs2_anymal/currentFeetPoses Unreliable: false Value: false - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: false Marker Topic: /ocs2_anymal/localTerrain Name: LocalTerrain @@ -741,7 +741,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/currentCollisionSpheres Name: Collisions @@ -751,7 +751,7 @@ Visualization Manager: Value: false Enabled: true Name: Current - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 0.5 Autocompute Intensity Bounds: true @@ -761,7 +761,7 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 @@ -805,7 +805,7 @@ Visualization Manager: Unreliable: false Use ColorMap: false Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: /boundaries Name: Boundaries @@ -847,26 +847,26 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + - Class: rviz_default_plugins/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - - Class: rviz/SetGoal + - Class: rviz_default_plugins/SetGoal Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: - Class: rviz/ThirdPersonFollower + Class: rviz_default_plugins/ThirdPersonFollower Distance: 5.270549297332764 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -888,7 +888,7 @@ Visualization Manager: Yaw: 1.556549310684204 Saved: - Angle: 3.140000104904175 - Class: rviz/TopDownOrtho + Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -902,7 +902,7 @@ Visualization Manager: X: 0.16045600175857544 Y: 0.09311659634113312 - Angle: 6.260000228881836 - Class: rviz/TopDownOrtho + Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -915,7 +915,7 @@ Visualization Manager: Target Frame: X: 2.279409885406494 Y: -0.7245129942893982 - - Class: rviz/Orbit + - Class: rviz_default_plugins/Orbit Distance: 3.5650899410247803 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -935,7 +935,7 @@ Visualization Manager: Pitch: 0.434798002243042 Target Frame: Yaw: 5.150000095367432 - - Class: rviz/Orbit + - Class: rviz_default_plugins/Orbit Distance: 1.7256799936294556 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/anymal_c.launch.py b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/anymal_c.launch.py new file mode 100644 index 000000000..c083d93ba --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/anymal_c.launch.py @@ -0,0 +1,30 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + target_command = get_package_share_directory('ocs2_robotic_assets') + "/resources/anymal_c/urdf/anymal.urdf" + description_name = get_package_share_directory('ocs2_anymal_models') + "/urdf/anymal_camel_rsl.urdf" + ld = launch.LaunchDescription([ + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_anymal_loopshaping_mpc'), 'launch/mpc.launch.py') + ), + launch_arguments={ + 'robot_name': 'anymal_c', + 'config_name': 'c_series', + 'description_name': description_name, + 'target_command': target_command + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/camel.launch.py b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/camel.launch.py new file mode 100644 index 000000000..8d7f87846 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/camel.launch.py @@ -0,0 +1,30 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + target_command = get_package_share_directory('ocs2_anymal_mpc') + "/config/c_series/targetCommand.info" + description_name = get_package_share_directory('ocs2_anymal_models') + "/urdf/anymal_camel_rsl.urdf" + ld = launch.LaunchDescription([ + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_anymal_loopshaping_mpc'), 'launch/mpc.launch.py') + ), + launch_arguments={ + 'robot_name': 'camel', + 'config_name': 'c_series', + 'description_name': description_name, + 'target_command': target_command + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/mpc.launch.py b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/mpc.launch.py new file mode 100644 index 000000000..cad486606 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/mpc.launch.py @@ -0,0 +1,91 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='robot_name' + ), + launch.actions.DeclareLaunchArgument( + name='config_name' + ), + DeclareLaunchArgument( + name='rviz', + default_value='true', + ) , + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), + launch.actions.DeclareLaunchArgument( + name='description_name', + default_value='ocs2_anymal_description' + ), + launch.actions.DeclareLaunchArgument( + name='target_command', + default_value='' + ), + launch.actions.DeclareLaunchArgument( + name='urdf_model_path', + default_value=get_package_share_directory('ocs2_robotic_assets') + "/resources/anymal_c/urdf/anymal.urdf" + ), + launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name='robot_state_publisher', + output='screen', + arguments=[LaunchConfiguration("urdf_model_path")], + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_quadruped_interface'), 'launch/visualization.launch.py') + ), + launch_arguments={ + 'description_name': LaunchConfiguration('description_name'), + }.items() + ), + launch_ros.actions.Node( + package='ocs2_anymal_loopshaping_mpc', + executable='ocs2_anymal_loopshaping_mpc_mpc_node', + name='ocs2_anymal_loopshaping_mpc_mpc_node', + prefix="", + arguments=[LaunchConfiguration('description_name'), LaunchConfiguration('config_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_anymal_loopshaping_mpc', + executable='ocs2_anymal_loopshaping_mpc_dummy_mrt_node', + name='ocs2_anymal_loopshaping_mpc_dummy_mrt_node', + prefix=LaunchConfiguration('terminal_prefix'), + arguments=[LaunchConfiguration('description_name'), LaunchConfiguration('config_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_anymal_commands', + executable='gait_command_node', + name='gait_command_node', + prefix=LaunchConfiguration('terminal_prefix'), + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_anymal_commands', + executable='target_command_node', + name='target_command_node', + prefix=LaunchConfiguration('terminal_prefix'), + arguments=[LaunchConfiguration('target_command')], + output='screen' + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/perceptive_mpc_demo.launch.py b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/perceptive_mpc_demo.launch.py new file mode 100644 index 000000000..b07b585ca --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/launch/perceptive_mpc_demo.launch.py @@ -0,0 +1,78 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + rviz_config_file = get_package_share_directory('ocs2_anymal_loopshaping_mpc') + "/config/rviz/demo_config.rviz" + urdf_model_path = get_package_share_directory('ocs2_robotic_assets') + "/resources/anymal_c/urdf/anymal.urdf" + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='robot_name', + default_value='anymal_c' + ), + launch.actions.DeclareLaunchArgument( + name='config_name', + default_value='c_series' + ), + launch.actions.DeclareLaunchArgument( + name='description_name', + default_value='ocs2_anymal_description' + ), + launch.actions.DeclareLaunchArgument( + name='perception_parameter_file', + default_value=get_package_share_directory( + 'convex_plane_decomposition_ros') + '/config/parameters.yaml' + ), + launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name='robot_state_publisher', + output='screen', + arguments=[urdf_model_path], + ), + launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] + ), + launch_ros.actions.Node( + package='ocs2_anymal_loopshaping_mpc', + executable='ocs2_anymal_loopshaping_mpc_perceptive_demo', + name='ocs2_anymal_loopshaping_mpc_perceptive_demo', + output='screen', + parameters=[ + { + 'config_name': launch.substitutions.LaunchConfiguration('config_name') + }, + { + 'forward_velocity': 0.5 + }, + { + 'terrain_name': 'step.png' + }, + { + 'ocs2_anymal_description': urdf_model_path + }, + { + 'terrain_scale': 0.35 + }, + { + 'adaptReferenceToTerrain': True + }, + launch.substitutions.LaunchConfiguration( + 'perception_parameter_file') + ] + ), + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/package.xml b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/package.xml index 0a8730417..a48d37dee 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/package.xml +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/package.xml @@ -1,5 +1,5 @@ - + ocs2_anymal_loopshaping_mpc 0.0.0 ocs2_anymal_loopshaping_mpc @@ -7,10 +7,17 @@ ruben TODO - catkin + ament_cmake - roslib + convex_plane_decomposition + convex_plane_decomposition_ros + grid_map_ros + rclcpp ocs2_anymal_mpc ocs2_quadruped_loopshaping_interface + segmented_planes_terrain_model + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingDummyMrt.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingDummyMrt.cpp index f784a4895..f1ef80458 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingDummyMrt.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingDummyMrt.cpp @@ -4,29 +4,35 @@ #include #include -#include #include "ocs2_anymal_loopshaping_mpc/AnymalLoopshapingInterface.h" +#include "rclcpp/rclcpp.hpp" int main(int argc, char* argv[]) { - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() < 3) { - throw std::runtime_error("No robot name and config folder specified. Aborting."); + throw std::runtime_error( + "No robot name and config folder specified. Aborting."); } - const std::string descriptionName(programArgs[1]); + const std::string urdfPath(programArgs[1]); const std::string configName(programArgs[2]); // Initialize ros node - ros::init(argc, argv, "anymal_loopshaping_mrt"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared("anymal_loopshaping_mrt"); - std::string urdfString; - nodeHandle.getParam(descriptionName, urdfString); + std::string urdfString = anymal::getUrdfString(urdfPath); - auto anymalInterface = anymal::getAnymalLoopshapingInterface(urdfString, anymal::getConfigFolderLoopshaping(configName)); - const auto mpcSettings = ocs2::mpc::loadSettings(anymal::getTaskFilePathLoopshaping(configName)); - quadrupedLoopshapingDummyNode(nodeHandle, *anymalInterface, mpcSettings.mrtDesiredFrequency_, mpcSettings.mpcDesiredFrequency_); + auto anymalInterface = anymal::getAnymalLoopshapingInterface( + urdfString, anymal::getConfigFolderLoopshaping(configName)); + const auto mpcSettings = + ocs2::mpc::loadSettings(anymal::getTaskFilePathLoopshaping(configName)); + quadrupedLoopshapingDummyNode(node, *anymalInterface, + mpcSettings.mrtDesiredFrequency_, + mpcSettings.mpcDesiredFrequency_); return 0; } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingInterface.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingInterface.cpp index eb72a91f1..d20c05d0b 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingInterface.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingInterface.cpp @@ -4,8 +4,8 @@ #include "ocs2_anymal_loopshaping_mpc/AnymalLoopshapingInterface.h" -#include - +#include "rclcpp/rclcpp.hpp" +#include #include namespace anymal { @@ -29,7 +29,7 @@ std::unique_ptr getAn } std::string getConfigFolderLoopshaping(const std::string& configName) { - return ros::package::getPath("ocs2_anymal_loopshaping_mpc") + "/config/" + configName; + return ament_index_cpp::get_package_share_directory("ocs2_anymal_loopshaping_mpc") + "/config/" + configName; } std::string getTaskFilePathLoopshaping(const std::string& configName) { diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingMpcNode.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingMpcNode.cpp index c16cce2be..f80856bd1 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingMpcNode.cpp @@ -2,7 +2,7 @@ // Created by rgrandia on 13.02.20. // -#include +#include "rclcpp/rclcpp.hpp" #include #include @@ -11,38 +11,42 @@ #include "ocs2_anymal_loopshaping_mpc/AnymalLoopshapingInterface.h" -int main(int argc, char* argv[]) { - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); - if (programArgs.size() < 3) { +int main(int argc, char *argv[]) +{ + std::vector programArgs = rclcpp::remove_ros_arguments(argc, argv); + + if (programArgs.size() < 3) + { throw std::runtime_error("No robot name and config folder specified. Aborting."); } - const std::string descriptionName(programArgs[1]); + const std::string urdfPath(programArgs[1]); const std::string configName(programArgs[2]); // Initialize ros node - ros::init(argc, argv, "anymal_loopshaping_mpc"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("anymal_loopshaping_mpc"); - std::string urdfString; - nodeHandle.getParam(descriptionName, urdfString); + std::string urdfString = anymal::getUrdfString(urdfPath); auto anymalInterface = anymal::getAnymalLoopshapingInterface(urdfString, anymal::getConfigFolderLoopshaping(configName)); const auto mpcSettings = ocs2::mpc::loadSettings(anymal::getTaskFilePathLoopshaping(configName)); - switch (anymalInterface->modelSettings().algorithm_) { - case switched_model::Algorithm::DDP: { - const auto ddpSettings = ocs2::ddp::loadSettings(anymal::getTaskFilePathLoopshaping(configName)); - auto mpcPtr = getDdpMpc(*anymalInterface, mpcSettings, ddpSettings); - quadrupedLoopshapingMpcNode(nodeHandle, *anymalInterface, std::move(mpcPtr)); - break; - } - case switched_model::Algorithm::SQP: { - const auto sqpSettings = ocs2::sqp::loadSettings(anymal::getConfigFolderLoopshaping(configName) + "/multiple_shooting.info"); - auto mpcPtr = getSqpMpc(*anymalInterface, mpcSettings, sqpSettings); - quadrupedLoopshapingMpcNode(nodeHandle, *anymalInterface, std::move(mpcPtr)); - break; - } + switch (anymalInterface->modelSettings().algorithm_) + { + case switched_model::Algorithm::DDP: + { + const auto ddpSettings = ocs2::ddp::loadSettings(anymal::getTaskFilePathLoopshaping(configName)); + auto mpcPtr = getDdpMpc(*anymalInterface, mpcSettings, ddpSettings); + quadrupedLoopshapingMpcNode(node, *anymalInterface, std::move(mpcPtr)); + break; + } + case switched_model::Algorithm::SQP: + { + const auto sqpSettings = ocs2::sqp::loadSettings(anymal::getConfigFolderLoopshaping(configName) + "/multiple_shooting.info"); + auto mpcPtr = getSqpMpc(*anymalInterface, mpcSettings, sqpSettings); + quadrupedLoopshapingMpcNode(node, *anymalInterface, std::move(mpcPtr)); + break; + } } return 0; diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/PerceptiveMpcDemo.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/PerceptiveMpcDemo.cpp index 64c3ac33e..0abe207ab 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/PerceptiveMpcDemo.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/PerceptiveMpcDemo.cpp @@ -2,11 +2,10 @@ // Created by rgrandia on 31.03.22. // -#include -#include - #include +#include "rclcpp/rclcpp.hpp" + // Plane segmentation #include #include @@ -27,51 +26,75 @@ #include #include #include +#include using namespace switched_model; int main(int argc, char* argv[]) { const std::string path(__FILE__); - const std::string ocs2_anymal = path.substr(0, path.find_last_of("/")) + "/../../"; - const std::string taskFolder = ocs2_anymal + "ocs2_anymal_loopshaping_mpc/config/"; - const std::string terrainFolder = ocs2_anymal + "ocs2_anymal_loopshaping_mpc/data/"; + const std::string ocs2_anymal = + path.substr(0, path.find_last_of("/")) + "/../../"; + const std::string taskFolder = + ocs2_anymal + "ocs2_anymal_loopshaping_mpc/config/"; + const std::string terrainFolder = + ocs2_anymal + "ocs2_anymal_loopshaping_mpc/data/"; const bool use_ros = false; std::string urdfString = getUrdfString(anymal::AnymalModel::Camel); - std::string configName = "c_series"; const std::string robotName = "anymal"; - ros::init(argc, argv, "anymal_perceptive_mpc_demo"); - ros::NodeHandle nodeHandle; - - ros::param::get("ocs2_anymal_description", urdfString); - nodeHandle.getParam("/config_name", configName); - - convex_plane_decomposition::PlaneDecompositionPipeline::Config perceptionConfig; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "anymal_perceptive_mpc_demo"); + node->declare_parameter("ocs2_anymal_description", "anymal"); + const std::string urdfPath = + node->get_parameter("ocs2_anymal_description").as_string(); + if (urdfPath != "anymal") urdfString = anymal::getUrdfString(urdfPath); + node->declare_parameter("config_name", "c_series"); + const std::string configName = node->get_parameter("config_name").as_string(); + + convex_plane_decomposition::PlaneDecompositionPipeline::Config + perceptionConfig; perceptionConfig.preprocessingParameters = - convex_plane_decomposition::loadPreprocessingParameters(nodeHandle, "/ocs2_anymal_loopshaping_mpc_perceptive_demo/preprocessing/"); - perceptionConfig.contourExtractionParameters = convex_plane_decomposition::loadContourExtractionParameters( - nodeHandle, "/ocs2_anymal_loopshaping_mpc_perceptive_demo/contour_extraction/"); - perceptionConfig.ransacPlaneExtractorParameters = convex_plane_decomposition::loadRansacPlaneExtractorParameters( - nodeHandle, "/ocs2_anymal_loopshaping_mpc_perceptive_demo/ransac_plane_refinement/"); - perceptionConfig.slidingWindowPlaneExtractorParameters = convex_plane_decomposition::loadSlidingWindowPlaneExtractorParameters( - nodeHandle, "/ocs2_anymal_loopshaping_mpc_perceptive_demo/sliding_window_plane_extractor/"); + convex_plane_decomposition::loadPreprocessingParameters( + node.get(), + "/ocs2_anymal_loopshaping_mpc_perceptive_demo/preprocessing/"); + perceptionConfig.contourExtractionParameters = + convex_plane_decomposition::loadContourExtractionParameters( + node.get(), + "/ocs2_anymal_loopshaping_mpc_perceptive_demo/contour_extraction/"); + perceptionConfig.ransacPlaneExtractorParameters = + convex_plane_decomposition::loadRansacPlaneExtractorParameters( + node.get(), + "/ocs2_anymal_loopshaping_mpc_perceptive_demo/" + "ransac_plane_refinement/"); + perceptionConfig.slidingWindowPlaneExtractorParameters = + convex_plane_decomposition::loadSlidingWindowPlaneExtractorParameters( + node.get(), + "/ocs2_anymal_loopshaping_mpc_perceptive_demo/" + "sliding_window_plane_extractor/"); perceptionConfig.postprocessingParameters = - convex_plane_decomposition::loadPostprocessingParameters(nodeHandle, "/ocs2_anymal_loopshaping_mpc_perceptive_demo/postprocessing/"); + convex_plane_decomposition::loadPostprocessingParameters( + node.get(), + "/ocs2_anymal_loopshaping_mpc_perceptive_demo/postprocessing/"); - auto anymalInterface = anymal::getAnymalLoopshapingInterface(urdfString, taskFolder + configName); - const ocs2::vector_t initSystemState = anymalInterface->getInitialState().head(switched_model::STATE_DIM); + auto anymalInterface = anymal::getAnymalLoopshapingInterface( + urdfString, taskFolder + configName); + const ocs2::vector_t initSystemState = + anymalInterface->getInitialState().head(switched_model::STATE_DIM); // ====== Scenario settings ======== - ocs2::scalar_t forwardVelocity{0.0}; - nodeHandle.getParam("/forward_velocity", forwardVelocity); + node->declare_parameter("forward_velocity", 0.5); + ocs2::scalar_t forwardVelocity = + node->get_parameter("forward_velocity").as_double(); ocs2::scalar_t gaitDuration{0.8}; ocs2::scalar_t forwardDistance{3.0}; ocs2::scalar_t initTime = 0.0; ocs2::scalar_t stanceTime = 1.0; - int numGaitCycles = std::ceil((forwardDistance / forwardVelocity) / gaitDuration); + int numGaitCycles = + std::ceil((forwardDistance / forwardVelocity) / gaitDuration); ocs2::scalar_t walkTime = numGaitCycles * gaitDuration; ocs2::scalar_t finalTime = walkTime + 2 * stanceTime; ocs2::scalar_t f_mpc = 100; // hz @@ -79,13 +102,15 @@ int main(int argc, char* argv[]) { // Load a map const std::string elevationLayer{"elevation"}; const std::string frameId{"world"}; - std::string terrainFile{""}; - nodeHandle.getParam("/terrain_name", terrainFile); - double heightScale{1.0}; - nodeHandle.getParam("/terrain_scale", heightScale); - auto gridMap = convex_plane_decomposition::loadGridmapFromImage(terrainFolder + "/" + terrainFile, elevationLayer, frameId, - perceptionConfig.preprocessingParameters.resolution, heightScale); - gridMap.get(elevationLayer).array() -= gridMap.atPosition(elevationLayer, {0., 0.}); + node->declare_parameter("terrain_name", "step.png"); + std::string terrainFile = node->get_parameter("terrain_name").as_string(); + node->declare_parameter("terrain_scale", 0.35); + double heightScale = node->get_parameter("terrain_scale").as_double(); + auto gridMap = convex_plane_decomposition::loadGridmapFromImage( + terrainFolder + "/" + terrainFile, elevationLayer, frameId, + perceptionConfig.preprocessingParameters.resolution, heightScale); + gridMap.get(elevationLayer).array() -= + gridMap.atPosition(elevationLayer, {0., 0.}); // Gait using MN = switched_model::ModeNumber; @@ -106,11 +131,13 @@ int main(int argc, char* argv[]) { gaitSequence.push_back(stance); // Reference trajectory - bool adaptReferenceToTerrain = true; - nodeHandle.getParam("/adaptReferenceToTerrain", adaptReferenceToTerrain); + node->declare_parameter("adaptReferenceToTerrain", true); + bool adaptReferenceToTerrain = + node->get_parameter("adaptReferenceToTerrain").as_bool(); const double dtRef = 0.1; - const BaseReferenceHorizon commandHorizon{dtRef, size_t(walkTime / dtRef) + 1}; + const BaseReferenceHorizon commandHorizon{dtRef, + size_t(walkTime / dtRef) + 1}; BaseReferenceCommand command; command.baseHeight = getPositionInOrigin(getBasePose(initSystemState)).z(); command.yawRate = 0.0; @@ -118,32 +145,43 @@ int main(int argc, char* argv[]) { command.lateralVelocity = 0.0; // ====== Run the perception pipeline ======== - convex_plane_decomposition::PlaneDecompositionPipeline planeDecompositionPipeline(perceptionConfig); + convex_plane_decomposition::PlaneDecompositionPipeline + planeDecompositionPipeline(perceptionConfig); planeDecompositionPipeline.update(grid_map::GridMap(gridMap), elevationLayer); auto& planarTerrain = planeDecompositionPipeline.getPlanarTerrain(); - auto terrainModel = std::unique_ptr(new SegmentedPlanesTerrainModel(planarTerrain)); + auto terrainModel = std::unique_ptr( + new SegmentedPlanesTerrainModel(planarTerrain)); // Read min-max from elevation map - const float heightMargin = 0.5; // Create SDF till this amount above and below the map. + const float heightMargin = + 0.5; // Create SDF till this amount above and below the map. const auto& elevationData = gridMap.get(elevationLayer); const float minValue = elevationData.minCoeffOfFinites() - heightMargin; const float maxValue = elevationData.maxCoeffOfFinites() + heightMargin; - terrainModel->createSignedDistanceBetween({-1e30, -1e30, minValue}, {1e30, 1e30, maxValue}); // will project XY range to map limits + terrainModel->createSignedDistanceBetween( + {-1e30, -1e30, minValue}, + {1e30, 1e30, maxValue}); // will project XY range to map limits // ====== Generate reference trajectory ======== - const auto& baseToHipInBase = anymalInterface->getKinematicModel().baseToLegRootInBaseFrame(0); - const double nominalStanceWidthInHeading = 2.0 * (std::abs(baseToHipInBase.x()) + 0.15); - const double nominalStanceWidthLateral = 2.0 * (std::abs(baseToHipInBase.y()) + 0.10); + const auto& baseToHipInBase = + anymalInterface->getKinematicModel().baseToLegRootInBaseFrame(0); + const double nominalStanceWidthInHeading = + 2.0 * (std::abs(baseToHipInBase.x()) + 0.15); + const double nominalStanceWidthLateral = + 2.0 * (std::abs(baseToHipInBase.y()) + 0.10); - BaseReferenceState initialBaseState{stanceTime, getPositionInOrigin(getBasePose(initSystemState)), - getOrientation(getBasePose(initSystemState))}; + BaseReferenceState initialBaseState{ + stanceTime, getPositionInOrigin(getBasePose(initSystemState)), + getOrientation(getBasePose(initSystemState))}; BaseReferenceTrajectory terrainAdaptedBaseReference; if (adaptReferenceToTerrain) { - terrainAdaptedBaseReference = generateExtrapolatedBaseReference(commandHorizon, initialBaseState, command, planarTerrain.gridMap, - nominalStanceWidthInHeading, nominalStanceWidthLateral); + terrainAdaptedBaseReference = generateExtrapolatedBaseReference( + commandHorizon, initialBaseState, command, planarTerrain.gridMap, + nominalStanceWidthInHeading, nominalStanceWidthLateral); } else { - terrainAdaptedBaseReference = generateExtrapolatedBaseReference(commandHorizon, initialBaseState, command, TerrainPlane()); + terrainAdaptedBaseReference = generateExtrapolatedBaseReference( + commandHorizon, initialBaseState, command, TerrainPlane()); } ocs2::TargetTrajectories targetTrajectories; @@ -154,44 +192,55 @@ int main(int argc, char* argv[]) { targetTrajectories.inputTrajectory.push_back(vector_t::Zero(INPUT_DIM)); targetTrajectories.inputTrajectory.push_back(vector_t::Zero(INPUT_DIM)); for (int k = 0; k < terrainAdaptedBaseReference.time.size(); ++k) { - targetTrajectories.timeTrajectory.push_back(terrainAdaptedBaseReference.time[k]); + targetTrajectories.timeTrajectory.push_back( + terrainAdaptedBaseReference.time[k]); - const auto R_WtoB = rotationMatrixOriginToBase(terrainAdaptedBaseReference.eulerXyz[k]); + const auto R_WtoB = + rotationMatrixOriginToBase(terrainAdaptedBaseReference.eulerXyz[k]); Eigen::VectorXd costReference(switched_model::STATE_DIM); - costReference << terrainAdaptedBaseReference.eulerXyz[k], terrainAdaptedBaseReference.positionInWorld[k], - R_WtoB * terrainAdaptedBaseReference.angularVelocityInWorld[k], R_WtoB * terrainAdaptedBaseReference.linearVelocityInWorld[k], + costReference << terrainAdaptedBaseReference.eulerXyz[k], + terrainAdaptedBaseReference.positionInWorld[k], + R_WtoB * terrainAdaptedBaseReference.angularVelocityInWorld[k], + R_WtoB * terrainAdaptedBaseReference.linearVelocityInWorld[k], getJointPositions(initSystemState); targetTrajectories.stateTrajectory.push_back(std::move(costReference)); targetTrajectories.inputTrajectory.push_back(vector_t::Zero(INPUT_DIM)); } targetTrajectories.timeTrajectory.push_back(stanceTime + walkTime); targetTrajectories.timeTrajectory.push_back(finalTime); - targetTrajectories.stateTrajectory.push_back(targetTrajectories.stateTrajectory.back()); - targetTrajectories.stateTrajectory.push_back(targetTrajectories.stateTrajectory.back()); + targetTrajectories.stateTrajectory.push_back( + targetTrajectories.stateTrajectory.back()); + targetTrajectories.stateTrajectory.push_back( + targetTrajectories.stateTrajectory.back()); targetTrajectories.inputTrajectory.push_back(vector_t::Zero(INPUT_DIM)); targetTrajectories.inputTrajectory.push_back(vector_t::Zero(INPUT_DIM)); // ====== Set the scenario to the correct interfaces ======== - auto referenceManager = anymalInterface->getQuadrupedInterface().getSwitchedModelModeScheduleManagerPtr(); + auto referenceManager = anymalInterface->getQuadrupedInterface() + .getSwitchedModelModeScheduleManagerPtr(); // Register the terrain model referenceManager->getTerrainModel().reset(std::move(terrainModel)); // Register the gait - referenceManager->getGaitSchedule()->setGaitSequenceAtTime(gaitSequence, initTime); + referenceManager->getGaitSchedule()->setGaitSequenceAtTime(gaitSequence, + initTime); // Register the target trajectory referenceManager->setTargetTrajectories(targetTrajectories); // ====== Create MPC solver ======== - const auto mpcSettings = ocs2::mpc::loadSettings(taskFolder + configName + "/task.info"); + const auto mpcSettings = + ocs2::mpc::loadSettings(taskFolder + configName + "/task.info"); std::unique_ptr mpcPtr; - const auto sqpSettings = ocs2::sqp::loadSettings(taskFolder + configName + "/multiple_shooting.info"); + const auto sqpSettings = ocs2::sqp::loadSettings(taskFolder + configName + + "/multiple_shooting.info"); switch (anymalInterface->modelSettings().algorithm_) { case switched_model::Algorithm::DDP: { - const auto ddpSettings = ocs2::ddp::loadSettings(taskFolder + configName + "/task.info"); + const auto ddpSettings = + ocs2::ddp::loadSettings(taskFolder + configName + "/task.info"); mpcPtr = getDdpMpc(*anymalInterface, mpcSettings, ddpSettings); break; } @@ -202,7 +251,8 @@ int main(int argc, char* argv[]) { } ocs2::MPC_MRT_Interface mpcInterface(*mpcPtr); - std::unique_ptr rollout(anymalInterface->getRollout().clone()); + std::unique_ptr rollout( + anymalInterface->getRollout().clone()); // ====== Execute the scenario ======== ocs2::SystemObservation observation; @@ -231,18 +281,24 @@ int main(int argc, char* argv[]) { // Evaluate the optimized solution - change to optimal controller ocs2::vector_t tmp; - mpcInterface.evaluatePolicy(observation.time, observation.state, tmp, observation.input, observation.mode); - observation.input = ocs2::LinearInterpolation::interpolate(observation.time, mpcInterface.getPolicy().timeTrajectory_, - mpcInterface.getPolicy().inputTrajectory_); + mpcInterface.evaluatePolicy(observation.time, observation.state, tmp, + observation.input, observation.mode); + observation.input = ocs2::LinearInterpolation::interpolate( + observation.time, mpcInterface.getPolicy().timeTrajectory_, + mpcInterface.getPolicy().inputTrajectory_); closedLoopSolution.timeTrajectory_.push_back(observation.time); closedLoopSolution.stateTrajectory_.push_back(observation.state); closedLoopSolution.inputTrajectory_.push_back(observation.input); if (closedLoopSolution.modeSchedule_.modeSequence.empty()) { - closedLoopSolution.modeSchedule_.modeSequence.push_back(observation.mode); - } else if (closedLoopSolution.modeSchedule_.modeSequence.back() != observation.mode) { - closedLoopSolution.modeSchedule_.modeSequence.push_back(observation.mode); - closedLoopSolution.modeSchedule_.eventTimes.push_back(observation.time - 0.5 / f_mpc); + closedLoopSolution.modeSchedule_.modeSequence.push_back( + observation.mode); + } else if (closedLoopSolution.modeSchedule_.modeSequence.back() != + observation.mode) { + closedLoopSolution.modeSchedule_.modeSequence.push_back( + observation.mode); + closedLoopSolution.modeSchedule_.eventTimes.push_back(observation.time - + 0.5 / f_mpc); } // perform a rollout @@ -251,113 +307,150 @@ int main(int argc, char* argv[]) { vector_array_t stateTrajectory, inputTrajectory; const scalar_t finalTime = observation.time + 1.0 / f_mpc; auto modeschedule = mpcInterface.getPolicy().modeSchedule_; - rollout->run(observation.time, observation.state, finalTime, mpcInterface.getPolicy().controllerPtr_.get(), modeschedule, - timeTrajectory, postEventIndicesStock, stateTrajectory, inputTrajectory); + rollout->run(observation.time, observation.state, finalTime, + mpcInterface.getPolicy().controllerPtr_.get(), modeschedule, + timeTrajectory, postEventIndicesStock, stateTrajectory, + inputTrajectory); observation.time = finalTime; observation.state = stateTrajectory.back(); - observation.input.setZero(switched_model_loopshaping::INPUT_DIM); // reset + observation.input.setZero( + switched_model_loopshaping::INPUT_DIM); // reset } catch (std::exception& e) { std::cout << "MPC failed\n"; std::cout << e.what() << "\n"; break; } } - const auto closedLoopSystemSolution = - ocs2::loopshapingToSystemPrimalSolution(closedLoopSolution, *anymalInterface->getLoopshapingDefinition()); + const auto closedLoopSystemSolution = ocs2::loopshapingToSystemPrimalSolution( + closedLoopSolution, *anymalInterface->getLoopshapingDefinition()); // ====== Print result ========== - const auto totalCost = std::accumulate(performances.cbegin(), performances.cend(), 0.0, - [](double v, const ocs2::PerformanceIndex& p) { return v + p.cost; }); + const auto totalCost = std::accumulate( + performances.cbegin(), performances.cend(), 0.0, + [](double v, const ocs2::PerformanceIndex& p) { return v + p.cost; }); const auto totalDynamics = std::accumulate(performances.cbegin(), performances.cend(), 0.0, - [](double v, const ocs2::PerformanceIndex& p) { return v + std::sqrt(p.dynamicsViolationSSE); }); - const auto maxDynamics = std::sqrt(std::max_element(performances.cbegin(), performances.cend(), - [](const ocs2::PerformanceIndex& lhs, const ocs2::PerformanceIndex& rhs) { - return lhs.dynamicsViolationSSE < rhs.dynamicsViolationSSE; - }) - ->dynamicsViolationSSE); + [](double v, const ocs2::PerformanceIndex& p) { + return v + std::sqrt(p.dynamicsViolationSSE); + }); + const auto maxDynamics = + std::sqrt(std::max_element(performances.cbegin(), performances.cend(), + [](const ocs2::PerformanceIndex& lhs, + const ocs2::PerformanceIndex& rhs) { + return lhs.dynamicsViolationSSE < + rhs.dynamicsViolationSSE; + }) + ->dynamicsViolationSSE); const auto totalEquality = std::accumulate(performances.cbegin(), performances.cend(), 0.0, - [](double v, const ocs2::PerformanceIndex& p) { return v + std::sqrt(p.equalityConstraintsSSE); }); - const auto maxEquality = std::sqrt(std::max_element(performances.cbegin(), performances.cend(), - [](const ocs2::PerformanceIndex& lhs, const ocs2::PerformanceIndex& rhs) { - return lhs.equalityConstraintsSSE < rhs.equalityConstraintsSSE; - }) - ->equalityConstraintsSSE); + [](double v, const ocs2::PerformanceIndex& p) { + return v + std::sqrt(p.equalityConstraintsSSE); + }); + const auto maxEquality = + std::sqrt(std::max_element(performances.cbegin(), performances.cend(), + [](const ocs2::PerformanceIndex& lhs, + const ocs2::PerformanceIndex& rhs) { + return lhs.equalityConstraintsSSE < + rhs.equalityConstraintsSSE; + }) + ->equalityConstraintsSSE); double achievedWalkTime = observation.time - stanceTime; std::cout << "Speed: " << forwardVelocity << "\n"; std::cout << "Scale: " << heightScale << "\n"; - std::cout << "Completed: " << std::min(1.0, (achievedWalkTime / walkTime)) * 100.0 << "\n"; + std::cout << "Completed: " + << std::min(1.0, (achievedWalkTime / walkTime)) * 100.0 << "\n"; std::cout << "average Cost: " << totalCost / performances.size() << "\n"; - std::cout << "average Dynamics constr: " << totalDynamics / performances.size() << "\n"; + std::cout << "average Dynamics constr: " + << totalDynamics / performances.size() << "\n"; std::cout << "max Dynamics constr: " << maxDynamics << "\n"; - std::cout << "average Equality constr: " << totalEquality / performances.size() << "\n"; + std::cout << "average Equality constr: " + << totalEquality / performances.size() << "\n"; std::cout << "max Equality constr: " << maxEquality << "\n"; // ====== Visualize ========== - QuadrupedVisualizer visualizer(anymalInterface->getKinematicModel(), anymalInterface->getJointNames(), anymalInterface->getBaseName(), - nodeHandle); - ros::Publisher elevationmapPublisher = nodeHandle.advertise("elevation_map", 1); - ros::Publisher filteredmapPublisher = nodeHandle.advertise("filtered_map", 1); - ros::Publisher boundaryPublisher = nodeHandle.advertise("boundaries", 1); - ros::Publisher insetPublisher = nodeHandle.advertise("insets", 1); - ros::Publisher distanceFieldPublisher = nodeHandle.advertise("signed_distance_field", 1, true); - - // Create pointcloud for visualization (terrain model ownership is now with the swing planner - const auto* sdfPtr = - dynamic_cast(referenceManager->getSwingTrajectoryPlanner().getSignedDistanceField()); - sensor_msgs::PointCloud2 pointCloud2Msg; + QuadrupedVisualizer visualizer(anymalInterface->getKinematicModel(), + anymalInterface->getJointNames(), + anymalInterface->getBaseName(), node); + auto elevationmapPublisher = + node->create_publisher("elevation_map", 1); + auto filteredmapPublisher = + node->create_publisher("filtered_map", 1); + auto boundaryPublisher = + node->create_publisher("boundaries", + 1); + auto insetPublisher = + node->create_publisher("insets", 1); + auto distanceFieldPublisher = + node->create_publisher( + "signed_distance_field", 1); + + // Create pointcloud for visualization (terrain model ownership is now with + // the swing planner + const auto* sdfPtr = dynamic_cast( + referenceManager->getSwingTrajectoryPlanner().getSignedDistanceField()); + sensor_msgs::msg::PointCloud2 pointCloud2Msg; if (sdfPtr != nullptr) { - const auto& sdf = sdfPtr->asGridmapSdf(); - grid_map::GridMapRosConverter::toPointCloud(sdf, pointCloud2Msg, 1, [](float val) { return val <= 0.0F; }); + SegmentedPlanesTerrainModelRos::toPointCloud( + *sdfPtr, pointCloud2Msg, 1, [](float val) { return val <= 0.0F; }); } // Grid map - grid_map_msgs::GridMap filteredMapMessage; - grid_map::GridMapRosConverter::toMessage(planarTerrain.gridMap, filteredMapMessage); - grid_map_msgs::GridMap elevationMapMessage; - grid_map::GridMapRosConverter::toMessage(gridMap, elevationMapMessage); + grid_map_msgs::msg::GridMap filteredMapMessage = + *(grid_map::GridMapRosConverter::toMessage(planarTerrain.gridMap)); + grid_map_msgs::msg::GridMap elevationMapMessage = + *(grid_map::GridMapRosConverter::toMessage(gridMap)); // Segmentation const double lineWidth = 0.005; // [m] RViz marker size - auto boundaries = convertBoundariesToRosMarkers(planarTerrain.planarRegions, planarTerrain.gridMap.getFrameId(), - planarTerrain.gridMap.getTimestamp(), lineWidth); - auto boundaryInsets = convertInsetsToRosMarkers(planarTerrain.planarRegions, planarTerrain.gridMap.getFrameId(), - planarTerrain.gridMap.getTimestamp(), lineWidth); - - while (ros::ok()) { - visualizer.publishOptimizedStateTrajectory(ros::Time::now(), closedLoopSystemSolution.timeTrajectory_, - closedLoopSystemSolution.stateTrajectory_, closedLoopSystemSolution.modeSchedule_); - visualizer.publishDesiredTrajectory(ros::Time::now(), targetTrajectories); - - filteredmapPublisher.publish(filteredMapMessage); - elevationmapPublisher.publish(elevationMapMessage); - boundaryPublisher.publish(boundaries); - insetPublisher.publish(boundaryInsets); + auto boundaries = convertBoundariesToRosMarkers( + planarTerrain.planarRegions, planarTerrain.gridMap.getFrameId(), + planarTerrain.gridMap.getTimestamp(), lineWidth); + auto boundaryInsets = convertInsetsToRosMarkers( + planarTerrain.planarRegions, planarTerrain.gridMap.getFrameId(), + planarTerrain.gridMap.getTimestamp(), lineWidth); + + rclcpp::Rate rate(1); + while (rclcpp::ok()) { + visualizer.publishOptimizedStateTrajectory( + node->get_clock()->now(), closedLoopSystemSolution.timeTrajectory_, + closedLoopSystemSolution.stateTrajectory_, + closedLoopSystemSolution.modeSchedule_); + visualizer.publishDesiredTrajectory(node->get_clock()->now(), + targetTrajectories); + + filteredmapPublisher->publish(filteredMapMessage); + elevationmapPublisher->publish(elevationMapMessage); + boundaryPublisher->publish(boundaries); + insetPublisher->publish(boundaryInsets); if (sdfPtr != nullptr) { - distanceFieldPublisher.publish(pointCloud2Msg); + distanceFieldPublisher->publish(pointCloud2Msg); } // Visualize the individual execution double speed = 1.0; - for (size_t k = 0; k < closedLoopSystemSolution.timeTrajectory_.size() - 1; k++) { - double frameDuration = speed * (closedLoopSystemSolution.timeTrajectory_[k + 1] - closedLoopSystemSolution.timeTrajectory_[k]); + for (size_t k = 0; k < closedLoopSystemSolution.timeTrajectory_.size() - 1; + k++) { + double frameDuration = + speed * (closedLoopSystemSolution.timeTrajectory_[k + 1] - + closedLoopSystemSolution.timeTrajectory_[k]); double publishDuration = ocs2::timedExecutionInSeconds([&]() { ocs2::SystemObservation observation; observation.time = closedLoopSystemSolution.timeTrajectory_[k]; observation.state = closedLoopSystemSolution.stateTrajectory_[k]; observation.input = closedLoopSystemSolution.inputTrajectory_[k]; - observation.mode = closedLoopSystemSolution.modeSchedule_.modeAtTime(observation.time); - visualizer.publishObservation(ros::Time::now(), observation); + observation.mode = + closedLoopSystemSolution.modeSchedule_.modeAtTime(observation.time); + visualizer.publishObservation(node->get_clock()->now(), observation); }); if (frameDuration > publishDuration) { - ros::WallDuration(frameDuration - publishDuration).sleep(); + const rclcpp::Duration duration = + rclcpp::Duration::from_seconds(frameDuration - publishDuration); + rclcpp::sleep_for((std::chrono::nanoseconds(duration.nanoseconds()))); } } - - ros::WallDuration(1.0).sleep(); + rate.sleep(); } } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/CMakeLists.txt index ebaa1fa72..c4e8a6634 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/CMakeLists.txt @@ -1,33 +1,29 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) +set(CMAKE_CXX_STANDARD 17) project(ocs2_anymal_models) -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - ocs2_switched_model_interface - ocs2_pinocchio_interface - ocs2_robotic_tools - ocs2_robotic_assets - roslib -) +## Find ament_cmake macros and libraries +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(ocs2_switched_model_interface REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) ## Eigen3 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_switched_model_interface - ocs2_robotic_tools - ocs2_pinocchio_interface - roslib -# DEPENDS +find_package(Boost REQUIRED COMPONENTS + system + filesystem + log_setup + log +) + +set(dependencies + ocs2_switched_model_interface + ocs2_robotic_tools + ocs2_pinocchio_interface + rclcpp ) ########### @@ -35,15 +31,16 @@ catkin_package( ########### # Resolve for the package path at compile time. +file(MAKE_DIRECTORY "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}") configure_file( "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h.in" - "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY + "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY ) include_directories( include + ${PROJECT_BINARY_DIR}/include ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} ) add_library(${PROJECT_NAME} @@ -54,13 +51,8 @@ add_library(${PROJECT_NAME} src/QuadrupedKinematics.cpp src/QuadrupedCom.cpp ) -add_dependencies(${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - dl +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) @@ -68,33 +60,41 @@ target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) ## 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 + INCLUDES DESTINATION include/${PROJECT_NAME} ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) +install(FILES "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" + DESTINATION include/${PROJECT_NAME}/${PROJECT_NAME} ) -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +install(DIRECTORY config launch + DESTINATION share/${PROJECT_NAME}/ ) ############# ## Testing ## ############# +find_package(ament_lint_auto REQUIRED) +ament_lint_auto_find_test_dependencies() +find_package(ament_cmake_gtest) # Build unit tests -catkin_add_gtest(${PROJECT_NAME}_switched_model_test +ament_add_gtest(${PROJECT_NAME}_switched_model_test test/TestDynamicsHelpers.cpp ) +ament_target_dependencies(${PROJECT_NAME}_switched_model_test + ${dependencies} +) target_link_libraries(${PROJECT_NAME}_switched_model_test ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main ) -catkin_add_gtest(TestQuadrupedPinocchio +ament_add_gtest(TestQuadrupedPinocchio test/camel/AnymalCamelCom.cpp test/camel/AnymalCamelKinematics.cpp test/TestFrameDeclaration.cpp @@ -102,8 +102,14 @@ catkin_add_gtest(TestQuadrupedPinocchio test/TestQuadrupedPinocchioCom.cpp test/TestQuadrupedPinocchioKinematics.cpp ) +ament_target_dependencies(TestQuadrupedPinocchio + ${dependencies} +) target_link_libraries(TestQuadrupedPinocchio ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main ) + +ament_export_dependencies(${dependencies}) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/config/visualize_urdf.rviz b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/config/visualize_urdf.rviz index 6ee6bb1fe..c2cd1f273 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/config/visualize_urdf.rviz +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/config/visualize_urdf.rviz @@ -1,44 +1,40 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 + - /Grid1 - /RobotModel1 - - /TF1 + - /RobotModel1/Description Topic1 Splitter Ratio: 0.5 - Tree Height: 549 - - Class: rviz/Selection + Tree Height: 439 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 + - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -54,222 +50,463 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Alpha: 0.5 - Class: rviz/RobotModel + - Alpha: 1 + Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description Enabled: true Links: All Links Enabled: true Expand Joint Details: false Expand Link Details: false Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: + LF_FOOT: Alpha: 1 Show Axes: false Show Trail: false - body_bracket: + Value: true + LF_HAA: Alpha: 1 Show Axes: false Show Trail: false Value: true - hip_bracket_f_l: + LF_HFE: Alpha: 1 Show Axes: false Show Trail: false Value: true - hip_bracket_f_r: + LF_HIP: + Alpha: 1 + Show Axes: false + Show Trail: false + LF_KFE: Alpha: 1 Show Axes: false Show Trail: false Value: true - hip_bracket_h_l: + LF_SHANK: + Alpha: 1 + Show Axes: false + Show Trail: false + LF_THIGH: + Alpha: 1 + Show Axes: false + Show Trail: false + LF_hip_fixed: Alpha: 1 Show Axes: false Show Trail: false Value: true - hip_bracket_h_r: + LF_shank_fixed: Alpha: 1 Show Axes: false Show Trail: false Value: true - lowerleg_f_l: + LF_thigh_fixed: Alpha: 1 Show Axes: false Show Trail: false Value: true - lowerleg_f_r: + LH_FOOT: Alpha: 1 Show Axes: false Show Trail: false Value: true - lowerleg_h_l: + LH_HAA: Alpha: 1 Show Axes: false Show Trail: false Value: true - lowerleg_h_r: + LH_HFE: Alpha: 1 Show Axes: false Show Trail: false Value: true - pelvis: + LH_HIP: + Alpha: 1 + Show Axes: false + Show Trail: false + LH_KFE: Alpha: 1 Show Axes: false Show Trail: false Value: true - torso: + LH_SHANK: + Alpha: 1 + Show Axes: false + Show Trail: false + LH_THIGH: + Alpha: 1 + Show Axes: false + Show Trail: false + LH_hip_fixed: Alpha: 1 Show Axes: false Show Trail: false Value: true - upperleg_f_l: + LH_shank_fixed: Alpha: 1 Show Axes: false Show Trail: false Value: true - upperleg_f_r: + LH_thigh_fixed: Alpha: 1 Show Axes: false Show Trail: false Value: true - upperleg_h_l: + Link Tree Style: Links in Alphabetic Order + RF_FOOT: Alpha: 1 Show Axes: false Show Trail: false Value: true - upperleg_h_r: + RF_HAA: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_HFE: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_HIP: + Alpha: 1 + Show Axes: false + Show Trail: false + RF_KFE: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_SHANK: + Alpha: 1 + Show Axes: false + Show Trail: false + RF_THIGH: + Alpha: 1 + Show Axes: false + Show Trail: false + RF_hip_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_shank_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_thigh_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_FOOT: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_HAA: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_HFE: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_HIP: + Alpha: 1 + Show Axes: false + Show Trail: false + RH_KFE: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_SHANK: + Alpha: 1 + Show Axes: false + Show Trail: false + RH_THIGH: + Alpha: 1 + Show Axes: false + Show Trail: false + RH_hip_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_shank_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_thigh_fixed: Alpha: 1 Show Axes: false Show Trail: false Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true base: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - body_bracket: + base_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + battery: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - hip_bracket_f_l: + bottom_shell: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - hip_bracket_f_r: + depth_camera_front_camera: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - hip_bracket_h_l: + depth_camera_front_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_front_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_front_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_front_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_left_camera: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - hip_bracket_h_r: + depth_camera_left_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_left_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_left_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_left_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_rear_camera: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - lowerleg_f_l: + depth_camera_rear_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_rear_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_rear_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_rear_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_right_camera: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - lowerleg_f_r: + depth_camera_right_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_right_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_right_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_right_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + docking_hatch_cover: + Alpha: 1 + Show Axes: false + Show Trail: false + face_front: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - lowerleg_h_l: + face_rear: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - lowerleg_h_r: + handle: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - pelvis: + hatch: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - torso: + lidar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lidar_cage: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - upperleg_f_l: + remote: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - upperleg_f_r: + top_shell: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - upperleg_h_l: + wide_angle_camera_front_camera: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - upperleg_h_r: + wide_angle_camera_front_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_angle_camera_rear_camera: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - Marker Alpha: 1 - Marker Scale: 0.30000001192092896 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - base: - pelvis: - body_bracket: - torso: - hip_bracket_f_l: - upperleg_f_l: - lowerleg_f_l: - {} - hip_bracket_f_r: - upperleg_f_r: - lowerleg_f_r: - {} - hip_bracket_h_l: - upperleg_h_l: - lowerleg_h_l: - {} - hip_bracket_h_r: - upperleg_h_r: - lowerleg_h_r: - {} + wide_angle_camera_rear_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" Update Interval: 0 Value: true + Visual Enabled: true Enabled: true Global Options: - Background Color: 211; 215; 207 - Default Light: true + Background Color: 48; 48; 48 Fixed Frame: base Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit - Distance: 1.0566951036453247 + Class: rviz_default_plugins/Orbit + Distance: 1.830826997756958 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 Focal Point: - X: 0 - Y: 0 - Z: 0 + X: 0.14730370044708252 + Y: 0.07969090342521667 + Z: -0.16050982475280762 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.330398291349411 + Pitch: 0.785398006439209 Target Frame: - Yaw: 0.4453873336315155 + Value: Orbit (rviz) + Yaw: 0.785398006439209 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 846 + Height: 736 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000282000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000015600000242fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000242000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000242000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004ba0000003efc0100000002fb0000000800540069006d00650100000000000004ba000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002490000024200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -278,6 +515,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1267 - X: 72 - Y: 27 \ No newline at end of file + Width: 1210 + X: 70 + Y: 27 diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/include/ocs2_anymal_models/AnymalModels.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/include/ocs2_anymal_models/AnymalModels.h index 61b9b523f..4e0ca1096 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/include/ocs2_anymal_models/AnymalModels.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/include/ocs2_anymal_models/AnymalModels.h @@ -19,6 +19,7 @@ std::string toString(AnymalModel model); AnymalModel stringToAnymalModel(const std::string& name); std::string getUrdfPath(AnymalModel model); +std::string getUrdfString(const std::string& urdfPath); std::string getUrdfString(AnymalModel model); std::unique_ptr getAnymalInverseKinematics(const FrameDeclaration& frameDeclaration, diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/launch/vizualize_urdf.launch.py b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/launch/vizualize_urdf.launch.py new file mode 100644 index 000000000..8aa44023a --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/launch/vizualize_urdf.launch.py @@ -0,0 +1,66 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + rviz_config_file = get_package_share_directory('ocs2_anymal_models') + "/config/visualize_urdf.rviz" + # 'anymal_c': + # get_package_share_directory('ocs2_robotic_assets') + "/resources/anymal_c/urdf/anymal.urdf" + # 'camel': + # get_package_share_directory('ocs2_anymal_models') + "/urdf/anymal_camel_rsl.urdf" + + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='robot_name', + default_value='anymal_c', + ), + launch.actions.DeclareLaunchArgument( + name='urdfFile', + default_value=get_package_share_directory('ocs2_robotic_assets') + "/resources/anymal_c/urdf/anymal.urdf" + ), + launch_ros.actions.Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[ + { + 'publish_frequency': 100.0 + }, + { + 'use_tf_static': True + } + ], + arguments=[launch.substitutions.LaunchConfiguration("urdfFile")], + ), + launch_ros.actions.Node( + package='joint_state_publisher_gui', + executable='joint_state_publisher_gui', + name='joint_state_publisher', + output='screen', + parameters=[ + { + 'use_gui': True + }, + { + 'rate': 100.0 + } + ] + ), + launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/package.xml b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/package.xml index cdc7ac229..edf60626e 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/package.xml +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/package.xml @@ -10,12 +10,15 @@ TODO - catkin + ament_cmake ocs2_switched_model_interface ocs2_pinocchio_interface ocs2_robotic_assets ocs2_robotic_tools - roslib + rclcpp + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/src/AnymalModels.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/src/AnymalModels.cpp index 3f4e6c493..425136836 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/src/AnymalModels.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_models/src/AnymalModels.cpp @@ -3,26 +3,25 @@ // #include - -#include - -#include - #include #include #include - #include +#include + +#include namespace anymal { std::string toString(AnymalModel model) { - static const std::unordered_map map{{AnymalModel::Camel, "camel"}}; + static const std::unordered_map map{ + {AnymalModel::Camel, "camel"}}; return map.at(model); } AnymalModel stringToAnymalModel(const std::string& name) { - static const std::unordered_map map{{"camel", AnymalModel::Camel}}; + static const std::unordered_map map{ + {"camel", AnymalModel::Camel}}; return map.at(name); } @@ -35,45 +34,61 @@ std::string getUrdfPath(AnymalModel model) { } } -std::string getUrdfString(AnymalModel model) { - const auto path = getUrdfPath(model); - std::ifstream stream(path.c_str()); +std::string getUrdfString(const std::string& urdfPath) { + std::ifstream stream(urdfPath.c_str()); if (!stream) { - throw std::runtime_error("File " + path + " does not exist"); + throw std::runtime_error("File " + urdfPath + " does not exist"); } - std::string xml_str((std::istreambuf_iterator(stream)), std::istreambuf_iterator()); + std::string xml_str((std::istreambuf_iterator(stream)), + std::istreambuf_iterator()); return xml_str; } -std::unique_ptr getAnymalInverseKinematics(const FrameDeclaration& frameDeclaration, - const std::string& urdf) { +std::string getUrdfString(AnymalModel model) { + const auto path = getUrdfPath(model); + return getUrdfString(path); +} + +std::unique_ptr +getAnymalInverseKinematics(const FrameDeclaration& frameDeclaration, + const std::string& urdf) { return std::unique_ptr( - new QuadrupedInverseKinematics(frameDeclaration, ocs2::getPinocchioInterfaceFromUrdfString(urdf))); + new QuadrupedInverseKinematics( + frameDeclaration, ocs2::getPinocchioInterfaceFromUrdfString(urdf))); } -std::unique_ptr> getAnymalKinematics(const FrameDeclaration& frameDeclaration, - const std::string& urdf) { +std::unique_ptr> +getAnymalKinematics(const FrameDeclaration& frameDeclaration, + const std::string& urdf) { return std::unique_ptr>( - new QuadrupedKinematics(frameDeclaration, ocs2::getPinocchioInterfaceFromUrdfString(urdf))); + new QuadrupedKinematics(frameDeclaration, + ocs2::getPinocchioInterfaceFromUrdfString(urdf))); } -std::unique_ptr> getAnymalKinematicsAd(const FrameDeclaration& frameDeclaration, - const std::string& urdf) { - return std::unique_ptr>( - new QuadrupedKinematicsAd(frameDeclaration, ocs2::getPinocchioInterfaceFromUrdfString(urdf))); +std::unique_ptr> +getAnymalKinematicsAd(const FrameDeclaration& frameDeclaration, + const std::string& urdf) { + return std::unique_ptr< + switched_model::KinematicsModelBase>( + new QuadrupedKinematicsAd( + frameDeclaration, ocs2::getPinocchioInterfaceFromUrdfString(urdf))); } -std::unique_ptr> getAnymalComModel(const FrameDeclaration& frameDeclaration, - const std::string& urdf) { +std::unique_ptr> getAnymalComModel( + const FrameDeclaration& frameDeclaration, const std::string& urdf) { return std::unique_ptr>( - new QuadrupedCom(frameDeclaration, createQuadrupedPinocchioInterfaceFromUrdfString(urdf))); + new QuadrupedCom(frameDeclaration, + createQuadrupedPinocchioInterfaceFromUrdfString(urdf))); } -std::unique_ptr> getAnymalComModelAd(const FrameDeclaration& frameDeclaration, - const std::string& urdf) { +std::unique_ptr> +getAnymalComModelAd(const FrameDeclaration& frameDeclaration, + const std::string& urdf) { return std::unique_ptr>( - new QuadrupedComAd(frameDeclaration, createQuadrupedPinocchioInterfaceFromUrdfString(urdf))); + new QuadrupedComAd( + frameDeclaration, + createQuadrupedPinocchioInterfaceFromUrdfString(urdf))); } } // namespace anymal diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/CMakeLists.txt index c60700daa..3c30ae591 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/CMakeLists.txt @@ -1,36 +1,29 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) +set(CMAKE_CXX_STANDARD 17) project(ocs2_anymal_mpc) -find_package(catkin REQUIRED COMPONENTS - roslib - ocs2_anymal_models - ocs2_quadruped_interface -) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(ocs2_anymal_models REQUIRED) +find_package(ocs2_quadruped_interface REQUIRED) ## Eigen3 find_package(Eigen3 3.3 REQUIRED NO_MODULE) find_package(Boost REQUIRED COMPONENTS + system filesystem + log_setup + log ) -# 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_anymal_models - ocs2_quadruped_interface - DEPENDS - Boost + + + +set(dependencies + ocs2_anymal_models + ocs2_quadruped_interface + Boost ) ########### @@ -40,93 +33,81 @@ catkin_package( include_directories( include ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} ) # Declare a C++ library add_library(${PROJECT_NAME} src/AnymalInterface.cpp ) -add_dependencies(${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) add_executable(${PROJECT_NAME}_mpc_node src/AnymalMpcNode.cpp ) -add_dependencies(${PROJECT_NAME}_mpc_node - ${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} +ament_target_dependencies(${PROJECT_NAME}_mpc_node + ${dependencies} ) target_link_libraries(${PROJECT_NAME}_mpc_node ${PROJECT_NAME} - ${catkin_LIBRARIES} ) add_executable(${PROJECT_NAME}_dummy_mrt_node src/AnymalDummyMRT.cpp ) -add_dependencies(${PROJECT_NAME}_dummy_mrt_node - ${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} +ament_target_dependencies(${PROJECT_NAME}_dummy_mrt_node + ${dependencies} ) target_link_libraries(${PROJECT_NAME}_dummy_mrt_node ${PROJECT_NAME} - ${catkin_LIBRARIES} ) -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if (cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_mpc_node ${PROJECT_NAME}_dummy_mrt_node - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -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}_mpc_node ${PROJECT_NAME}_dummy_mrt_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include/${PROJECT_NAME} ) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) install(DIRECTORY config launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME}/ +) +install(TARGETS + ${PROJECT_NAME}_mpc_node + ${PROJECT_NAME}_dummy_mrt_node + DESTINATION lib/${PROJECT_NAME} ) ############# ## Testing ## ############# +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + + # Build unit tests + ament_add_gtest(${PROJECT_NAME}_test + test/testProblemFormulation.cpp + ) + ament_target_dependencies(${PROJECT_NAME}_test + ${dependencies} + ) + target_link_libraries(${PROJECT_NAME}_test + ${PROJECT_NAME} + ) + set_tests_properties(${PROJECT_NAME}_test PROPERTIES TIMEOUT 1200) +endif() -catkin_add_nosetests(test) - -# Build unit tests -catkin_add_gtest(${PROJECT_NAME}_test - test/testProblemFormulation.cpp -) -target_link_libraries(${PROJECT_NAME}_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main -) +ament_export_dependencies(${dependencies}) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/launch/camel.launch.py b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/launch/camel.launch.py new file mode 100644 index 000000000..1c6d5eb52 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/launch/camel.launch.py @@ -0,0 +1,33 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + +def load_file_content(file_path): + with open(file_path, 'r') as file: + return file.read() + +def generate_launch_description(): + target_command = get_package_share_directory('ocs2_anymal_mpc') + "/config/c_series/targetCommand.info" + description_name = get_package_share_directory('ocs2_anymal_models') + "/urdf/anymal_camel_rsl.urdf" + ld = launch.LaunchDescription([ + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_anymal_mpc'), 'launch/mpc.launch.py') + ), + launch_arguments={ + 'robot_name': 'camel', + 'config_name': 'c_series', + 'description_name': description_name, + 'target_command': target_command + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/launch/mpc.launch.py b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/launch/mpc.launch.py new file mode 100644 index 000000000..ce54af32f --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/launch/mpc.launch.py @@ -0,0 +1,98 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='robot_name' + ), + launch.actions.DeclareLaunchArgument( + name='config_name', + default_value='' + ), + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), + launch.actions.DeclareLaunchArgument( + name='target_command', + default_value='' + ), + launch.actions.DeclareLaunchArgument( + name='description_name', + default_value='ocs2_anymal_description' + ), + launch.actions.DeclareLaunchArgument( + name='urdf_model_path', + default_value=get_package_share_directory('ocs2_robotic_assets') + "/resources/anymal_c/urdf/anymal.urdf" + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_quadruped_interface'), 'launch/visualization.launch.py') + ), + launch_arguments={ + 'description_name': LaunchConfiguration('description_name'), + }.items() + ), + launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name='robot_state_publisher', + output='screen', + arguments=[LaunchConfiguration("urdf_model_path")], + ), + launch_ros.actions.Node( + package='ocs2_anymal_mpc', + executable='ocs2_anymal_mpc_mpc_node', + name='ocs2_anymal_mpc_mpc_node', + prefix="", + arguments=[LaunchConfiguration('description_name'), LaunchConfiguration('config_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_anymal_mpc', + executable='ocs2_anymal_mpc_dummy_mrt_node', + name='ocs2_anymal_mpc_dummy_mrt_node', + prefix=LaunchConfiguration('terminal_prefix'), + arguments=[LaunchConfiguration('description_name'), LaunchConfiguration('config_name')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_anymal_commands', + executable='gait_command_node', + name='gait_command_node', + prefix=LaunchConfiguration('terminal_prefix'), + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_anymal_commands', + executable='target_command_node', + name='target_command_node', + prefix=LaunchConfiguration('terminal_prefix'), + arguments=[LaunchConfiguration('target_command')], + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_anymal_commands', + executable='motion_command_node', + name='motion_command_node', + prefix=LaunchConfiguration('terminal_prefix'), + arguments=['dummy'], + output='screen' + ), + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/package.xml b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/package.xml index f52ef3d87..696a36fa3 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/package.xml +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/package.xml @@ -8,12 +8,17 @@ TODO - catkin + ament_cmake - roslib + rclcpp ocs2_anymal_models ocs2_quadruped_interface ocs2_anymal_commands + ament_cmake_gtest + ament_cmake_pytest + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalDummyMRT.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalDummyMRT.cpp index 7a0554f83..59e77eaaa 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalDummyMRT.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalDummyMRT.cpp @@ -5,31 +5,34 @@ * Author: farbod */ +#include #include -#include -#include #include "ocs2_anymal_mpc/AnymalInterface.h" +#include "rclcpp/rclcpp.hpp" int main(int argc, char* argv[]) { - std::vector programArgs{}; - ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() < 3) { - throw std::runtime_error("No robot name and config folder specified. Aborting."); + throw std::runtime_error( + "No robot name and config folder specified. Aborting."); } - const std::string descriptionName(programArgs[1]); + const std::string urdfPath(programArgs[1]); const std::string configName(programArgs[2]); // Initialize ros node - ros::init(argc, argv, "anymal_mrt"); - ros::NodeHandle nodeHandle; - - std::string urdfString; - nodeHandle.getParam(descriptionName, urdfString); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("anymal_mrt"); + std::string urdfString = anymal::getUrdfString(urdfPath); - auto anymalInterface = anymal::getAnymalInterface(urdfString, anymal::getConfigFolder(configName)); - const auto mpcSettings = ocs2::mpc::loadSettings(anymal::getTaskFilePath(configName)); - quadrupedDummyNode(nodeHandle, *anymalInterface, &anymalInterface->getRollout(), mpcSettings.mrtDesiredFrequency_, + auto anymalInterface = anymal::getAnymalInterface( + urdfString, anymal::getConfigFolder(configName)); + const auto mpcSettings = + ocs2::mpc::loadSettings(anymal::getTaskFilePath(configName)); + quadrupedDummyNode(node, *anymalInterface, &anymalInterface->getRollout(), + mpcSettings.mrtDesiredFrequency_, mpcSettings.mpcDesiredFrequency_); return 0; diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalInterface.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalInterface.cpp index 872999cec..69d8e7281 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalInterface.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalInterface.cpp @@ -4,8 +4,8 @@ #include "ocs2_anymal_mpc/AnymalInterface.h" -#include - +#include "rclcpp/rclcpp.hpp" +#include #include namespace anymal { @@ -36,7 +36,7 @@ std::unique_ptr getAnymalInterface(const std } std::string getConfigFolder(const std::string& configName) { - return ros::package::getPath("ocs2_anymal_mpc") + "/config/" + configName; + return ament_index_cpp::get_package_share_directory("ocs2_anymal_mpc") + "/config/" + configName; } std::string getTaskFilePath(const std::string& configName) { diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalMpcNode.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalMpcNode.cpp index e57519b1a..a16f4b943 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalMpcNode.cpp @@ -5,45 +5,48 @@ * Author: farbod */ -#include -#include - #include #include #include +#include #include "ocs2_anymal_mpc/AnymalInterface.h" +#include "rclcpp/rclcpp.hpp" int main(int argc, char* argv[]) { - std::vector programArgs{}; - ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() < 3) { - throw std::runtime_error("No robot name and config folder specified. Aborting."); + throw std::runtime_error( + "No robot name and config folder specified. Aborting."); } - const std::string descriptionName(programArgs[1]); + const std::string urdfPath(programArgs[1]); const std::string configName(programArgs[2]); // Initialize ros node - ros::init(argc, argv, "anymal_mpc"); - ros::NodeHandle nodeHandle; - - std::string urdfString; - nodeHandle.getParam(descriptionName, urdfString); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("anymal_mpc"); - auto anymalInterface = anymal::getAnymalInterface(urdfString, anymal::getConfigFolder(configName)); - const auto mpcSettings = ocs2::mpc::loadSettings(anymal::getTaskFilePath(configName)); + std::string urdfString = anymal::getUrdfString(urdfPath); + auto anymalInterface = anymal::getAnymalInterface( + urdfString, anymal::getConfigFolder(configName)); + const auto mpcSettings = + ocs2::mpc::loadSettings(anymal::getTaskFilePath(configName)); switch (anymalInterface->modelSettings().algorithm_) { case switched_model::Algorithm::DDP: { - const auto ddpSettings = ocs2::ddp::loadSettings(anymal::getTaskFilePath(configName)); + const auto ddpSettings = + ocs2::ddp::loadSettings(anymal::getTaskFilePath(configName)); auto mpcPtr = getDdpMpc(*anymalInterface, mpcSettings, ddpSettings); - quadrupedMpcNode(nodeHandle, *anymalInterface, std::move(mpcPtr)); + quadrupedMpcNode(node, *anymalInterface, std::move(mpcPtr)); break; } case switched_model::Algorithm::SQP: { - const auto sqpSettings = ocs2::sqp::loadSettings(anymal::getConfigFolder(configName) + "/multiple_shooting.info"); + const auto sqpSettings = ocs2::sqp::loadSettings( + anymal::getConfigFolder(configName) + "/multiple_shooting.info"); auto mpcPtr = getSqpMpc(*anymalInterface, mpcSettings, sqpSettings); - quadrupedMpcNode(nodeHandle, *anymalInterface, std::move(mpcPtr)); + quadrupedMpcNode(node, *anymalInterface, std::move(mpcPtr)); break; } } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/test/testProblemFormulation.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/test/testProblemFormulation.cpp index c95dd0072..a8be563b3 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/test/testProblemFormulation.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/test/testProblemFormulation.cpp @@ -44,7 +44,7 @@ TEST_F(TestAnymalModel, all) { const ocs2::vector_t x = anymalInterface->getInitialState(); const ocs2::vector_t u = ocs2::vector_t::Zero(24); ocs2::benchmark::RepeatedTimer timer; - int N = 100000; + int N = 10000; timer.startTimer(); for (int i = 0; i < N; i++) { @@ -76,7 +76,7 @@ TEST_F(TestAnymalModel, dynamics) { const ocs2::vector_t x = anymalInterface->getInitialState(); const ocs2::vector_t u = ocs2::vector_t::Zero(24); ocs2::benchmark::RepeatedTimer timer; - int N = 100000; + int N = 10000; timer.startTimer(); for (int i = 0; i < N; i++) { @@ -91,7 +91,7 @@ TEST_F(TestAnymalModel, precomputation) { const ocs2::vector_t x = anymalInterface->getInitialState(); const ocs2::vector_t u = ocs2::vector_t::Zero(24); ocs2::benchmark::RepeatedTimer timer; - int N = 100000; + int N = 10000; constexpr auto request = ocs2::Request::Cost + ocs2::Request::SoftConstraint + ocs2::Request::Constraint + ocs2::Request::Approximation; @@ -108,7 +108,7 @@ TEST_F(TestAnymalModel, constraints_eq) { const ocs2::vector_t x = anymalInterface->getInitialState(); const ocs2::vector_t u = ocs2::vector_t::Zero(24); ocs2::benchmark::RepeatedTimer timer; - int N = 1000000; + int N = 10000; constexpr auto request = ocs2::Request::Constraint + ocs2::Request::Approximation; problem.preComputationPtr->request(request, t, x, u); @@ -126,7 +126,7 @@ TEST_F(TestAnymalModel, cost) { const ocs2::vector_t x = anymalInterface->getInitialState(); const ocs2::vector_t u = ocs2::vector_t::Zero(24); ocs2::benchmark::RepeatedTimer timer; - int N = 100000; + int N = 10000; constexpr auto request = ocs2::Request::Cost + ocs2::Request::SoftConstraint + ocs2::Request::Constraint + ocs2::Request::Approximation; problem.preComputationPtr->request(request, t, x, u); @@ -144,7 +144,7 @@ TEST_F(TestAnymalModel, terminalCost) { const ocs2::vector_t x = anymalInterface->getInitialState(); const ocs2::vector_t u = ocs2::vector_t::Zero(24); ocs2::benchmark::RepeatedTimer timer; - int N = 100000; + int N = 10000; constexpr auto request = ocs2::Request::Cost + ocs2::Request::SoftConstraint + ocs2::Request::Approximation; problem.preComputationPtr->requestFinal(request, t, x); diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/CMakeLists.txt index 359aaef44..f14f0aef0 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/CMakeLists.txt @@ -1,8 +1,9 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) +set(CMAKE_CXX_STANDARD 17) project(ocs2_quadruped_interface) -## Catkin Dependencies -set(CATKIN_PACKAGE_DEPENDENCIES +## ament_cmake Dependencies +set(dependencies ocs2_core ocs2_ddp ocs2_mpc @@ -11,36 +12,41 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_robotic_tools ocs2_switched_model_interface ocs2_anymal_commands - roscpp - tf + rclcpp + tf2 kdl_parser robot_state_publisher segmented_planes_terrain_model ) -find_package(catkin REQUIRED COMPONENTS - ${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_sqp REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_switched_model_interface REQUIRED) +find_package(ocs2_anymal_commands REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2 REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(robot_state_publisher REQUIRED) +find_package(segmented_planes_terrain_model REQUIRED) +find_package(PCL REQUIRED) ## Eigen3 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 - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS +find_package(Boost REQUIRED COMPONENTS + system + filesystem + log_setup + log ) + + + ########### ## Build ## ########### @@ -48,7 +54,7 @@ catkin_package( include_directories( include ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} ) # Declare a C++ library @@ -65,38 +71,29 @@ add_library(${PROJECT_NAME} src/TerrainPlaneVisualizer.cpp src/TerrainReceiver.cpp ) -add_dependencies(${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -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 + INCLUDES DESTINATION include/${PROJECT_NAME} ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) +install(DIRECTORY config launch + DESTINATION share/${PROJECT_NAME}/ ) + +ament_export_dependencies(${dependencies}) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/config/config.rviz b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/config/config.rviz index cb2b630b9..c2be4140c 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/config/config.rviz +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/config/config.rviz @@ -1,5 +1,5 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 138 Name: Displays Property Tree Widget: @@ -9,16 +9,16 @@ Panels: - /Optimized Trajectory1/Feet Pose Trajectories1 Splitter Ratio: 0.472075879573822 Tree Height: 1682 - - Class: rviz/Selection + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 - /CoGTrajectory1 @@ -26,7 +26,7 @@ Panels: - /video_side1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 @@ -40,7 +40,7 @@ Visualization Manager: Displays: - Alpha: 0.5 Cell Size: 0.10000000149011612 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -57,7 +57,7 @@ Visualization Manager: Reference Frame: world Value: true - Alpha: 1 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false Enabled: true Links: @@ -430,9 +430,9 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true - - Class: rviz/Group + - Class: rviz_common/Group Displays: - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /ocs2_anymal/desiredBaseTrajectory Name: Base Position Trajectory @@ -440,7 +440,7 @@ Visualization Manager: "": true Queue Size: 1 Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredVelTrajectory Name: Base Velocity Trajectory @@ -448,7 +448,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredAngVelTrajectory Name: Base Angular Velocity Trajectory @@ -460,7 +460,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -473,9 +473,9 @@ Visualization Manager: Topic: ocs2_anymal/desiredPoseTrajectory Unreliable: false Value: true - - Class: rviz/Group + - Class: rviz_common/Group Displays: - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /ocs2_anymal/desiredFeetTrajectory/LF Name: LF @@ -483,7 +483,7 @@ Visualization Manager: {} Queue Size: 1 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /ocs2_anymal/desiredFeetTrajectory/RF Name: RF @@ -491,7 +491,7 @@ Visualization Manager: {} Queue Size: 1 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /ocs2_anymal/desiredFeetTrajectory/LH Name: LH @@ -499,7 +499,7 @@ Visualization Manager: {} Queue Size: 1 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /ocs2_anymal/desiredFeetTrajectory/RH Name: RH @@ -511,7 +511,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -524,7 +524,7 @@ Visualization Manager: Topic: /ocs2_anymal/swing_planner/nominalFootholds_LF Unreliable: false Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/swing_planner/trajectory_LF Name: ref trajectory LF @@ -534,9 +534,9 @@ Visualization Manager: Value: false Enabled: false Name: Feet Position Trajectories - - Class: rviz/Group + - Class: rviz_common/Group Displays: - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredFeetVelTrajectory/LF Name: LF @@ -544,7 +544,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredFeetVelTrajectory/RF Name: RF @@ -552,7 +552,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredFeetVelTrajectory/LH Name: LH @@ -560,7 +560,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/desiredFeetVelTrajectory/RH Name: RH @@ -570,13 +570,13 @@ Visualization Manager: Value: false Enabled: false Name: Feet Velocity Trajectories - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 1 Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -593,7 +593,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -610,7 +610,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -627,7 +627,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -644,13 +644,13 @@ Visualization Manager: Name: Feet Pose Trajectories Enabled: true Name: Desired Trajectory - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 1 Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: false Head Length: 0.07000000029802322 @@ -663,7 +663,7 @@ Visualization Manager: Topic: ocs2_anymal/optimizedPoseTrajectory Unreliable: false Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: /ocs2_anymal/optimizedStateTrajectory Name: State Trajectory @@ -673,13 +673,13 @@ Visualization Manager: Future footholds: true Queue Size: 1 Value: true - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 1 Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -696,7 +696,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -713,7 +713,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -730,7 +730,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.05000000074505806 Axes Radius: 0.004999999888241291 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -747,13 +747,13 @@ Visualization Manager: Name: Feet Pose Trajectories Enabled: true Name: Optimized Trajectory - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 1 Arrow Length: 0.30000001192092896 Axes Length: 0.10000000149011612 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 @@ -766,7 +766,7 @@ Visualization Manager: Topic: ocs2_anymal/currentPose Unreliable: false Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: /ocs2_anymal/currentState Name: State @@ -781,7 +781,7 @@ Visualization Manager: Arrow Length: 0.30000001192092896 Axes Length: 0.10000000149011612 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: false Head Length: 0.07000000029802322 @@ -794,7 +794,7 @@ Visualization Manager: Topic: /ocs2_anymal/currentFeetPoses Unreliable: false Value: false - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: false Marker Topic: /ocs2_anymal/localTerrain Name: LocalTerrain @@ -802,7 +802,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /ocs2_anymal/currentCollisionSpheres Name: Collisions @@ -812,7 +812,7 @@ Visualization Manager: Value: false Enabled: true Name: Current - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 1 Autocompute Intensity Bounds: true @@ -822,7 +822,7 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 @@ -876,26 +876,26 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + - Class: rviz_default_plugins/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - - Class: rviz/SetGoal + - Class: rviz_default_plugins/SetGoal Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: - Class: rviz/ThirdPersonFollower + Class: rviz_default_plugins/ThirdPersonFollower Distance: 2.5349531173706055 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -917,7 +917,7 @@ Visualization Manager: Yaw: 0.857906699180603 Saved: - Angle: 3.140000104904175 - Class: rviz/TopDownOrtho + Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -931,7 +931,7 @@ Visualization Manager: X: 0.16045600175857544 Y: 0.09311659634113312 - Angle: 6.260000228881836 - Class: rviz/TopDownOrtho + Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -944,7 +944,7 @@ Visualization Manager: Target Frame: X: 2.279409885406494 Y: -0.7245129942893982 - - Class: rviz/Orbit + - Class: rviz_default_plugins/Orbit Distance: 3.5650899410247803 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -964,7 +964,7 @@ Visualization Manager: Pitch: 0.434798002243042 Target Frame: Yaw: 5.150000095367432 - - Class: rviz/Orbit + - Class: rviz_default_plugins/Orbit Distance: 1.7256799936294556 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedDummyNode.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedDummyNode.h index f00d5f6a5..0efcb9a7f 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedDummyNode.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedDummyNode.h @@ -4,12 +4,12 @@ #pragma once -#include +#include "rclcpp/rclcpp.hpp" #include "QuadrupedInterface.h" namespace switched_model { -void quadrupedDummyNode(ros::NodeHandle& nodeHandle, const QuadrupedInterface& quadrupedInterface, const ocs2::RolloutBase* rolloutPtr, +void quadrupedDummyNode(const rclcpp::Node::SharedPtr &node, const QuadrupedInterface& quadrupedInterface, const ocs2::RolloutBase* rolloutPtr, double mrtDesiredFrequency, double mpcDesiredFrequency); } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedMpcNode.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedMpcNode.h index 19475a982..b76ec2b2d 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedMpcNode.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedMpcNode.h @@ -4,7 +4,7 @@ #pragma once -#include +#include "rclcpp/rclcpp.hpp" #include @@ -12,5 +12,5 @@ namespace switched_model { -void quadrupedMpcNode(ros::NodeHandle& nodeHandle, const QuadrupedInterface& quadrupedInterface, std::unique_ptr mpcPtr); +void quadrupedMpcNode(const rclcpp::Node::SharedPtr &node, const QuadrupedInterface& quadrupedInterface, std::unique_ptr mpcPtr); } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedTfPublisher.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedTfPublisher.h index fab684845..7d41b5b27 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedTfPublisher.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedTfPublisher.h @@ -4,11 +4,13 @@ #pragma once -#include -#include -#include - #include +#include + +#include +#include + +#include "rclcpp/rclcpp.hpp" namespace switched_model { @@ -16,29 +18,35 @@ class QuadrupedTfPublisher { public: QuadrupedTfPublisher() = default; - void launchNode(ros::NodeHandle& nodeHandle, const std::string& descriptionName, std::vector jointNames, - std::string baseName, const std::string& tfPrefix = ""); + void launchNode(const rclcpp::Node::SharedPtr& node, + const std::string& descriptionName, + std::vector jointNames, std::string baseName, + const std::string& tfPrefix = ""); - void publish(ros::Time timeStamp, const vector_t& state, const std::string& worldFrame); + void publish(rclcpp::Time timeStamp, const vector_t& state, + const std::string& worldFrame); - void publish(ros::Time timeStamp, const base_coordinate_t& basePose, const joint_coordinate_t& jointPositions, + void publish(rclcpp::Time timeStamp, const base_coordinate_t& basePose, + const joint_coordinate_t& jointPositions, const std::string& worldFrame); private: void updateJointPositions(const joint_coordinate_t& jointPositions); - void updateBasePose(ros::Time timeStamp, const base_coordinate_t& basePose, const std::string& worldFrame); + void updateBasePose(rclcpp::Time timeStamp, const base_coordinate_t& basePose, + const std::string& worldFrame); + rclcpp::Node::SharedPtr node_; // Publishers - tf::TransformBroadcaster tfBroadcaster_; - std::unique_ptr robotStatePublisherPtr_; + std::unique_ptr tfBroadcaster_; + rclcpp::Publisher::SharedPtr jointPublisher_; // Messages std::string tfPrefix_ = ""; std::string baseName_; std::vector jointNames_; std::map jointPositionsMap_; - geometry_msgs::TransformStamped baseToWorldTransform_; - ros::Time lastTimeStamp_ = ros::Time::now(); + geometry_msgs::msg::TransformStamped baseToWorldTransform_; + rclcpp::Time lastTimeStamp_; }; } // namespace switched_model \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedVisualizer.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedVisualizer.h index 5a533279d..10a607d3e 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedVisualizer.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedVisualizer.h @@ -4,17 +4,20 @@ #pragma once -#include -#include -#include - +#include +#include #include #include +#include -#include -#include +#include #include "ocs2_quadruped_interface/QuadrupedTfPublisher.h" +#include "rclcpp/rclcpp.hpp" + +// Additional messages not in the helpers file +#include +#include namespace switched_model { @@ -23,16 +26,19 @@ class QuadrupedVisualizer : public ocs2::DummyObserver { using kinematic_model_t = KinematicsModelBase; /** Visualization settings (publicly available) */ - std::string frameId_ = "world"; // Frame name all messages are published in - scalar_t footMarkerDiameter_ = 0.03; // Size of the spheres at the feet - scalar_t footAlphaWhenLifted_ = 0.3; // Alpha value when a foot is lifted. - scalar_t forceScale_ = 1000.0; // Vector scale in N/m - scalar_t velScale_ = 20.0; // Vector scale in m/s - scalar_t copMarkerDiameter_ = 0.03; // Size of the sphere at the center of pressure - scalar_t supportPolygonLineWidth_ = 0.005; // LineThickness for the support polygon - scalar_t trajectoryLineWidth_ = 0.01; // LineThickness for trajectories - feet_array_t feetColorMap_ = {ocs2::Color::blue, ocs2::Color::orange, ocs2::Color::yellow, - ocs2::Color::purple}; // Colors for markers per feet + std::string frameId_ = "world"; // Frame name all messages are published in + scalar_t footMarkerDiameter_ = 0.03; // Size of the spheres at the feet + scalar_t footAlphaWhenLifted_ = 0.3; // Alpha value when a foot is lifted. + scalar_t forceScale_ = 1000.0; // Vector scale in N/m + scalar_t velScale_ = 20.0; // Vector scale in m/s + scalar_t copMarkerDiameter_ = + 0.03; // Size of the sphere at the center of pressure + scalar_t supportPolygonLineWidth_ = + 0.005; // LineThickness for the support polygon + scalar_t trajectoryLineWidth_ = 0.01; // LineThickness for trajectories + feet_array_t feetColorMap_ = { + ocs2::Color::blue, ocs2::Color::orange, ocs2::Color::yellow, + ocs2::Color::purple}; // Colors for markers per feet /** * @@ -41,57 +47,78 @@ class QuadrupedVisualizer : public ocs2::DummyObserver { * @param n * @param maxUpdateFrequency : maximum publish frequency measured in MPC time. */ - QuadrupedVisualizer(const kinematic_model_t& kinematicModel, std::vector jointNames, std::string baseName, - ros::NodeHandle& n, scalar_t maxUpdateFrequency = 1000.0) - : kinematicModelPtr_(kinematicModel.clone()), - lastTime_(std::numeric_limits::lowest()), - minPublishTimeDifference_(1.0 / maxUpdateFrequency) { - launchVisualizerNode(n, std::move(jointNames), std::move(baseName)); - }; + QuadrupedVisualizer(const kinematic_model_t& kinematicModel, + std::vector jointNames, std::string baseName, + const rclcpp::Node::SharedPtr& node, + scalar_t maxUpdateFrequency = 1000.0); ~QuadrupedVisualizer() override = default; - void update(const ocs2::SystemObservation& observation, const ocs2::PrimalSolution& primalSolution, + void update(const ocs2::SystemObservation& observation, + const ocs2::PrimalSolution& primalSolution, const ocs2::CommandData& command) override; - void launchVisualizerNode(ros::NodeHandle& nodeHandle, std::vector jointNames, std::string baseName); + void publishTrajectory( + const std::vector& system_observation_array, + scalar_t speed = 1.0); - void publishTrajectory(const std::vector& system_observation_array, scalar_t speed = 1.0); + void publishObservation(rclcpp::Time timeStamp, + const ocs2::SystemObservation& observation); - void publishObservation(ros::Time timeStamp, const ocs2::SystemObservation& observation); + void publishDesiredTrajectory( + rclcpp::Time timeStamp, + const ocs2::TargetTrajectories& targetTrajectories) const; - void publishDesiredTrajectory(ros::Time timeStamp, const ocs2::TargetTrajectories& targetTrajectories) const; - - void publishOptimizedStateTrajectory(ros::Time timeStamp, const scalar_array_t& mpcTimeTrajectory, - const vector_array_t& mpcStateTrajectory, const ocs2::ModeSchedule& modeSchedule) const; + void publishOptimizedStateTrajectory( + rclcpp::Time timeStamp, const scalar_array_t& mpcTimeTrajectory, + const vector_array_t& mpcStateTrajectory, + const ocs2::ModeSchedule& modeSchedule) const; private: - void publishCartesianMarkers(ros::Time timeStamp, const contact_flag_t& contactFlags, const feet_array_t& feetPosition, + void publishCartesianMarkers(rclcpp::Time timeStamp, + const contact_flag_t& contactFlags, + const feet_array_t& feetPosition, const feet_array_t& feetForce) const; - void publishEndEffectorPoses(ros::Time timeStamp, const feet_array_t& feetPositions, - const feet_array_t>& feetOrientations) const; - void publishCollisionSpheres(ros::Time timeStamp, const base_coordinate_t& basePose, const joint_coordinate_t& jointAngles) const; - + void publishEndEffectorPoses( + rclcpp::Time timeStamp, const feet_array_t& feetPositions, + const feet_array_t>& feetOrientations) const; + void publishCollisionSpheres(rclcpp::Time timeStamp, + const base_coordinate_t& basePose, + const joint_coordinate_t& jointAngles) const; + + rclcpp::Node::SharedPtr node_; std::unique_ptr kinematicModelPtr_; QuadrupedTfPublisher quadrupedTfPublisher_; // Cost desired publisher. - ros::Publisher costDesiredBasePositionPublisher_; - ros::Publisher costDesiredBasePosePublisher_; - ros::Publisher costDesiredBaseVelocityPublisher_; - ros::Publisher costDesiredBaseAngVelocityPublisher_; - feet_array_t costDesiredFeetPositionPublishers_; - feet_array_t costDesiredFeetVelocityPublishers_; + rclcpp::Publisher::SharedPtr + costDesiredBasePositionPublisher_; + rclcpp::Publisher::SharedPtr + costDesiredBasePosePublisher_; + rclcpp::Publisher::SharedPtr + costDesiredBaseVelocityPublisher_; + rclcpp::Publisher::SharedPtr + costDesiredBaseAngVelocityPublisher_; + feet_array_t::SharedPtr> + costDesiredFeetPositionPublishers_; + feet_array_t< + rclcpp::Publisher::SharedPtr> + costDesiredFeetVelocityPublishers_; // State optimized publisher. - ros::Publisher stateOptimizedPublisher_; - ros::Publisher stateOptimizedPosePublisher_; + rclcpp::Publisher::SharedPtr + stateOptimizedPublisher_; + rclcpp::Publisher::SharedPtr + stateOptimizedPosePublisher_; // Current state publisher.. - ros::Publisher currentStatePublisher_; - ros::Publisher currentFeetPosesPublisher_; - ros::Publisher currentCollisionSpheresPublisher_; + rclcpp::Publisher::SharedPtr + currentStatePublisher_; + rclcpp::Publisher::SharedPtr + currentFeetPosesPublisher_; + rclcpp::Publisher::SharedPtr + currentCollisionSpheresPublisher_; scalar_t lastTime_; scalar_t minPublishTimeDifference_; diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/SwingPlanningVisualizer.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/SwingPlanningVisualizer.h index c53234bc4..113055fcb 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/SwingPlanningVisualizer.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/SwingPlanningVisualizer.h @@ -1,16 +1,14 @@ -// -// Created by rgrandia on 30.04.20. -// - #pragma once -#include -#include - #include #include +#include +#include + #include "ocs2_switched_model_interface/foot_planner/SwingTrajectoryPlanner.h" +#include "rclcpp/publisher.hpp" +#include "rclcpp/rclcpp.hpp" namespace switched_model { @@ -18,21 +16,29 @@ class SwingPlanningVisualizer : public ocs2::SolverSynchronizedModule { public: /** Visualization settings (publicly available) */ std::string frameId_ = "world"; // Frame name all messages are published in - double arrowScale = 0.05; // Size of the arrow representing the velocity vector - switched_model::feet_array_t feetColorMap_ = {ocs2::Color::blue, ocs2::Color::orange, ocs2::Color::yellow, - ocs2::Color::purple}; // Colors for markers per feet + double arrowScale = + 0.05; // Size of the arrow representing the velocity vector + switched_model::feet_array_t feetColorMap_ = { + ocs2::Color::blue, ocs2::Color::orange, ocs2::Color::yellow, + ocs2::Color::purple}; // Colors for markers per feet - SwingPlanningVisualizer(const SwingTrajectoryPlanner& swingTrajectoryPlanner, ros::NodeHandle& nodeHandle); + SwingPlanningVisualizer(const SwingTrajectoryPlanner& swingTrajectoryPlanner, + const rclcpp::Node::SharedPtr& node); - void preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, - const ocs2::ReferenceManagerInterface& referenceManager) override; + void preSolverRun( + scalar_t initTime, scalar_t finalTime, const vector_t& currentState, + const ocs2::ReferenceManagerInterface& referenceManager) override; void postSolverRun(const ocs2::PrimalSolution& primalSolution) override{}; private: + rclcpp::Node::SharedPtr node_; const SwingTrajectoryPlanner* swingTrajectoryPlannerPtr_; - feet_array_t nominalFootholdPublishers_; - feet_array_t swingTrajectoryPublishers_; + feet_array_t::SharedPtr> + nominalFootholdPublishers_; + feet_array_t< + rclcpp::Publisher::SharedPtr> + swingTrajectoryPublishers_; }; } // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/TerrainPlaneVisualizer.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/TerrainPlaneVisualizer.h index 3fe30f930..251c8fae0 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/TerrainPlaneVisualizer.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/TerrainPlaneVisualizer.h @@ -4,17 +4,18 @@ #pragma once -#include -#include - #include #include - #include #include #include #include +#include + +#include "rclcpp/publisher.hpp" +#include "rclcpp/rclcpp.hpp" + namespace switched_model { class TerrainPlaneVisualizer { @@ -26,20 +27,26 @@ class TerrainPlaneVisualizer { double planeThickness_ = 0.005; double planeAlpha_ = 0.5; - explicit TerrainPlaneVisualizer(ros::NodeHandle& nodeHandle); + explicit TerrainPlaneVisualizer(const rclcpp::Node::SharedPtr& node); void update(scalar_t time, const TerrainPlane& terrainPlane); private: - ros::Publisher terrainPublisher_; + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr + terrainPublisher_; }; -class TerrainPlaneVisualizerSynchronizedModule : public ocs2::SolverSynchronizedModule { +class TerrainPlaneVisualizerSynchronizedModule + : public ocs2::SolverSynchronizedModule { public: - TerrainPlaneVisualizerSynchronizedModule(const SwingTrajectoryPlanner& swingTrajectoryPlanner, ros::NodeHandle& nodeHandle); + TerrainPlaneVisualizerSynchronizedModule( + const SwingTrajectoryPlanner& swingTrajectoryPlanner, + const rclcpp::Node::SharedPtr& node); - void preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, - const ocs2::ReferenceManagerInterface& referenceManager) override{}; + void preSolverRun( + scalar_t initTime, scalar_t finalTime, const vector_t& currentState, + const ocs2::ReferenceManagerInterface& referenceManager) override{}; void postSolverRun(const ocs2::PrimalSolution& primalSolution) override; diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/TerrainReceiver.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/TerrainReceiver.h index 14cca9053..f3944e3d3 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/TerrainReceiver.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/TerrainReceiver.h @@ -4,8 +4,8 @@ #pragma once -#include -#include +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/publisher.hpp" #include #include @@ -18,7 +18,7 @@ namespace switched_model { class TerrainReceiverSynchronizedModule : public ocs2::SolverSynchronizedModule { public: - TerrainReceiverSynchronizedModule(ocs2::Synchronized& terrainModel, ros::NodeHandle& nodeHandle); + TerrainReceiverSynchronizedModule(ocs2::Synchronized& terrainModel, const rclcpp::Node::SharedPtr &node); ~TerrainReceiverSynchronizedModule() override = default; void preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/launch/visualization.launch.py b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/launch/visualization.launch.py new file mode 100644 index 000000000..7c0b8659d --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/launch/visualization.launch.py @@ -0,0 +1,29 @@ +import os +import sys + +import launch +import launch_ros.actions +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + rviz_config_file = get_package_share_directory('ocs2_quadruped_interface') + "/config/config.rviz" + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='description_name', + default_value='' + ), + launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/package.xml b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/package.xml index 0bc4d3e6d..0bbbb44f2 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/package.xml +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/package.xml @@ -10,7 +10,7 @@ TODO - catkin + ament_cmake ocs2_core ocs2_ddp @@ -21,10 +21,13 @@ ocs2_robotic_tools ocs2_switched_model_interface ocs2_anymal_commands - roscpp - tf + rclcpp + tf2 kdl_parser robot_state_publisher segmented_planes_terrain_model + + ament_cmake + \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedDummyNode.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedDummyNode.cpp index 5920123a8..b244bce10 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedDummyNode.cpp @@ -14,7 +14,7 @@ namespace switched_model { -void quadrupedDummyNode(ros::NodeHandle& nodeHandle, const QuadrupedInterface& quadrupedInterface, const ocs2::RolloutBase* rolloutPtr, +void quadrupedDummyNode(const rclcpp::Node::SharedPtr &node, const QuadrupedInterface& quadrupedInterface, const ocs2::RolloutBase* rolloutPtr, double mrtDesiredFrequency, double mpcDesiredFrequency) { const std::string robotName = "anymal"; @@ -23,11 +23,11 @@ void quadrupedDummyNode(ros::NodeHandle& nodeHandle, const QuadrupedInterface& q if (rolloutPtr != nullptr) { mrt.initRollout(rolloutPtr); } - mrt.launchNodes(nodeHandle); + mrt.launchNodes(node); // Visualization auto visualizer = std::make_shared( - quadrupedInterface.getKinematicModel(), quadrupedInterface.getJointNames(), quadrupedInterface.getBaseName(), nodeHandle); + quadrupedInterface.getKinematicModel(), quadrupedInterface.getJointNames(), quadrupedInterface.getBaseName(), node); // Logging std::string logFileName = "/tmp/ocs2/QuadrupedDummyNodeLog.txt"; diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedMpcNode.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedMpcNode.cpp index dd922aa85..8bd1f6771 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedMpcNode.cpp @@ -16,34 +16,34 @@ namespace switched_model { -void quadrupedMpcNode(ros::NodeHandle& nodeHandle, const QuadrupedInterface& quadrupedInterface, std::unique_ptr mpcPtr) { +void quadrupedMpcNode(const rclcpp::Node::SharedPtr &node, const QuadrupedInterface& quadrupedInterface, std::unique_ptr mpcPtr) { const std::string robotName = "anymal"; auto solverModules = quadrupedInterface.getSynchronizedModules(); // Gait auto gaitReceiver = - std::make_shared(nodeHandle, quadrupedInterface.getSwitchedModelModeScheduleManagerPtr()->getGaitSchedule(), robotName); + std::make_shared(node, quadrupedInterface.getSwitchedModelModeScheduleManagerPtr()->getGaitSchedule(), robotName); solverModules.push_back(gaitReceiver); // Terrain Receiver auto terrainReceiver = std::make_shared( - quadrupedInterface.getSwitchedModelModeScheduleManagerPtr()->getTerrainModel(), nodeHandle); + quadrupedInterface.getSwitchedModelModeScheduleManagerPtr()->getTerrainModel(), node); solverModules.push_back(terrainReceiver); // Terrain plane visualization auto terrainVisualizer = std::make_shared( - quadrupedInterface.getSwitchedModelModeScheduleManagerPtr()->getSwingTrajectoryPlanner(), nodeHandle); + quadrupedInterface.getSwitchedModelModeScheduleManagerPtr()->getSwingTrajectoryPlanner(), node); solverModules.push_back(terrainVisualizer); // Swing planner auto swingPlanningVisualizer = std::make_shared( - quadrupedInterface.getSwitchedModelModeScheduleManagerPtr()->getSwingTrajectoryPlanner(), nodeHandle); + quadrupedInterface.getSwitchedModelModeScheduleManagerPtr()->getSwingTrajectoryPlanner(), node); solverModules.push_back(swingPlanningVisualizer); // reference manager auto rosReferenceManagerPtr = std::make_shared(robotName, quadrupedInterface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); + rosReferenceManagerPtr->subscribe(node); mpcPtr->getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // MPC @@ -51,6 +51,6 @@ void quadrupedMpcNode(ros::NodeHandle& nodeHandle, const QuadrupedInterface& qua // launch MPC nodes ocs2::MPC_ROS_Interface mpcNode(*mpcPtr, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); } } // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedTfPublisher.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedTfPublisher.cpp index f3045d7d9..25d2be5e4 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedTfPublisher.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedTfPublisher.cpp @@ -6,56 +6,66 @@ #include -// Additional messages not in the helpers file -#include - // URDF stuff +#include #include -#include -#include +#include namespace switched_model { -void QuadrupedTfPublisher::launchNode(ros::NodeHandle& nodeHandle, const std::string& descriptionName, std::vector jointNames, - std::string baseName, const std::string& tfPrefix) { +void QuadrupedTfPublisher::launchNode(const rclcpp::Node::SharedPtr& node, + const std::string& descriptionName, + std::vector jointNames, + std::string baseName, + const std::string& tfPrefix) { + node_ = node; tfPrefix_ = tfPrefix; jointNames_ = std::move(jointNames); baseName_ = std::move(baseName); - - // Load URDF model - urdf::Model urdfModel; - if (!urdfModel.initParam(descriptionName)) { - std::cerr << "[QuadrupedTfPublisher] Could not read URDF from: \"" << descriptionName << "\"" << std::endl; - } else { - KDL::Tree kdlTree; - kdl_parser::treeFromUrdfModel(urdfModel, kdlTree); - - robotStatePublisherPtr_.reset(new robot_state_publisher::RobotStatePublisher(kdlTree)); - robotStatePublisherPtr_->publishFixedTransforms(tfPrefix_, true); - } + tfBroadcaster_ = std::make_unique(node_); + jointPublisher_ = + node_->create_publisher("joint_states", 1); + lastTimeStamp_ = node_->get_clock()->now(); } -void QuadrupedTfPublisher::publish(ros::Time timeStamp, const vector_t& state, const std::string& worldFrame) { +void QuadrupedTfPublisher::publish(rclcpp::Time timeStamp, + const vector_t& state, + const std::string& worldFrame) { publish(timeStamp, getBasePose(state), getJointPositions(state), worldFrame); } -void QuadrupedTfPublisher::publish(ros::Time timeStamp, const base_coordinate_t& basePose, const joint_coordinate_t& jointPositions, +void QuadrupedTfPublisher::publish(rclcpp::Time timeStamp, + const base_coordinate_t& basePose, + const joint_coordinate_t& jointPositions, const std::string& worldFrame) { - if (robotStatePublisherPtr_ != nullptr && lastTimeStamp_ != timeStamp) { + if (jointPublisher_ != nullptr && lastTimeStamp_ != timeStamp) { // Joint positions updateJointPositions(jointPositions); - robotStatePublisherPtr_->publishTransforms(jointPositionsMap_, timeStamp, tfPrefix_); + + sensor_msgs::msg::JointState joint_state; + joint_state.header.stamp = timeStamp; + + const auto joint_size = jointNames_.size(); + joint_state.name.reserve(joint_size); + for (const auto& name : jointNames_) { + joint_state.name.emplace_back(name); + } + joint_state.position.reserve(joint_size); + for (const auto& jointPosition : jointPositions) + joint_state.position.emplace_back(jointPosition); + jointPublisher_->publish(joint_state); // Base positions updateBasePose(timeStamp, basePose, worldFrame); - tfBroadcaster_.sendTransform(baseToWorldTransform_); + tfBroadcaster_->sendTransform(baseToWorldTransform_); lastTimeStamp_ = timeStamp; } } -void QuadrupedTfPublisher::updateJointPositions(const joint_coordinate_t& jointPositions) { +void QuadrupedTfPublisher::updateJointPositions( + const joint_coordinate_t& jointPositions) { jointPositionsMap_[jointNames_[0]] = jointPositions[0]; jointPositionsMap_[jointNames_[1]] = jointPositions[1]; jointPositionsMap_[jointNames_[2]] = jointPositions[2]; @@ -70,13 +80,18 @@ void QuadrupedTfPublisher::updateJointPositions(const joint_coordinate_t& jointP jointPositionsMap_[jointNames_[11]] = jointPositions[11]; } -void QuadrupedTfPublisher::updateBasePose(ros::Time timeStamp, const base_coordinate_t& basePose, const std::string& worldFrame) { +void QuadrupedTfPublisher::updateBasePose(rclcpp::Time timeStamp, + const base_coordinate_t& basePose, + const std::string& worldFrame) { baseToWorldTransform_.header = ocs2::getHeaderMsg(worldFrame, timeStamp); baseToWorldTransform_.child_frame_id = tfPrefix_ + "/" + baseName_; - const Eigen::Quaternion q_world_base = quaternionBaseToOrigin(getOrientation(basePose)); - baseToWorldTransform_.transform.rotation = ocs2::getOrientationMsg(q_world_base); - baseToWorldTransform_.transform.translation = ocs2::getVectorMsg(getPositionInOrigin(basePose)); + const Eigen::Quaternion q_world_base = + quaternionBaseToOrigin(getOrientation(basePose)); + baseToWorldTransform_.transform.rotation = + ocs2::getOrientationMsg(q_world_base); + baseToWorldTransform_.transform.translation = + ocs2::getVectorMsg(getPositionInOrigin(basePose)); } } // namespace switched_model \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedVisualizer.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedVisualizer.cpp index 3b3ec1da1..b5f880b17 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedVisualizer.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedVisualizer.cpp @@ -4,16 +4,12 @@ #include "ocs2_quadruped_interface/QuadrupedVisualizer.h" -#include - #include - -// Additional messages not in the helpers file -#include -#include +#include // URDF stuff #include + #include // Switched model conversions @@ -22,51 +18,94 @@ namespace switched_model { -void QuadrupedVisualizer::launchVisualizerNode(ros::NodeHandle& nodeHandle, std::vector jointNames, std::string baseName) { - costDesiredBasePositionPublisher_ = nodeHandle.advertise("/ocs2_anymal/desiredBaseTrajectory", 1); - costDesiredBasePosePublisher_ = nodeHandle.advertise("/ocs2_anymal/desiredPoseTrajectory", 1); - costDesiredBaseAngVelocityPublisher_ = nodeHandle.advertise("/ocs2_anymal/desiredAngVelTrajectory", 1); - costDesiredBaseVelocityPublisher_ = nodeHandle.advertise("/ocs2_anymal/desiredVelTrajectory", 1); - costDesiredFeetPositionPublishers_[0] = nodeHandle.advertise("/ocs2_anymal/desiredFeetTrajectory/LF", 1); - costDesiredFeetPositionPublishers_[1] = nodeHandle.advertise("/ocs2_anymal/desiredFeetTrajectory/RF", 1); - costDesiredFeetPositionPublishers_[2] = nodeHandle.advertise("/ocs2_anymal/desiredFeetTrajectory/LH", 1); - costDesiredFeetPositionPublishers_[3] = nodeHandle.advertise("/ocs2_anymal/desiredFeetTrajectory/RH", 1); +QuadrupedVisualizer::QuadrupedVisualizer( + const kinematic_model_t& kinematicModel, + std::vector jointNames, std::string baseName, + const rclcpp::Node::SharedPtr& node, scalar_t maxUpdateFrequency) + : node_(node), + kinematicModelPtr_(kinematicModel.clone()), + lastTime_(std::numeric_limits::lowest()), + minPublishTimeDifference_(1.0 / maxUpdateFrequency) { + costDesiredBasePositionPublisher_ = + node->create_publisher( + "/ocs2_anymal/desiredBaseTrajectory", 1); + costDesiredBasePosePublisher_ = + node->create_publisher( + "/ocs2_anymal/desiredPoseTrajectory", 1); + costDesiredBaseAngVelocityPublisher_ = + node->create_publisher( + "/ocs2_anymal/desiredAngVelTrajectory", 1); + costDesiredBaseVelocityPublisher_ = + node->create_publisher( + "/ocs2_anymal/desiredVelTrajectory", 1); + costDesiredFeetPositionPublishers_[0] = + node->create_publisher( + "/ocs2_anymal/desiredFeetTrajectory/LF", 1); + costDesiredFeetPositionPublishers_[1] = + node->create_publisher( + "/ocs2_anymal/desiredFeetTrajectory/RF", 1); + costDesiredFeetPositionPublishers_[2] = + node->create_publisher( + "/ocs2_anymal/desiredFeetTrajectory/LH", 1); + costDesiredFeetPositionPublishers_[3] = + node->create_publisher( + "/ocs2_anymal/desiredFeetTrajectory/RH", 1); costDesiredFeetVelocityPublishers_[0] = - nodeHandle.advertise("/ocs2_anymal/desiredFeetVelTrajectory/LF", 1); + node->create_publisher( + "/ocs2_anymal/desiredFeetVelTrajectory/LF", 1); costDesiredFeetVelocityPublishers_[1] = - nodeHandle.advertise("/ocs2_anymal/desiredFeetVelTrajectory/RF", 1); + node->create_publisher( + "/ocs2_anymal/desiredFeetVelTrajectory/RF", 1); costDesiredFeetVelocityPublishers_[2] = - nodeHandle.advertise("/ocs2_anymal/desiredFeetVelTrajectory/LH", 1); + node->create_publisher( + "/ocs2_anymal/desiredFeetVelTrajectory/LH", 1); costDesiredFeetVelocityPublishers_[3] = - nodeHandle.advertise("/ocs2_anymal/desiredFeetVelTrajectory/RH", 1); - stateOptimizedPublisher_ = nodeHandle.advertise("/ocs2_anymal/optimizedStateTrajectory", 1); - stateOptimizedPosePublisher_ = nodeHandle.advertise("/ocs2_anymal/optimizedPoseTrajectory", 1); - currentStatePublisher_ = nodeHandle.advertise("/ocs2_anymal/currentState", 1); - currentFeetPosesPublisher_ = nodeHandle.advertise("/ocs2_anymal/currentFeetPoses", 1); - currentCollisionSpheresPublisher_ = nodeHandle.advertise("/ocs2_anymal/currentCollisionSpheres", 1); - - quadrupedTfPublisher_.launchNode(nodeHandle, "ocs2_anymal_description", std::move(jointNames), std::move(baseName)); + node->create_publisher( + "/ocs2_anymal/desiredFeetVelTrajectory/RH", 1); + stateOptimizedPublisher_ = + node->create_publisher( + "/ocs2_anymal/optimizedStateTrajectory", 1); + stateOptimizedPosePublisher_ = + node->create_publisher( + "/ocs2_anymal/optimizedPoseTrajectory", 1); + currentStatePublisher_ = + node->create_publisher( + "/ocs2_anymal/currentState", 1); + currentFeetPosesPublisher_ = + node->create_publisher( + "/ocs2_anymal/currentFeetPoses", 1); + currentCollisionSpheresPublisher_ = + node->create_publisher( + "/ocs2_anymal/currentCollisionSpheres", 1); + + quadrupedTfPublisher_.launchNode(node, "ocs2_anymal_description", + std::move(jointNames), std::move(baseName)); } -void QuadrupedVisualizer::update(const ocs2::SystemObservation& observation, const ocs2::PrimalSolution& primalSolution, +void QuadrupedVisualizer::update(const ocs2::SystemObservation& observation, + const ocs2::PrimalSolution& primalSolution, const ocs2::CommandData& command) { if (observation.time - lastTime_ > minPublishTimeDifference_) { - const auto timeStamp = ros::Time::now(); + const auto timeStamp = node_->get_clock()->now(); publishObservation(timeStamp, observation); publishDesiredTrajectory(timeStamp, command.mpcTargetTrajectories_); - publishOptimizedStateTrajectory(timeStamp, primalSolution.timeTrajectory_, primalSolution.stateTrajectory_, + publishOptimizedStateTrajectory(timeStamp, primalSolution.timeTrajectory_, + primalSolution.stateTrajectory_, primalSolution.modeSchedule_); lastTime_ = observation.time; } } -void QuadrupedVisualizer::publishObservation(ros::Time timeStamp, const ocs2::SystemObservation& observation) { +void QuadrupedVisualizer::publishObservation( + rclcpp::Time timeStamp, const ocs2::SystemObservation& observation) { // Extract components from state const state_vector_t switchedState = observation.state.head(STATE_DIM); const base_coordinate_t basePose = getBasePose(switchedState); const auto o_basePosition = getPositionInOrigin(basePose); - const Eigen::Matrix3d o_R_b = rotationMatrixBaseToOrigin(getOrientation(basePose)); - const joint_coordinate_t qJoints = getJointPositions(state_vector_t(observation.state)); + const Eigen::Matrix3d o_R_b = + rotationMatrixBaseToOrigin(getOrientation(basePose)); + const joint_coordinate_t qJoints = + getJointPositions(state_vector_t(observation.state)); // Compute cartesian state and inputs feet_array_t feetPosition; @@ -74,81 +113,103 @@ void QuadrupedVisualizer::publishObservation(ros::Time timeStamp, const ocs2::Sy feet_array_t> feetOrientations; for (size_t i = 0; i < NUM_CONTACT_POINTS; i++) { feetPosition[i] = o_basePosition; - feetPosition[i].noalias() += o_R_b * kinematicModelPtr_->positionBaseToFootInBaseFrame(i, qJoints); + feetPosition[i].noalias() += + o_R_b * kinematicModelPtr_->positionBaseToFootInBaseFrame(i, qJoints); feetForce[i] = o_R_b * observation.input.segment<3>(3 * i); - const matrix3_t footOrientationInOriginFrame = o_R_b * kinematicModelPtr_->footOrientationInBaseFrame(i, qJoints); - feetOrientations[i] = Eigen::Quaternion(footOrientationInOriginFrame); + const matrix3_t footOrientationInOriginFrame = + o_R_b * kinematicModelPtr_->footOrientationInBaseFrame(i, qJoints); + feetOrientations[i] = + Eigen::Quaternion(footOrientationInOriginFrame); } // Publish quadrupedTfPublisher_.publish(timeStamp, basePose, qJoints, frameId_); - publishCartesianMarkers(timeStamp, modeNumber2StanceLeg(observation.mode), feetPosition, feetForce); + publishCartesianMarkers(timeStamp, modeNumber2StanceLeg(observation.mode), + feetPosition, feetForce); publishEndEffectorPoses(timeStamp, feetPosition, feetOrientations); publishCollisionSpheres(timeStamp, basePose, qJoints); } -void QuadrupedVisualizer::publishTrajectory(const std::vector& system_observation_array, double speed) { +void QuadrupedVisualizer::publishTrajectory( + const std::vector& system_observation_array, + double speed) { for (size_t k = 0; k < system_observation_array.size() - 1; k++) { - double frameDuration = speed * (system_observation_array[k + 1].time - system_observation_array[k].time); - double publishDuration = ocs2::timedExecutionInSeconds([&]() { publishObservation(ros::Time::now(), system_observation_array[k]); }); + double frameDuration = speed * (system_observation_array[k + 1].time - + system_observation_array[k].time); + double publishDuration = ocs2::timedExecutionInSeconds([&]() { + publishObservation(node_->get_clock()->now(), + system_observation_array[k]); + }); if (frameDuration > publishDuration) { - ros::WallDuration(frameDuration - publishDuration).sleep(); + const rclcpp::Duration duration = + rclcpp::Duration::from_seconds(frameDuration - publishDuration); + rclcpp::sleep_for((std::chrono::nanoseconds(duration.nanoseconds()))); } } } -void QuadrupedVisualizer::publishCartesianMarkers(ros::Time timeStamp, const contact_flag_t& contactFlags, - const feet_array_t& feetPosition, - const feet_array_t& feetForce) const { +void QuadrupedVisualizer::publishCartesianMarkers( + rclcpp::Time timeStamp, const contact_flag_t& contactFlags, + const feet_array_t& feetPosition, + const feet_array_t& feetForce) const { // Reserve message const int numberOfCartesianMarkers = 10; - visualization_msgs::MarkerArray markerArray; + visualization_msgs::msg::MarkerArray markerArray; markerArray.markers.reserve(numberOfCartesianMarkers); // Feet positions and Forces for (int i = 0; i < NUM_CONTACT_POINTS; ++i) { markerArray.markers.emplace_back( - getFootMarker(feetPosition[i], contactFlags[i], feetColorMap_[i], footMarkerDiameter_, footAlphaWhenLifted_)); - markerArray.markers.emplace_back(getForceMarker(feetForce[i], feetPosition[i], contactFlags[i], ocs2::Color::green, forceScale_)); + getFootMarker(feetPosition[i], contactFlags[i], feetColorMap_[i], + footMarkerDiameter_, footAlphaWhenLifted_)); + markerArray.markers.emplace_back( + getForceMarker(feetForce[i], feetPosition[i], contactFlags[i], + ocs2::Color::green, forceScale_)); } // Center of pressure - markerArray.markers.emplace_back(getCenterOfPressureMarker(feetForce.begin(), feetForce.end(), feetPosition.begin(), contactFlags.begin(), - ocs2::Color::green, copMarkerDiameter_)); + markerArray.markers.emplace_back(getCenterOfPressureMarker( + feetForce.begin(), feetForce.end(), feetPosition.begin(), + contactFlags.begin(), ocs2::Color::green, copMarkerDiameter_)); // Support polygon - markerArray.markers.emplace_back(getSupportPolygonMarker(feetPosition.begin(), feetPosition.end(), contactFlags.begin(), - ocs2::Color::black, supportPolygonLineWidth_)); + markerArray.markers.emplace_back(getSupportPolygonMarker( + feetPosition.begin(), feetPosition.end(), contactFlags.begin(), + ocs2::Color::black, supportPolygonLineWidth_)); // Give markers an id and a frame - ocs2::assignHeader(markerArray.markers.begin(), markerArray.markers.end(), ocs2::getHeaderMsg(frameId_, timeStamp)); - ocs2::assignIncreasingId(markerArray.markers.begin(), markerArray.markers.end()); + ocs2::assignHeader(markerArray.markers.begin(), markerArray.markers.end(), + ocs2::getHeaderMsg(frameId_, timeStamp)); + ocs2::assignIncreasingId(markerArray.markers.begin(), + markerArray.markers.end()); // Publish cartesian markers (minus the CoM Pose) - currentStatePublisher_.publish(markerArray); + currentStatePublisher_->publish(markerArray); } -void QuadrupedVisualizer::publishDesiredTrajectory(ros::Time timeStamp, const ocs2::TargetTrajectories& targetTrajectories) const { +void QuadrupedVisualizer::publishDesiredTrajectory( + rclcpp::Time timeStamp, + const ocs2::TargetTrajectories& targetTrajectories) const { const auto& stateTrajectory = targetTrajectories.stateTrajectory; const auto& inputTrajectory = targetTrajectories.inputTrajectory; const size_t stateTrajSize = stateTrajectory.size(); const size_t inputTrajSize = inputTrajectory.size(); // Reserve Base messages - std::vector desiredBasePositionMsg; + std::vector desiredBasePositionMsg; desiredBasePositionMsg.reserve(stateTrajSize); - geometry_msgs::PoseArray poseArray; + geometry_msgs::msg::PoseArray poseArray; poseArray.poses.reserve(stateTrajSize); - visualization_msgs::MarkerArray velArray; + visualization_msgs::msg::MarkerArray velArray; velArray.markers.reserve(stateTrajSize); - visualization_msgs::MarkerArray angVelArray; + visualization_msgs::msg::MarkerArray angVelArray; angVelArray.markers.reserve(stateTrajSize); // Reserve feet messages - feet_array_t> desiredFeetPositionMsgs; - feet_array_t feetVelArray; + feet_array_t> desiredFeetPositionMsgs; + feet_array_t feetVelArray; for (size_t i = 0; i < NUM_CONTACT_POINTS; i++) { desiredFeetPositionMsgs[i].reserve(stateTrajSize); feetVelArray[i].markers.reserve(stateTrajSize); @@ -168,14 +229,16 @@ void QuadrupedVisualizer::publishDesiredTrajectory(ros::Time timeStamp, const oc const base_coordinate_t baseTwist = state.segment<6>(6); const auto o_basePosition = getPositionInOrigin(basePose); const auto eulerXYZ = getOrientation(basePose); - const Eigen::Matrix3d o_R_b = rotationMatrixBaseToOrigin(eulerXYZ); + const Eigen::Matrix3d o_R_b = + rotationMatrixBaseToOrigin(eulerXYZ); const auto qJoints = getJointPositions(state); const auto qVelJoints = getJointVelocities(input); // Construct pose msg - geometry_msgs::Pose pose; + geometry_msgs::msg::Pose pose; pose.position = ocs2::getPointMsg(o_basePosition); - pose.orientation = ocs2::getOrientationMsg(quaternionBaseToOrigin(eulerXYZ)); + pose.orientation = + ocs2::getOrientationMsg(quaternionBaseToOrigin(eulerXYZ)); // Construct vel msg const vector3_t o_baseVel = o_R_b * getLinearVelocity(baseTwist); @@ -184,104 +247,132 @@ void QuadrupedVisualizer::publishDesiredTrajectory(ros::Time timeStamp, const oc // Fill message containers desiredBasePositionMsg.push_back(pose.position); poseArray.poses.push_back(std::move(pose)); - velArray.markers.emplace_back(getArrowAtPointMsg(o_baseVel / velScale_, o_basePosition, ocs2::Color::blue)); - angVelArray.markers.emplace_back(getArrowAtPointMsg(o_baseAngVel / velScale_, o_basePosition, ocs2::Color::green)); + velArray.markers.emplace_back(getArrowAtPointMsg( + o_baseVel / velScale_, o_basePosition, ocs2::Color::blue)); + angVelArray.markers.emplace_back(getArrowAtPointMsg( + o_baseAngVel / velScale_, o_basePosition, ocs2::Color::green)); for (size_t i = 0; i < NUM_CONTACT_POINTS; i++) { - const vector3_t b_baseToFoot = kinematicModelPtr_->positionBaseToFootInBaseFrame(i, qJoints); + const vector3_t b_baseToFoot = + kinematicModelPtr_->positionBaseToFootInBaseFrame(i, qJoints); auto o_footPosition = o_basePosition; o_footPosition.noalias() += o_R_b * b_baseToFoot; - const vector3_t b_footVelocity = kinematicModelPtr_->footVelocityRelativeToBaseInBaseFrame(i, qJoints, qVelJoints) + - getLinearVelocity(baseTwist) + getAngularVelocity(baseTwist).cross(b_baseToFoot); + const vector3_t b_footVelocity = + kinematicModelPtr_->footVelocityRelativeToBaseInBaseFrame( + i, qJoints, qVelJoints) + + getLinearVelocity(baseTwist) + + getAngularVelocity(baseTwist).cross(b_baseToFoot); const vector3_t o_footVelocity = o_R_b * b_footVelocity; desiredFeetPositionMsgs[i].push_back(ocs2::getPointMsg(o_footPosition)); - feetVelArray[i].markers.emplace_back(getArrowAtPointMsg(o_footVelocity / velScale_, o_footPosition, feetColorMap_[i])); + feetVelArray[i].markers.emplace_back(getArrowAtPointMsg( + o_footVelocity / velScale_, o_footPosition, feetColorMap_[i])); } } // Headers - auto baseLineMsg = getLineMsg(std::move(desiredBasePositionMsg), ocs2::Color::green, trajectoryLineWidth_); + auto baseLineMsg = getLineMsg(std::move(desiredBasePositionMsg), + ocs2::Color::green, trajectoryLineWidth_); baseLineMsg.header = ocs2::getHeaderMsg(frameId_, timeStamp); baseLineMsg.id = 0; poseArray.header = ocs2::getHeaderMsg(frameId_, timeStamp); - ocs2::assignHeader(velArray.markers.begin(), velArray.markers.end(), ocs2::getHeaderMsg(frameId_, timeStamp)); + ocs2::assignHeader(velArray.markers.begin(), velArray.markers.end(), + ocs2::getHeaderMsg(frameId_, timeStamp)); ocs2::assignIncreasingId(velArray.markers.begin(), velArray.markers.end()); - ocs2::assignHeader(angVelArray.markers.begin(), angVelArray.markers.end(), ocs2::getHeaderMsg(frameId_, timeStamp)); - ocs2::assignIncreasingId(angVelArray.markers.begin(), angVelArray.markers.end()); + ocs2::assignHeader(angVelArray.markers.begin(), angVelArray.markers.end(), + ocs2::getHeaderMsg(frameId_, timeStamp)); + ocs2::assignIncreasingId(angVelArray.markers.begin(), + angVelArray.markers.end()); // Publish - costDesiredBasePositionPublisher_.publish(baseLineMsg); - costDesiredBasePosePublisher_.publish(poseArray); - costDesiredBaseVelocityPublisher_.publish(velArray); - costDesiredBaseAngVelocityPublisher_.publish(angVelArray); + costDesiredBasePositionPublisher_->publish(baseLineMsg); + costDesiredBasePosePublisher_->publish(poseArray); + costDesiredBaseVelocityPublisher_->publish(velArray); + costDesiredBaseAngVelocityPublisher_->publish(angVelArray); for (size_t i = 0; i < NUM_CONTACT_POINTS; i++) { - auto footLineMsg = getLineMsg(std::move(desiredFeetPositionMsgs[i]), feetColorMap_[i], trajectoryLineWidth_); + auto footLineMsg = getLineMsg(std::move(desiredFeetPositionMsgs[i]), + feetColorMap_[i], trajectoryLineWidth_); footLineMsg.header = ocs2::getHeaderMsg(frameId_, timeStamp); footLineMsg.id = 0; - costDesiredFeetPositionPublishers_[i].publish(footLineMsg); - ocs2::assignHeader(feetVelArray[i].markers.begin(), feetVelArray[i].markers.end(), ocs2::getHeaderMsg(frameId_, timeStamp)); - ocs2::assignIncreasingId(feetVelArray[i].markers.begin(), feetVelArray[i].markers.end()); - costDesiredFeetVelocityPublishers_[i].publish(feetVelArray[i]); + costDesiredFeetPositionPublishers_[i]->publish(footLineMsg); + ocs2::assignHeader(feetVelArray[i].markers.begin(), + feetVelArray[i].markers.end(), + ocs2::getHeaderMsg(frameId_, timeStamp)); + ocs2::assignIncreasingId(feetVelArray[i].markers.begin(), + feetVelArray[i].markers.end()); + costDesiredFeetVelocityPublishers_[i]->publish(feetVelArray[i]); } } -void QuadrupedVisualizer::publishOptimizedStateTrajectory(ros::Time timeStamp, const scalar_array_t& mpcTimeTrajectory, - const vector_array_t& mpcStateTrajectory, - const ocs2::ModeSchedule& modeSchedule) const { +void QuadrupedVisualizer::publishOptimizedStateTrajectory( + rclcpp::Time timeStamp, const scalar_array_t& mpcTimeTrajectory, + const vector_array_t& mpcStateTrajectory, + const ocs2::ModeSchedule& modeSchedule) const { 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 Base Msg - std::vector mpcBasePositionMsgs; + std::vector mpcBasePositionMsgs; mpcBasePositionMsgs.reserve(mpcStateTrajectory.size()); // Reserve Pose array - geometry_msgs::PoseArray poseArray; + geometry_msgs::msg::PoseArray poseArray; poseArray.poses.reserve(mpcStateTrajectory.size()); // Extract Base and Feet from state - std::for_each(mpcStateTrajectory.begin(), mpcStateTrajectory.end(), [&](const vector_t& state) { - const state_vector_t switchedState = state.head(STATE_DIM); - const base_coordinate_t basePose = getBasePose(switchedState); - const vector3_t o_basePosition = getPositionInOrigin(basePose); - const vector3_t eulerXYZ = getOrientation(basePose); - const matrix3_t o_R_b = rotationMatrixBaseToOrigin(eulerXYZ); - const joint_coordinate_t qJoints = getJointPositions(switchedState); - - // Fill Base position and pose msgs - geometry_msgs::Pose pose; - pose.position = ocs2::getPointMsg(o_basePosition); - pose.orientation = ocs2::getOrientationMsg(quaternionBaseToOrigin(eulerXYZ)); - mpcBasePositionMsgs.push_back(pose.position); - poseArray.poses.push_back(std::move(pose)); - - // Fill feet msgs - for (size_t i = 0; i < NUM_CONTACT_POINTS; i++) { - auto o_footPosition = o_basePosition; - o_footPosition.noalias() += o_R_b * kinematicModelPtr_->positionBaseToFootInBaseFrame(i, qJoints); - feetMsgs[i].push_back(ocs2::getPointMsg(o_footPosition)); - } - }); + std::for_each( + mpcStateTrajectory.begin(), mpcStateTrajectory.end(), + [&](const vector_t& state) { + const state_vector_t switchedState = state.head(STATE_DIM); + const base_coordinate_t basePose = getBasePose(switchedState); + const vector3_t o_basePosition = getPositionInOrigin(basePose); + const vector3_t eulerXYZ = getOrientation(basePose); + const matrix3_t o_R_b = rotationMatrixBaseToOrigin(eulerXYZ); + const joint_coordinate_t qJoints = getJointPositions(switchedState); + + // Fill Base position and pose msgs + geometry_msgs::msg::Pose pose; + pose.position = ocs2::getPointMsg(o_basePosition); + pose.orientation = + ocs2::getOrientationMsg(quaternionBaseToOrigin(eulerXYZ)); + mpcBasePositionMsgs.push_back(pose.position); + poseArray.poses.push_back(std::move(pose)); + + // Fill feet msgs + for (size_t i = 0; i < NUM_CONTACT_POINTS; i++) { + auto o_footPosition = o_basePosition; + o_footPosition.noalias() += + o_R_b * + kinematicModelPtr_->positionBaseToFootInBaseFrame(i, qJoints); + feetMsgs[i].push_back(ocs2::getPointMsg(o_footPosition)); + } + }); // Convert feet msgs to Array message - visualization_msgs::MarkerArray markerArray; - markerArray.markers.reserve(NUM_CONTACT_POINTS + 2); // 1 trajectory per foot + 1 for the future footholds + 1 for the Base trajectory + visualization_msgs::msg::MarkerArray markerArray; + markerArray.markers.reserve(NUM_CONTACT_POINTS + + 2); // 1 trajectory per foot + 1 for the future + // footholds + 1 for the Base trajectory for (int i = 0; i < NUM_CONTACT_POINTS; i++) { - markerArray.markers.emplace_back(getLineMsg(std::move(feetMsgs[i]), feetColorMap_[i], trajectoryLineWidth_)); + markerArray.markers.emplace_back(getLineMsg( + std::move(feetMsgs[i]), feetColorMap_[i], trajectoryLineWidth_)); markerArray.markers.back().ns = "EE Trajectories"; } - markerArray.markers.emplace_back(getLineMsg(std::move(mpcBasePositionMsgs), ocs2::Color::red, trajectoryLineWidth_)); + markerArray.markers.emplace_back(getLineMsg( + std::move(mpcBasePositionMsgs), ocs2::Color::red, trajectoryLineWidth_)); markerArray.markers.back().ns = "Base 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_; @@ -292,17 +383,28 @@ void QuadrupedVisualizer::publishOptimizedStateTrajectory(ros::Time timeStamp, c const double tStart = mpcTimeTrajectory.front(); const double tEnd = mpcTimeTrajectory.back(); for (int event = 0; event < eventTimes.size(); ++event) { - if (tStart < eventTimes[event] && eventTimes[event] < tEnd) { // Only publish future footholds within the optimized horizon - const auto preEventContactFlags = modeNumber2StanceLeg(subsystemSequence[event]); - const auto postEventContactFlags = modeNumber2StanceLeg(subsystemSequence[event + 1]); - const vector_t postEventState = ocs2::LinearInterpolation::interpolate(eventTimes[event], mpcTimeTrajectory, mpcStateTrajectory); - const state_vector_t postEventSwitchedState = postEventState.head(STATE_DIM); + if (tStart < eventTimes[event] && + eventTimes[event] < tEnd) { // Only publish future footholds within the + // optimized horizon + const auto preEventContactFlags = + modeNumber2StanceLeg(subsystemSequence[event]); + const auto postEventContactFlags = + modeNumber2StanceLeg(subsystemSequence[event + 1]); + const vector_t postEventState = ocs2::LinearInterpolation::interpolate( + eventTimes[event], mpcTimeTrajectory, mpcStateTrajectory); + const state_vector_t postEventSwitchedState = + postEventState.head(STATE_DIM); const base_coordinate_t basePose = getBasePose(postEventSwitchedState); - const joint_coordinate_t qJoints = getJointPositions(postEventSwitchedState); + const joint_coordinate_t qJoints = + getJointPositions(postEventSwitchedState); for (int i = 0; i < NUM_CONTACT_POINTS; i++) { - if (!preEventContactFlags[i] && postEventContactFlags[i]) { // If a foot lands, a marker is added at that location. - const auto o_feetPosition = kinematicModelPtr_->footPositionInOriginFrame(i, basePose, qJoints); + if (!preEventContactFlags[i] && + postEventContactFlags[i]) { // If a foot lands, a marker is added + // at that location. + const auto o_feetPosition = + kinematicModelPtr_->footPositionInOriginFrame(i, basePose, + qJoints); sphereList.points.emplace_back(ocs2::getPointMsg(o_feetPosition)); sphereList.colors.push_back(getColor(feetColorMap_[i])); } @@ -312,45 +414,53 @@ void QuadrupedVisualizer::publishOptimizedStateTrajectory(ros::Time timeStamp, c markerArray.markers.push_back(std::move(sphereList)); // Add headers and Id - ocs2::assignHeader(markerArray.markers.begin(), markerArray.markers.end(), ocs2::getHeaderMsg(frameId_, timeStamp)); - ocs2::assignIncreasingId(markerArray.markers.begin(), markerArray.markers.end()); + ocs2::assignHeader(markerArray.markers.begin(), markerArray.markers.end(), + ocs2::getHeaderMsg(frameId_, timeStamp)); + ocs2::assignIncreasingId(markerArray.markers.begin(), + markerArray.markers.end()); poseArray.header = ocs2::getHeaderMsg(frameId_, timeStamp); - stateOptimizedPublisher_.publish(markerArray); - stateOptimizedPosePublisher_.publish(poseArray); + stateOptimizedPublisher_->publish(markerArray); + stateOptimizedPosePublisher_->publish(poseArray); } -void QuadrupedVisualizer::publishEndEffectorPoses(ros::Time timeStamp, const feet_array_t& feetPositions, - const feet_array_t>& feetOrientations) const { +void QuadrupedVisualizer::publishEndEffectorPoses( + rclcpp::Time timeStamp, const feet_array_t& feetPositions, + const feet_array_t>& feetOrientations) const { // Feet positions and Forces - geometry_msgs::PoseArray poseArray; + geometry_msgs::msg::PoseArray poseArray; poseArray.header = ocs2::getHeaderMsg(frameId_, timeStamp); for (int i = 0; i < NUM_CONTACT_POINTS; ++i) { - geometry_msgs::Pose pose; + geometry_msgs::msg::Pose pose; pose.position = ocs2::getPointMsg(feetPositions[i]); pose.orientation = ocs2::getOrientationMsg(feetOrientations[i]); poseArray.poses.push_back(std::move(pose)); } - currentFeetPosesPublisher_.publish(poseArray); + currentFeetPosesPublisher_->publish(poseArray); } -void QuadrupedVisualizer::publishCollisionSpheres(ros::Time timeStamp, const base_coordinate_t& basePose, - const joint_coordinate_t& jointAngles) const { - const auto collisionSpheres = kinematicModelPtr_->collisionSpheresInOriginFrame(basePose, jointAngles); +void QuadrupedVisualizer::publishCollisionSpheres( + rclcpp::Time timeStamp, const base_coordinate_t& basePose, + const joint_coordinate_t& jointAngles) const { + const auto collisionSpheres = + kinematicModelPtr_->collisionSpheresInOriginFrame(basePose, jointAngles); - visualization_msgs::MarkerArray markerArray; + visualization_msgs::msg::MarkerArray markerArray; markerArray.markers.reserve(collisionSpheres.size()); for (const auto& sphere : collisionSpheres) { - markerArray.markers.emplace_back(ocs2::getSphereMsg(sphere.position, ocs2::Color::red, 2.0 * sphere.radius)); + markerArray.markers.emplace_back(ocs2::getSphereMsg( + sphere.position, ocs2::Color::red, 2.0 * sphere.radius)); } // Give markers an id and a frame - ocs2::assignHeader(markerArray.markers.begin(), markerArray.markers.end(), ocs2::getHeaderMsg(frameId_, timeStamp)); - ocs2::assignIncreasingId(markerArray.markers.begin(), markerArray.markers.end()); + ocs2::assignHeader(markerArray.markers.begin(), markerArray.markers.end(), + ocs2::getHeaderMsg(frameId_, timeStamp)); + ocs2::assignIncreasingId(markerArray.markers.begin(), + markerArray.markers.end()); - currentCollisionSpheresPublisher_.publish(markerArray); + currentCollisionSpheresPublisher_->publish(markerArray); } } // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/SwingPlanningVisualizer.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/SwingPlanningVisualizer.cpp index 423998c2b..743116cda 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/SwingPlanningVisualizer.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/SwingPlanningVisualizer.cpp @@ -4,81 +4,85 @@ #include "ocs2_quadruped_interface/SwingPlanningVisualizer.h" -#include -#include - #include -namespace switched_model { - -SwingPlanningVisualizer::SwingPlanningVisualizer(const SwingTrajectoryPlanner& swingTrajectoryPlanner, ros::NodeHandle& nodeHandle) - : swingTrajectoryPlannerPtr_(&swingTrajectoryPlanner) { - nominalFootholdPublishers_[0] = nodeHandle.advertise("/ocs2_anymal/swing_planner/nominalFootholds_LF", 1); - nominalFootholdPublishers_[1] = nodeHandle.advertise("/ocs2_anymal/swing_planner/nominalFootholds_RF", 1); - nominalFootholdPublishers_[2] = nodeHandle.advertise("/ocs2_anymal/swing_planner/nominalFootholds_LH", 1); - nominalFootholdPublishers_[3] = nodeHandle.advertise("/ocs2_anymal/swing_planner/nominalFootholds_RH", 1); - swingTrajectoryPublishers_[0] = nodeHandle.advertise("/ocs2_anymal/swing_planner/trajectory_LF", 1); - swingTrajectoryPublishers_[1] = nodeHandle.advertise("/ocs2_anymal/swing_planner/trajectory_RF", 1); - swingTrajectoryPublishers_[2] = nodeHandle.advertise("/ocs2_anymal/swing_planner/trajectory_LH", 1); - swingTrajectoryPublishers_[3] = nodeHandle.advertise("/ocs2_anymal/swing_planner/trajectory_RH", 1); -} - -void SwingPlanningVisualizer::preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, - const ocs2::ReferenceManagerInterface& referenceManager) { - const auto timeStamp = ros::Time::now(); - - // Nominal footholds - for (int leg = 0; leg < NUM_CONTACT_POINTS; leg++) { - const auto nominalFootholds = swingTrajectoryPlannerPtr_->getNominalFootholds(leg); - - // Reserve pose array - geometry_msgs::PoseArray poseArray; - poseArray.poses.reserve(nominalFootholds.size()); - - // Convert terrain planes to pose msgs - std::for_each(nominalFootholds.begin(), nominalFootholds.end(), [&](const ConvexTerrain& foothold) { +namespace switched_model +{ + + SwingPlanningVisualizer::SwingPlanningVisualizer(const SwingTrajectoryPlanner &swingTrajectoryPlanner, const rclcpp::Node::SharedPtr &node) + : node_(node), swingTrajectoryPlannerPtr_(&swingTrajectoryPlanner) + { + nominalFootholdPublishers_[0] = node->create_publisher("/ocs2_anymal/swing_planner/nominalFootholds_LF", 1); + nominalFootholdPublishers_[1] = node->create_publisher("/ocs2_anymal/swing_planner/nominalFootholds_RF", 1); + nominalFootholdPublishers_[2] = node->create_publisher("/ocs2_anymal/swing_planner/nominalFootholds_LH", 1); + nominalFootholdPublishers_[3] = node->create_publisher("/ocs2_anymal/swing_planner/nominalFootholds_RH", 1); + swingTrajectoryPublishers_[0] = node->create_publisher("/ocs2_anymal/swing_planner/trajectory_LF", 1); + swingTrajectoryPublishers_[1] = node->create_publisher("/ocs2_anymal/swing_planner/trajectory_RF", 1); + swingTrajectoryPublishers_[2] = node->create_publisher("/ocs2_anymal/swing_planner/trajectory_LH", 1); + swingTrajectoryPublishers_[3] = node->create_publisher("/ocs2_anymal/swing_planner/trajectory_RH", 1); + } + + void SwingPlanningVisualizer::preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t ¤tState, + const ocs2::ReferenceManagerInterface &referenceManager) + { + const auto timeStamp = node_->get_clock()->now(); + + // Nominal footholds + for (int leg = 0; leg < NUM_CONTACT_POINTS; leg++) + { + const auto nominalFootholds = swingTrajectoryPlannerPtr_->getNominalFootholds(leg); + + // Reserve pose array + geometry_msgs::msg::PoseArray poseArray; + poseArray.poses.reserve(nominalFootholds.size()); + + // Convert terrain planes to pose msgs + std::for_each(nominalFootholds.begin(), nominalFootholds.end(), [&](const ConvexTerrain &foothold) + { // Construct pose msg - geometry_msgs::Pose pose; + geometry_msgs::msg::Pose pose; pose.position = ocs2::getPointMsg(foothold.plane.positionInWorld); Eigen::Quaterniond terrainOrientation(foothold.plane.orientationWorldToTerrain.transpose()); pose.orientation = ocs2::getOrientationMsg(terrainOrientation); // Fill message container - poseArray.poses.push_back(std::move(pose)); - }); + poseArray.poses.push_back(std::move(pose)); }); - poseArray.header = ocs2::getHeaderMsg(frameId_, timeStamp); - nominalFootholdPublishers_[leg].publish(poseArray); - } + poseArray.header = ocs2::getHeaderMsg(frameId_, timeStamp); + nominalFootholdPublishers_[leg]->publish(poseArray); + } - // Feet trajectories - visualization_msgs::Marker deleteMarker; - deleteMarker.action = visualization_msgs::Marker::DELETEALL; - for (int leg = 0; leg < NUM_CONTACT_POINTS; leg++) { - // Initialize and add marker that deletes old visualizations - visualization_msgs::MarkerArray swingTrajectoriesMessage; - swingTrajectoriesMessage.markers.reserve(swingTrajectoryPlannerPtr_->getTargetTrajectories().timeTrajectory.size() + 1); // lower bound - swingTrajectoriesMessage.markers.push_back(deleteMarker); - - for (const auto& t : swingTrajectoryPlannerPtr_->getTargetTrajectories().timeTrajectory) { - if (t < initTime || t > finalTime) { - continue; + // Feet trajectories + visualization_msgs::msg::Marker deleteMarker; + deleteMarker.action = visualization_msgs::msg::Marker::DELETEALL; + for (int leg = 0; leg < NUM_CONTACT_POINTS; leg++) + { + // Initialize and add marker that deletes old visualizations + visualization_msgs::msg::MarkerArray swingTrajectoriesMessage; + swingTrajectoriesMessage.markers.reserve(swingTrajectoryPlannerPtr_->getTargetTrajectories().timeTrajectory.size() + 1); // lower bound + swingTrajectoriesMessage.markers.push_back(deleteMarker); + + for (const auto &t : swingTrajectoryPlannerPtr_->getTargetTrajectories().timeTrajectory) + { + if (t < initTime || t > finalTime) + { + continue; + } + + const auto &footPhase = swingTrajectoryPlannerPtr_->getFootPhase(leg, t); + SwingNode3d node{t, footPhase.getPositionInWorld(t), footPhase.getVelocityInWorld(t)}; + + swingTrajectoriesMessage.markers.push_back(ocs2::getArrowAtPointMsg(arrowScale * node.velocity, node.position, feetColorMap_[leg])); + swingTrajectoriesMessage.markers.back().scale.x = 0.005; // shaft diameter + swingTrajectoriesMessage.markers.back().scale.y = 0.01; // arrow-head diameter + swingTrajectoriesMessage.markers.back().scale.z = 0.02; // arrow-head length } - const auto& footPhase = swingTrajectoryPlannerPtr_->getFootPhase(leg, t); - SwingNode3d node{t, footPhase.getPositionInWorld(t), footPhase.getVelocityInWorld(t)}; - - swingTrajectoriesMessage.markers.push_back(ocs2::getArrowAtPointMsg(arrowScale * node.velocity, node.position, feetColorMap_[leg])); - swingTrajectoriesMessage.markers.back().scale.x = 0.005; // shaft diameter - swingTrajectoriesMessage.markers.back().scale.y = 0.01; // arrow-head diameter - swingTrajectoriesMessage.markers.back().scale.z = 0.02; // arrow-head length + ocs2::assignHeader(swingTrajectoriesMessage.markers.begin(), swingTrajectoriesMessage.markers.end(), + ocs2::getHeaderMsg(frameId_, timeStamp)); + ocs2::assignIncreasingId(swingTrajectoriesMessage.markers.begin(), swingTrajectoriesMessage.markers.end()); + swingTrajectoryPublishers_[leg]->publish(swingTrajectoriesMessage); } - - ocs2::assignHeader(swingTrajectoriesMessage.markers.begin(), swingTrajectoriesMessage.markers.end(), - ocs2::getHeaderMsg(frameId_, timeStamp)); - ocs2::assignIncreasingId(swingTrajectoriesMessage.markers.begin(), swingTrajectoriesMessage.markers.end()); - swingTrajectoryPublishers_[leg].publish(swingTrajectoriesMessage); } -} -} // namespace switched_model +} // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/TerrainPlaneVisualizer.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/TerrainPlaneVisualizer.cpp index 9d414f10e..51943baef 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/TerrainPlaneVisualizer.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/TerrainPlaneVisualizer.cpp @@ -8,42 +8,49 @@ #include "ocs2_switched_model_interface/terrain/PlaneFitting.h" -namespace switched_model { - -TerrainPlaneVisualizer::TerrainPlaneVisualizer(ros::NodeHandle& nodeHandle) { - terrainPublisher_ = nodeHandle.advertise("/ocs2_anymal/localTerrain", 1, true); -} - -void TerrainPlaneVisualizer::update(scalar_t time, const TerrainPlane& terrainPlane) { - // Headers - Eigen::Quaterniond terrainOrientation(terrainPlane.orientationWorldToTerrain.transpose()); - auto planeMsg = - getPlaneMsg(terrainPlane.positionInWorld, terrainOrientation, ocs2::Color::black, planeWidth_, planeLength_, planeThickness_); - planeMsg.header = ocs2::getHeaderMsg(frameId_, ros::Time::now()); - planeMsg.id = 0; - planeMsg.color.a = planeAlpha_; - - terrainPublisher_.publish(planeMsg); -} - -TerrainPlaneVisualizerSynchronizedModule::TerrainPlaneVisualizerSynchronizedModule(const SwingTrajectoryPlanner& swingTrajectoryPlanner, - ros::NodeHandle& nodeHandle) - : swingTrajectoryPlanner_(&swingTrajectoryPlanner), planeVisualizer_(nodeHandle) {} - -void TerrainPlaneVisualizerSynchronizedModule::postSolverRun(const ocs2::PrimalSolution& primalSolution) { - std::vector regressionPoints; - for (size_t leg = 0; leg < NUM_CONTACT_POINTS; ++leg) { - const auto& footholds = swingTrajectoryPlanner_->getNominalFootholds(leg); - if (!footholds.empty()) { - regressionPoints.push_back(footholds.front().plane.positionInWorld); - } +namespace switched_model +{ + + TerrainPlaneVisualizer::TerrainPlaneVisualizer(const rclcpp::Node::SharedPtr &node) : node_(node) + { + terrainPublisher_ = node_->create_publisher("/ocs2_anymal/localTerrain", 1); + } + + void TerrainPlaneVisualizer::update(scalar_t time, const TerrainPlane &terrainPlane) + { + // Headers + Eigen::Quaterniond terrainOrientation(terrainPlane.orientationWorldToTerrain.transpose()); + auto planeMsg = + getPlaneMsg(terrainPlane.positionInWorld, terrainOrientation, ocs2::Color::black, planeWidth_, planeLength_, planeThickness_); + planeMsg.header = ocs2::getHeaderMsg(frameId_, node_->get_clock()->now()); + planeMsg.id = 0; + planeMsg.color.a = planeAlpha_; + + terrainPublisher_->publish(planeMsg); } - if (regressionPoints.size() >= 3) { - const auto normalAndPosition = estimatePlane(regressionPoints); - TerrainPlane plane(normalAndPosition.position, orientationWorldToTerrainFromSurfaceNormalInWorld(normalAndPosition.normal)); - planeVisualizer_.update(primalSolution.timeTrajectory_.front(), plane); + TerrainPlaneVisualizerSynchronizedModule::TerrainPlaneVisualizerSynchronizedModule(const SwingTrajectoryPlanner &swingTrajectoryPlanner, + const rclcpp::Node::SharedPtr &node) + : swingTrajectoryPlanner_(&swingTrajectoryPlanner), planeVisualizer_(node) {} + + void TerrainPlaneVisualizerSynchronizedModule::postSolverRun(const ocs2::PrimalSolution &primalSolution) + { + std::vector regressionPoints; + for (size_t leg = 0; leg < NUM_CONTACT_POINTS; ++leg) + { + const auto &footholds = swingTrajectoryPlanner_->getNominalFootholds(leg); + if (!footholds.empty()) + { + regressionPoints.push_back(footholds.front().plane.positionInWorld); + } + } + + if (regressionPoints.size() >= 3) + { + const auto normalAndPosition = estimatePlane(regressionPoints); + TerrainPlane plane(normalAndPosition.position, orientationWorldToTerrainFromSurfaceNormalInWorld(normalAndPosition.normal)); + planeVisualizer_.update(primalSolution.timeTrajectory_.front(), plane); + } } -} -} // namespace switched_model +} // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/TerrainReceiver.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/TerrainReceiver.cpp index 99b0c55df..5f071a6f5 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/TerrainReceiver.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/TerrainReceiver.cpp @@ -7,8 +7,8 @@ namespace switched_model { TerrainReceiverSynchronizedModule::TerrainReceiverSynchronizedModule(ocs2::Synchronized& terrainModel, - ros::NodeHandle& nodeHandle) - : terrainModelPtr_(&terrainModel), segmentedPlanesRos_(new switched_model::SegmentedPlanesTerrainModelRos(nodeHandle)) {} + const rclcpp::Node::SharedPtr &node) + : terrainModelPtr_(&terrainModel), segmentedPlanesRos_(new switched_model::SegmentedPlanesTerrainModelRos(node)) {} void TerrainReceiverSynchronizedModule::preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, const ocs2::ReferenceManagerInterface& referenceManager) { diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/CMakeLists.txt index 92331cf9c..badf8910c 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/CMakeLists.txt @@ -1,42 +1,35 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) +set(CMAKE_CXX_STANDARD 17) project(ocs2_quadruped_loopshaping_interface) -## Catkin Dependencies -set(CATKIN_PACKAGE_DEPENDENCIES +## ament_cmake Dependencies +set(dependencies ocs2_quadruped_interface ) -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} - ) +find_package(ament_cmake REQUIRED) +find_package(ocs2_quadruped_interface REQUIRED) ## Eigen3 find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(PCL REQUIRED) +find_package(Boost REQUIRED COMPONENTS + system + filesystem + log_setup + log +) + -# 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 -) ########### ## Build ## ########### include_directories( - include + include ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} ) # Declare a C++ library @@ -46,39 +39,26 @@ add_library(${PROJECT_NAME} src/QuadrupedLoopshapingMpc.cpp src/QuadrupedLoopshapingMpcNode.cpp ) -add_dependencies(${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if (cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -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 + INCLUDES DESTINATION include/${PROJECT_NAME} ) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) +ament_export_dependencies(${dependencies}) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/include/ocs2_quadruped_loopshaping_interface/QuadrupedLoopshapingDummyNode.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/include/ocs2_quadruped_loopshaping_interface/QuadrupedLoopshapingDummyNode.h index 7ffd10078..27560bca1 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/include/ocs2_quadruped_loopshaping_interface/QuadrupedLoopshapingDummyNode.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/include/ocs2_quadruped_loopshaping_interface/QuadrupedLoopshapingDummyNode.h @@ -4,13 +4,13 @@ #pragma once -#include +#include "rclcpp/rclcpp.hpp" #include "QuadrupedLoopshapingInterface.h" namespace switched_model_loopshaping { -void quadrupedLoopshapingDummyNode(ros::NodeHandle& nodeHandle, const QuadrupedLoopshapingInterface& quadrupedInterface, +void quadrupedLoopshapingDummyNode(const rclcpp::Node::SharedPtr &node, const QuadrupedLoopshapingInterface& quadrupedInterface, double mrtDesiredFrequency, double mpcDesiredFrequency); } // namespace switched_model_loopshaping diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/include/ocs2_quadruped_loopshaping_interface/QuadrupedLoopshapingMpcNode.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/include/ocs2_quadruped_loopshaping_interface/QuadrupedLoopshapingMpcNode.h index 5f8c9ed36..261ed7ff2 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/include/ocs2_quadruped_loopshaping_interface/QuadrupedLoopshapingMpcNode.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/include/ocs2_quadruped_loopshaping_interface/QuadrupedLoopshapingMpcNode.h @@ -4,7 +4,7 @@ #pragma once -#include +#include "rclcpp/rclcpp.hpp" #include @@ -12,7 +12,7 @@ namespace switched_model_loopshaping { -void quadrupedLoopshapingMpcNode(ros::NodeHandle& nodeHandle, const QuadrupedLoopshapingInterface& quadrupedInterface, +void quadrupedLoopshapingMpcNode(const rclcpp::Node::SharedPtr &node, const QuadrupedLoopshapingInterface& quadrupedInterface, std::unique_ptr mpcPtr); } // namespace switched_model_loopshaping \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/package.xml b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/package.xml index 027573941..bb923f366 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/package.xml +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/package.xml @@ -12,8 +12,11 @@ TODO - catkin + ament_cmake ocs2_quadruped_interface + + ament_cmake + \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/src/QuadrupedLoopshapingDummyNode.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/src/QuadrupedLoopshapingDummyNode.cpp index 84937a7ff..a5bbf6bb6 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/src/QuadrupedLoopshapingDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/src/QuadrupedLoopshapingDummyNode.cpp @@ -16,18 +16,18 @@ namespace switched_model_loopshaping { -void quadrupedLoopshapingDummyNode(ros::NodeHandle& nodeHandle, const QuadrupedLoopshapingInterface& quadrupedInterface, +void quadrupedLoopshapingDummyNode(const rclcpp::Node::SharedPtr &node, const QuadrupedLoopshapingInterface& quadrupedInterface, double mrtDesiredFrequency, double mpcDesiredFrequency) { const std::string robotName = "anymal"; // MRT ocs2::MRT_ROS_Interface mrt(robotName); mrt.initRollout(&quadrupedInterface.getRollout()); - mrt.launchNodes(nodeHandle); + mrt.launchNodes(node); // Visualization auto visualizer = std::make_shared( - quadrupedInterface.getKinematicModel(), quadrupedInterface.getJointNames(), quadrupedInterface.getBaseName(), nodeHandle); + quadrupedInterface.getKinematicModel(), quadrupedInterface.getJointNames(), quadrupedInterface.getBaseName(), node); // Logging std::string logFileName = "/tmp/ocs2/QuadrupedLoopshapingDummyNodeLog.txt"; diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/src/QuadrupedLoopshapingMpcNode.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/src/QuadrupedLoopshapingMpcNode.cpp index b9143e3a1..83eb8eb0f 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/src/QuadrupedLoopshapingMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/src/QuadrupedLoopshapingMpcNode.cpp @@ -15,7 +15,7 @@ namespace switched_model_loopshaping { -void quadrupedLoopshapingMpcNode(ros::NodeHandle& nodeHandle, const QuadrupedLoopshapingInterface& quadrupedInterface, +void quadrupedLoopshapingMpcNode(const rclcpp::Node::SharedPtr &node, const QuadrupedLoopshapingInterface& quadrupedInterface, std::unique_ptr mpcPtr) { const std::string robotName = "anymal"; @@ -23,27 +23,27 @@ void quadrupedLoopshapingMpcNode(ros::NodeHandle& nodeHandle, const QuadrupedLoo // Gait auto gaitReceiver = std::make_shared( - nodeHandle, quadrupedInterface.getQuadrupedInterface().getSwitchedModelModeScheduleManagerPtr()->getGaitSchedule(), robotName); + node, quadrupedInterface.getQuadrupedInterface().getSwitchedModelModeScheduleManagerPtr()->getGaitSchedule(), robotName); loopshapingSolverModule->add(gaitReceiver); // Terrain Receiver auto terrainReceiver = std::make_shared( - quadrupedInterface.getQuadrupedInterface().getSwitchedModelModeScheduleManagerPtr()->getTerrainModel(), nodeHandle); + quadrupedInterface.getQuadrupedInterface().getSwitchedModelModeScheduleManagerPtr()->getTerrainModel(), node); loopshapingSolverModule->add(terrainReceiver); // Terrain plane visualization auto terrainVisualizer = std::make_shared( - quadrupedInterface.getQuadrupedInterface().getSwitchedModelModeScheduleManagerPtr()->getSwingTrajectoryPlanner(), nodeHandle); + quadrupedInterface.getQuadrupedInterface().getSwitchedModelModeScheduleManagerPtr()->getSwingTrajectoryPlanner(), node); loopshapingSolverModule->add(terrainVisualizer); // Swing planner auto swingPlanningVisualizer = std::make_shared( - quadrupedInterface.getQuadrupedInterface().getSwitchedModelModeScheduleManagerPtr()->getSwingTrajectoryPlanner(), nodeHandle); + quadrupedInterface.getQuadrupedInterface().getSwitchedModelModeScheduleManagerPtr()->getSwingTrajectoryPlanner(), node); loopshapingSolverModule->add(swingPlanningVisualizer); // reference manager auto rosReferenceManagerPtr = std::make_shared(robotName, quadrupedInterface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); + rosReferenceManagerPtr->subscribe(node); mpcPtr->getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // MPC @@ -51,7 +51,7 @@ void quadrupedLoopshapingMpcNode(ros::NodeHandle& nodeHandle, const QuadrupedLoo // launch MPC nodes ocs2::MPC_ROS_Interface mpcNode(*mpcPtr, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); } } // namespace switched_model_loopshaping diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/CMakeLists.txt index 534994e69..b437ea3ff 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/CMakeLists.txt @@ -1,9 +1,10 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) +set(CMAKE_CXX_STANDARD 17) project(ocs2_switched_model_interface) -## Catkin Dependencies -set(CATKIN_PACKAGE_DEPENDENCIES - roscpp +## ament_cmake Dependencies +set(dependencies + rclcpp ocs2_core ocs2_msgs ocs2_oc @@ -11,37 +12,34 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_robotic_tools ocs2_switched_model_msgs ) -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_msgs REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_switched_model_msgs REQUIRED) +find_package(rclcpp REQUIRED) ## Eigen3 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 - ${CATKIN_PACKAGE_DEPENDENCIES} -# DEPENDS +find_package(Boost REQUIRED COMPONENTS + system + filesystem + log_setup + log ) + + + ########### ## Build ## ########### include_directories( include ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} ) add_library(${PROJECT_NAME} @@ -87,84 +85,77 @@ add_library(${PROJECT_NAME} src/core/SwitchedModelPrecomputation.cpp src/core/TorqueApproximation.cpp ) -add_dependencies(${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - dl +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -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(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include/${PROJECT_NAME} ) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) + ############# ## Testing ## ############# +find_package(ament_lint_auto REQUIRED) +ament_lint_auto_find_test_dependencies() +find_package(ament_cmake_gtest) -catkin_add_gtest(test_constraints +ament_add_gtest(test_constraints test/constraint/testZeroForceConstraint.cpp test/constraint/testFrictionConeConstraint.cpp ) +ament_target_dependencies(test_constraints + ${dependencies} +) target_link_libraries(test_constraints ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main ) -catkin_add_gtest(test_${PROJECT_NAME}_core +ament_add_gtest(test_${PROJECT_NAME}_core test/core/testRotation.cpp ) +ament_target_dependencies(test_${PROJECT_NAME}_core + ${dependencies} +) target_link_libraries(test_${PROJECT_NAME}_core ${PROJECT_NAME} - gtest_main ) -catkin_add_gtest(test_${PROJECT_NAME}_cost +ament_add_gtest(test_${PROJECT_NAME}_cost test/cost/testFootplacementCost.cpp test/cost/testFrictionConeCost.cpp test/cost/testTorqueLimitsSoftConstraint.cpp ) +ament_target_dependencies(test_${PROJECT_NAME}_cost + ${dependencies} +) target_link_libraries(test_${PROJECT_NAME}_cost ${PROJECT_NAME} - gtest_main ) -catkin_add_gtest(test_${PROJECT_NAME}_footplanner +ament_add_gtest(test_${PROJECT_NAME}_footplanner test/foot_planner/testSwingPhase.cpp ) +ament_target_dependencies(test_${PROJECT_NAME}_footplanner + ${dependencies} +) target_link_libraries(test_${PROJECT_NAME}_footplanner ${PROJECT_NAME} - gtest_main ) -catkin_add_gtest(test_${PROJECT_NAME}_logic +ament_add_gtest(test_${PROJECT_NAME}_logic test/logic/testEarlyTouchDown.cpp test/logic/testExtractContractTimings.cpp test/logic/testGait.cpp @@ -172,23 +163,34 @@ catkin_add_gtest(test_${PROJECT_NAME}_logic test/logic/testGaitSchedule.cpp test/logic/testSingleLegLogic.cpp ) +ament_target_dependencies(test_${PROJECT_NAME}_logic + ${dependencies} +) target_link_libraries(test_${PROJECT_NAME}_logic ${PROJECT_NAME} - gtest_main ) -catkin_add_gtest(test_${PROJECT_NAME}_terrain +ament_add_gtest(test_${PROJECT_NAME}_terrain test/terrain/testTerrainPlane.cpp ) +ament_target_dependencies(test_${PROJECT_NAME}_terrain + ${dependencies} +) target_link_libraries(test_${PROJECT_NAME}_terrain ${PROJECT_NAME} - gtest_main ) -catkin_add_gtest(test_${PROJECT_NAME}_convexTerrain +ament_add_gtest(test_${PROJECT_NAME}_convexTerrain test/terrain/testConvexTerrain.cpp ) +ament_target_dependencies(test_${PROJECT_NAME}_convexTerrain + ${dependencies} +) target_link_libraries(test_${PROJECT_NAME}_convexTerrain ${PROJECT_NAME} - gtest_main ) + +ament_export_dependencies(${dependencies}) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/Gait.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/Gait.h index 643a4b37d..67433f340 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/Gait.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/Gait.h @@ -14,15 +14,17 @@ namespace switched_model { /** * A gait is a periodic mode schedule parameterized by a "phase" variable. * - * The eventPhases only indicate switches of modes, i.e. phase = 0 and phase = 1 are not part of the eventPhases. - * The number of modes is therefore number of phases + 1 + * The eventPhases only indicate switches of modes, i.e. phase = 0 and phase = + * 1 are not part of the eventPhases. The number of modes is therefore number + * of phases + 1 * * The conversion to time is regulated by a duration */ struct Gait { /** time for one gait cycle*/ scalar_t duration; - /** points in (0.0, 1.0) along the gait cycle where the contact mode changes, size N-1 */ + /** points in (0.0, 1.0) along the gait cycle where the contact mode changes, + * size N-1 */ std::vector eventPhases; /** sequence of contact modes, size: N */ std::vector modeSequence; diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/GaitAdaptation.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/GaitAdaptation.h index d206375ee..8559cde0f 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/GaitAdaptation.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/GaitAdaptation.h @@ -62,7 +62,7 @@ class GaitAdaptation { std::function earlyTouchDownAdaptation(const switched_model::feet_array_t& earlyTouchDownPerLeg); -/** Gets the mode index of the next phase with the specified contact state, returns the size of the modesequence of no such contact state +/** Gets the mode index of the next phase with the specified contact state, returns the size of the modeSequence of no such contact state * was found */ int getModeIndexOfNextContactStateOfLeg(bool contact, int startModeIdx, size_t leg, const Gait& gait); diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/GaitReceiver.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/GaitReceiver.h index 060908fa8..8723243dc 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/GaitReceiver.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/GaitReceiver.h @@ -4,45 +4,54 @@ #pragma once -#include - -#include - #include -#include #include -#include +#include +#include +#include #include "ocs2_switched_model_interface/core/SwitchedModel.h" #include "ocs2_switched_model_interface/logic/GaitSchedule.h" +#include "rclcpp/rclcpp.hpp" namespace switched_model { class GaitReceiver : public ocs2::SolverSynchronizedModule { public: - GaitReceiver(ros::NodeHandle nodeHandle, ocs2::Synchronized& gaitSchedule, const std::string& robotName); + GaitReceiver(const rclcpp::Node::SharedPtr& node, + ocs2::Synchronized& gaitSchedule, + const std::string& robotName); - void preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, - const ocs2::ReferenceManagerInterface& referenceManager) override; + void preSolverRun( + scalar_t initTime, scalar_t finalTime, const vector_t& currentState, + const ocs2::ReferenceManagerInterface& referenceManager) override; void postSolverRun(const ocs2::PrimalSolution& primalSolution) override{}; private: - void mpcModeSequenceCallback(const ocs2_msgs::mode_schedule::ConstPtr& msg); - void mpcModeScheduledGaitCallback(const ocs2_msgs::mode_schedule::ConstPtr& msg); - void mpcGaitSequenceCallback(const ocs2_switched_model_msgs::scheduled_gait_sequenceConstPtr& msg); - - ros::Subscriber mpcModeSequenceSubscriber_; - ros::Subscriber mpcScheduledModeSequenceSubscriber_; - ros::Subscriber mpcGaitSequenceSubscriber_; + void mpcModeSequenceCallback( + const ocs2_msgs::msg::ModeSchedule::ConstSharedPtr& msg); + void mpcModeScheduledGaitCallback( + const ocs2_msgs::msg::ModeSchedule::ConstSharedPtr& msg); + void mpcGaitSequenceCallback( + const ocs2_switched_model_msgs::msg::ScheduledGaitSequence::ConstSharedPtr& + msg); + + rclcpp::Subscription::SharedPtr + mpcModeSequenceSubscriber_; + rclcpp::Subscription::SharedPtr + mpcScheduledModeSequenceSubscriber_; + rclcpp::Subscription:: + SharedPtr mpcGaitSequenceSubscriber_; ocs2::Synchronized* gaitSchedulePtr_; std::atomic_bool gaitUpdated_; std::mutex receivedGaitMutex_; // protects the setGaitAction_ variable - std::function setGaitAction_; }; diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/ModeSequenceTemplate.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/ModeSequenceTemplate.h index a1b20f67d..41d991f57 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/ModeSequenceTemplate.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/logic/ModeSequenceTemplate.h @@ -1,12 +1,11 @@ #pragma once +#include + #include +#include #include -#include - -#include - #include "ocs2_switched_model_interface/core/MotionPhaseDefinition.h" #include "ocs2_switched_model_interface/core/SwitchedModel.h" #include "ocs2_switched_model_interface/logic/Gait.h" @@ -15,24 +14,28 @@ namespace switched_model { /** * ModeSequenceTemplate describes a periodic sequence of modes. It is defined by - * - switching times (size N+1), where the first time is 0, and the last time denotes the period of the cycle + * - switching times (size N+1), where the first time is 0, and the last time + * denotes the period of the cycle * - modeSequence (size N), indicating the mode between the switching times. */ struct ModeSequenceTemplate { /** - * Constructor for a ModeSequenceTemplate. The number of modes must be greater than zero (N > 0) + * Constructor for a ModeSequenceTemplate. The number of modes must be greater + * than zero (N > 0) * @param [in] switchingTimesInput : switching times of size N + 1 * @param [in] modeSequenceInput : mode sequence of size N */ - ModeSequenceTemplate(std::vector switchingTimesInput, std::vector modeSequenceInput) - : switchingTimes(std::move(switchingTimesInput)), modeSequence(std::move(modeSequenceInput)) { + ModeSequenceTemplate(std::vector switchingTimesInput, + std::vector modeSequenceInput) + : switchingTimes(std::move(switchingTimesInput)), + modeSequence(std::move(modeSequenceInput)) { assert(!modeSequence.empty()); assert(switchingTimes.size() == modeSequence.size() + 1); } /** - * Defined as [t_0=0, t_1, .., t_n, t_(n+1)=T], where T is the overall duration - * of the template logic. t_1 to t_n are the event moments. + * Defined as [t_0=0, t_1, .., t_n, t_(n+1)=T], where T is the overall + * duration of the template logic. t_1 to t_n are the event moments. */ std::vector switchingTimes; @@ -43,26 +46,30 @@ struct ModeSequenceTemplate { std::vector modeSequence; }; -/** Swap two modesequence templates */ +/** Swap two modeSequence templates */ inline void swap(ModeSequenceTemplate& lh, ModeSequenceTemplate& rh) { lh.switchingTimes.swap(rh.switchingTimes); lh.modeSequence.swap(rh.modeSequence); } -/** Print the modesequence template */ -std::ostream& operator<<(std::ostream& stream, const ModeSequenceTemplate& modeSequenceTemplate); +/** Print the modeSequence template */ +std::ostream& operator<<(std::ostream& stream, + const ModeSequenceTemplate& ModeSequenceTemplate); /** Convert mode sequence template to ROS message */ -ocs2_msgs::mode_schedule createModeSequenceTemplateMsg(const ModeSequenceTemplate& modeSequenceTemplate); +ocs2_msgs::msg::ModeSchedule createModeSequenceTemplateMsg( + const ModeSequenceTemplate& ModeSequenceTemplate); /** Convert ROS message to mode sequence template */ -ModeSequenceTemplate readModeSequenceTemplateMsg(const ocs2_msgs::mode_schedule& modeScheduleMsg); +ModeSequenceTemplate readModeSequenceTemplateMsg( + const ocs2_msgs::msg::ModeSchedule& modeScheduleMsg); /** Converts a mode sequence template to a gait */ -Gait toGait(const ModeSequenceTemplate& modeSequenceTemplate); +Gait toGait(const ModeSequenceTemplate& ModeSequenceTemplate); /** - * Load a modesequence template from file. The template needs to be declared as: + * Load a modeSequence template from file. The template needs to be declared + * as: * * topicName * { @@ -79,10 +86,13 @@ Gait toGait(const ModeSequenceTemplate& modeSequenceTemplate); * } * } */ -ModeSequenceTemplate loadModeSequenceTemplate(const std::string& filename, const std::string& topicName, bool verbose = true); +ModeSequenceTemplate loadModeSequenceTemplate(const std::string& filename, + const std::string& topicName, + bool verbose = true); /** - * Load a mode schedule template from file. The schedule needs to be declared as: + * Load a mode schedule template from file. The schedule needs to be declared + * as: * * topicName * { @@ -99,6 +109,7 @@ ModeSequenceTemplate loadModeSequenceTemplate(const std::string& filename, const * } * } */ -ocs2::ModeSchedule loadModeSchedule(const std::string& filename, const std::string& topicName, bool verbose); +ocs2::ModeSchedule loadModeSchedule(const std::string& filename, + const std::string& topicName, bool verbose); } // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/ros_msg_conversions/RosMsgConversions.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/ros_msg_conversions/RosMsgConversions.h index d6aa3a4ce..a6a0d56ef 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/ros_msg_conversions/RosMsgConversions.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/include/ocs2_switched_model_interface/ros_msg_conversions/RosMsgConversions.h @@ -11,10 +11,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include "ocs2_switched_model_interface/logic/Gait.h" #include "ocs2_switched_model_interface/logic/GaitSchedule.h" @@ -22,14 +22,18 @@ namespace switched_model { namespace ros_msg_conversions { -ocs2_switched_model_msgs::gait toMessage(const Gait& gait); -Gait fromMessage(const ocs2_switched_model_msgs::gait& msg); +ocs2_switched_model_msgs::msg::Gait toMessage(const Gait& gait); +Gait fromMessage(const ocs2_switched_model_msgs::msg::Gait& msg); -ocs2_switched_model_msgs::gait_sequence toMessage(const GaitSchedule::GaitSequence& gaitSequence); -GaitSchedule::GaitSequence fromMessage(const ocs2_switched_model_msgs::gait_sequence& msg); +ocs2_switched_model_msgs::msg::GaitSequence toMessage( + const GaitSchedule::GaitSequence& gaitSequence); +GaitSchedule::GaitSequence fromMessage( + const ocs2_switched_model_msgs::msg::GaitSequence& msg); -ocs2_switched_model_msgs::scheduled_gait_sequence toMessage(scalar_t startTime, const GaitSchedule::GaitSequence& gaitSequence); -std::pair fromMessage(const ocs2_switched_model_msgs::scheduled_gait_sequence& msg); +ocs2_switched_model_msgs::msg::ScheduledGaitSequence toMessage( + scalar_t startTime, const GaitSchedule::GaitSequence& gaitSequence); +std::pair fromMessage( + const ocs2_switched_model_msgs::msg::ScheduledGaitSequence& msg); } // namespace ros_msg_conversions } // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/package.xml b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/package.xml index 3b4aff970..73388beb6 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/package.xml +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/package.xml @@ -10,11 +10,9 @@ TODO - catkin + ament_cmake - cmake_clang_tools - - roscpp + rclcpp ocs2_core ocs2_oc ocs2_msgs @@ -22,4 +20,7 @@ ocs2_robotic_tools ocs2_switched_model_msgs + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/Gait.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/Gait.cpp index a834621b0..506e6e89c 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/Gait.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/Gait.cpp @@ -3,7 +3,6 @@ // #include "ocs2_switched_model_interface/logic/Gait.h" -#include "ocs2_switched_model_interface/core/MotionPhaseDefinition.h" #include @@ -11,20 +10,23 @@ #include #include +#include "ocs2_switched_model_interface/core/MotionPhaseDefinition.h" + namespace switched_model { bool isValidGait(const Gait& gait) { bool validGait = true; validGait &= gait.duration > 0.0; - validGait &= std::all_of(gait.eventPhases.begin(), gait.eventPhases.end(), [](scalar_t phase) { return 0.0 < phase && phase < 1.0; }); - validGait &= std::is_sorted(gait.eventPhases.begin(), gait.eventPhases.end()); + validGait &= + std::all_of(gait.eventPhases.begin(), gait.eventPhases.end(), + [](scalar_t phase) { return 0.0 < phase && phase < 1.0; }); + validGait &= + std::is_sorted(gait.eventPhases.begin(), gait.eventPhases.end()); validGait &= gait.eventPhases.size() + 1 == gait.modeSequence.size(); return validGait; } -bool isValidPhase(scalar_t phase) { - return phase >= 0.0 && phase < 1.0; -} +bool isValidPhase(scalar_t phase) { return phase >= 0.0 && phase < 1.0; } scalar_t wrapPhase(scalar_t phase) { phase = std::fmod(phase, 1.0); @@ -37,7 +39,8 @@ scalar_t wrapPhase(scalar_t phase) { int getModeIndexFromPhase(scalar_t phase, const Gait& gait) { assert(isValidPhase(phase)); assert(isValidGait(gait)); - auto firstLargerValueIterator = std::upper_bound(gait.eventPhases.begin(), gait.eventPhases.end(), phase); + auto firstLargerValueIterator = std::upper_bound( + gait.eventPhases.begin(), gait.eventPhases.end(), phase); return static_cast(firstLargerValueIterator - gait.eventPhases.begin()); } @@ -66,8 +69,10 @@ scalar_t timeLeftInMode(scalar_t phase, const Gait& gait) { std::ostream& operator<<(std::ostream& stream, const Gait& gait) { stream << "Duration: " << gait.duration << "\n"; - stream << "Event phases: {" << ocs2::toDelimitedString(gait.eventPhases) << "}\n"; - stream << "Mode sequence: {" << ocs2::toDelimitedString(gait.modeSequence) << "}\n"; + stream << "Event phases: {" << ocs2::toDelimitedString(gait.eventPhases) + << "}\n"; + stream << "Mode sequence: {" << ocs2::toDelimitedString(gait.modeSequence) + << "}\n"; return stream; } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/GaitReceiver.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/GaitReceiver.cpp index a18755644..d41bb7794 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/GaitReceiver.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/GaitReceiver.cpp @@ -3,80 +3,104 @@ // #include "ocs2_switched_model_interface/logic/GaitReceiver.h" + #include "ocs2_switched_model_interface/core/MotionPhaseDefinition.h" #include "ocs2_switched_model_interface/logic/ModeSequenceTemplate.h" #include "ocs2_switched_model_interface/ros_msg_conversions/RosMsgConversions.h" namespace switched_model { -GaitReceiver::GaitReceiver(ros::NodeHandle nodeHandle, ocs2::Synchronized& gaitSchedule, const std::string& robotName) +GaitReceiver::GaitReceiver(const rclcpp::Node::SharedPtr& node, + ocs2::Synchronized& gaitSchedule, + const std::string& robotName) : gaitSchedulePtr_(&gaitSchedule), gaitUpdated_(false) { - mpcModeSequenceSubscriber_ = nodeHandle.subscribe(robotName + "_mpc_mode_schedule", 1, &GaitReceiver::mpcModeSequenceCallback, this, - ::ros::TransportHints().udp()); - mpcScheduledModeSequenceSubscriber_ = nodeHandle.subscribe( - robotName + "_mpc_scheduled_mode_schedule", 1, &GaitReceiver::mpcModeScheduledGaitCallback, this, ::ros::TransportHints().udp()); - mpcGaitSequenceSubscriber_ = nodeHandle.subscribe(robotName + "_mpc_gait_schedule", 1, &GaitReceiver::mpcGaitSequenceCallback, this, - ::ros::TransportHints().udp()); + mpcModeSequenceSubscriber_ = + node->create_subscription( + robotName + "_mpc_mode_schedule", 1, + std::bind(&GaitReceiver::mpcModeSequenceCallback, this, + std::placeholders::_1)); + mpcScheduledModeSequenceSubscriber_ = + node->create_subscription( + robotName + "_mpc_scheduled_mode_schedule", 1, + std::bind(&GaitReceiver::mpcModeScheduledGaitCallback, this, + std::placeholders::_1)); + mpcGaitSequenceSubscriber_ = node->create_subscription< + ocs2_switched_model_msgs::msg::ScheduledGaitSequence>( + robotName + "_mpc_gait_schedule", 1, + std::bind(&GaitReceiver::mpcGaitSequenceCallback, this, + std::placeholders::_1)); } -void GaitReceiver::preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, - const ocs2::ReferenceManagerInterface& referenceManager) { +void GaitReceiver::preSolverRun( + scalar_t initTime, scalar_t finalTime, const vector_t& currentState, + const ocs2::ReferenceManagerInterface& referenceManager) { if (gaitUpdated_) { { std::lock_guard lock(receivedGaitMutex_); - setGaitAction_(initTime, finalTime, currentState, referenceManager.getTargetTrajectories()); + setGaitAction_(initTime, finalTime, currentState, + referenceManager.getTargetTrajectories()); } std::cout << std::endl; gaitUpdated_ = false; } } -void GaitReceiver::mpcModeSequenceCallback(const ocs2_msgs::mode_schedule::ConstPtr& msg) { - const auto modeSequenceTemplate = readModeSequenceTemplateMsg(*msg); - const auto gait = toGait(modeSequenceTemplate); +void GaitReceiver::mpcModeSequenceCallback( + const ocs2_msgs::msg::ModeSchedule::ConstSharedPtr& msg) { + const auto ModeSequenceTemplate = readModeSequenceTemplateMsg(*msg); + const auto gait = toGait(ModeSequenceTemplate); { std::lock_guard lock(receivedGaitMutex_); - setGaitAction_ = [=](scalar_t initTime, scalar_t finalTime, const vector_t& currentState, + setGaitAction_ = [=](scalar_t initTime, scalar_t finalTime, + const vector_t& currentState, const ocs2::TargetTrajectories& targetTrajectories) { - std::cout << "[GaitReceiver]: Setting new gait after time " << finalTime << "\n[GaitReceiver]: " << gait; + std::cout << "[GaitReceiver]: Setting new gait after time " << finalTime + << "\n[GaitReceiver]: " << gait; this->gaitSchedulePtr_->lock()->setGaitAfterTime(gait, finalTime); }; gaitUpdated_ = true; } } -void GaitReceiver::mpcModeScheduledGaitCallback(const ocs2_msgs::mode_schedule::ConstPtr& msg) { - const auto modeSequenceTemplate = readModeSequenceTemplateMsg(*msg); - const auto gait = toGait(modeSequenceTemplate); - const auto scheduledGaitTime = modeSequenceTemplate.switchingTimes.front(); +void GaitReceiver::mpcModeScheduledGaitCallback( + const ocs2_msgs::msg::ModeSchedule::ConstSharedPtr& msg) { + const auto ModeSequenceTemplate = readModeSequenceTemplateMsg(*msg); + const auto gait = toGait(ModeSequenceTemplate); + const auto scheduledGaitTime = ModeSequenceTemplate.switchingTimes.front(); std::cout << "ScheduledGaitCallback:\n"; std::cout << "\nReceivedGait:\n" << gait << "\n"; { std::lock_guard lock(receivedGaitMutex_); - setGaitAction_ = [=](scalar_t initTime, scalar_t finalTime, const vector_t& currentState, + setGaitAction_ = [=](scalar_t initTime, scalar_t finalTime, + const vector_t& currentState, const ocs2::TargetTrajectories& targetTrajectories) { - std::cout << "[GaitReceiver]: Received new scheduled gait, setting it at time " << scheduledGaitTime << ", current time: " << initTime - << "\n[GaitReceiver]: " << gait; + std::cout + << "[GaitReceiver]: Received new scheduled gait, setting it at time " + << scheduledGaitTime << ", current time: " << initTime + << "\n[GaitReceiver]: " << gait; this->gaitSchedulePtr_->lock()->setGaitAtTime(gait, scheduledGaitTime); }; gaitUpdated_ = true; } } -void GaitReceiver::mpcGaitSequenceCallback(const ocs2_switched_model_msgs::scheduled_gait_sequenceConstPtr& msg) { +void GaitReceiver::mpcGaitSequenceCallback( + const ocs2_switched_model_msgs::msg::ScheduledGaitSequence::ConstSharedPtr& + msg) { const auto scheduledGaitSequence = ros_msg_conversions::fromMessage(*msg); std::cout << "ScheduledGaitCallback:\n"; - std::cout << *msg << std::endl; { std::lock_guard lock(receivedGaitMutex_); - setGaitAction_ = [=](scalar_t initTime, scalar_t finalTime, const vector_t& currentState, + setGaitAction_ = [=](scalar_t initTime, scalar_t finalTime, + const vector_t& currentState, const ocs2::TargetTrajectories& targetTrajectories) { - this->gaitSchedulePtr_->lock()->setGaitSequenceAtTime(scheduledGaitSequence.second, scheduledGaitSequence.first); + this->gaitSchedulePtr_->lock()->setGaitSequenceAtTime( + scheduledGaitSequence.second, scheduledGaitSequence.first); }; gaitUpdated_ = true; } diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/GaitSchedule.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/GaitSchedule.cpp index b84af1dd4..a3d992dbd 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/GaitSchedule.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/GaitSchedule.cpp @@ -9,7 +9,8 @@ namespace switched_model { -GaitSchedule::GaitSchedule(scalar_t time, Gait gait) : time_(time), phase_(0.0), gaitSchedule_{std::move(gait)} {} +GaitSchedule::GaitSchedule(scalar_t time, Gait gait) + : time_(time), phase_(0.0), gaitSchedule_{std::move(gait)} {} void GaitSchedule::advanceToTime(scalar_t time) { if (time <= time_) { @@ -18,7 +19,8 @@ void GaitSchedule::advanceToTime(scalar_t time) { const scalar_t dt = time - time_; std::deque::iterator newActiveGait; - std::tie(phase_, newActiveGait) = advancePhase(phase_, dt, gaitSchedule_.begin(), gaitSchedule_.end()); + std::tie(phase_, newActiveGait) = + advancePhase(phase_, dt, gaitSchedule_.begin(), gaitSchedule_.end()); time_ = time; // Remove gaits that have been completed. @@ -31,62 +33,74 @@ void GaitSchedule::setNextGait(const Gait& gait) { setGaitSequenceAfterCurrentGait({gait}); } -void GaitSchedule::setGaitSequenceAfterCurrentGait(const GaitSequence& gaitSequence) { +void GaitSchedule::setGaitSequenceAfterCurrentGait( + const GaitSequence& gaitSequence) { gaitSchedule_.erase(gaitSchedule_.begin() + 1, gaitSchedule_.end()); - gaitSchedule_.insert(gaitSchedule_.end(), gaitSequence.begin(), gaitSequence.end()); + gaitSchedule_.insert(gaitSchedule_.end(), gaitSequence.begin(), + gaitSequence.end()); } void GaitSchedule::setGaitAtTime(const Gait& gait, scalar_t time) { setGaitSequenceAtTime({gait}, time); } -void GaitSchedule::setGaitSequenceAtTime(const GaitSequence& gaitSequence, scalar_t time) { +void GaitSchedule::setGaitSequenceAtTime(const GaitSequence& gaitSequence, + scalar_t time) { assert(time >= time_); rolloutGaitScheduleTillTime(time); scalar_t newPhase; std::deque::iterator newActiveGait; - std::tie(newPhase, newActiveGait) = advancePhase(phase_, time - time_, gaitSchedule_.begin(), gaitSchedule_.end()); + std::tie(newPhase, newActiveGait) = advancePhase( + phase_, time - time_, gaitSchedule_.begin(), gaitSchedule_.end()); // Shrink the gait that is active at "time", s.t. it ends at "time" if (newActiveGait == gaitSchedule_.begin()) { - // Gait to shrink is already active. Determine new duration from the amount of time and phase left. + // Gait to shrink is already active. Determine new duration from the amount + // of time and phase left. newActiveGait->duration = (time - time_) / (1.0 - phase_); } else { newActiveGait->duration *= newPhase; } // Check if duration was set to zero - if (newActiveGait->duration > 0.0) { // keep newActiveGait and delete the ones after + if (newActiveGait->duration > + 0.0) { // keep newActiveGait and delete the ones after gaitSchedule_.erase(newActiveGait + 1, gaitSchedule_.end()); } else { // delete newActiveGait and the ones after gaitSchedule_.erase(newActiveGait, gaitSchedule_.end()); } - gaitSchedule_.insert(gaitSchedule_.end(), gaitSequence.begin(), gaitSequence.end()); + gaitSchedule_.insert(gaitSchedule_.end(), gaitSequence.begin(), + gaitSequence.end()); } void GaitSchedule::setGaitAfterTime(const Gait& gait, scalar_t time) { setGaitSequenceAfterTime({gait}, time); } -void GaitSchedule::setGaitSequenceAfterTime(const GaitSequence& gaitSequence, scalar_t time) { +void GaitSchedule::setGaitSequenceAfterTime(const GaitSequence& gaitSequence, + scalar_t time) { assert(time >= time_); rolloutGaitScheduleTillTime(time); scalar_t newPhase; std::deque::iterator newActiveGait; - std::tie(newPhase, newActiveGait) = advancePhase(phase_, time - time_, gaitSchedule_.begin(), gaitSchedule_.end()); + std::tie(newPhase, newActiveGait) = advancePhase( + phase_, time - time_, gaitSchedule_.begin(), gaitSchedule_.end()); gaitSchedule_.erase(newActiveGait + 1, gaitSchedule_.end()); - gaitSchedule_.insert(gaitSchedule_.end(), gaitSequence.begin(), gaitSequence.end()); + gaitSchedule_.insert(gaitSchedule_.end(), gaitSequence.begin(), + gaitSequence.end()); } void GaitSchedule::adaptCurrentGait( - const std::function& gaitAdaptor) { - // Repeat the last gait until we have 3 gaits in the schedule. This prevents that the gaitAdaptor modifies the nominal gait. + const std::function& gaitAdaptor) { + // Repeat the last gait until we have 3 gaits in the schedule. This prevents + // that the gaitAdaptor modifies the nominal gait. while (gaitSchedule_.size() < 3) { gaitSchedule_.push_back(gaitSchedule_.back()); } @@ -96,32 +110,40 @@ void GaitSchedule::adaptCurrentGait( } ocs2::ModeSchedule GaitSchedule::getModeSchedule(scalar_t timeHorizon) const { - return ::switched_model::getModeSchedule(phase_, time_, timeHorizon, gaitSchedule_.begin(), gaitSchedule_.end()); + return ::switched_model::getModeSchedule( + phase_, time_, timeHorizon, gaitSchedule_.begin(), gaitSchedule_.end()); } void GaitSchedule::rolloutGaitScheduleTillTime(scalar_t time) { - scalar_t tGaitEnd = time_ + timeLeftInGait(getCurrentPhase(), getCurrentGait()); + scalar_t tGaitEnd = + time_ + timeLeftInGait(getCurrentPhase(), getCurrentGait()); auto gaitIt = gaitSchedule_.begin(); while (tGaitEnd <= time) { if (std::next(gaitIt) == gaitSchedule_.end()) { - // End of the schedule reached: make the repetition of the last gait explicit + // End of the schedule reached: make the repetition of the last gait + // explicit gaitSchedule_.push_back(gaitSchedule_.back()); - gaitIt = std::prev(gaitSchedule_.end(), 2); // Iterator can be invalidated after push_back, so reset set explicitly. + gaitIt = std::prev(gaitSchedule_.end(), + 2); // Iterator can be invalidated after push_back, so + // reset set explicitly. } tGaitEnd += gaitIt->duration; ++gaitIt; } } -bool isStandingDuringTimeHorizon(scalar_t timeHorizon, const GaitSchedule& gaitSchedule) { +bool isStandingDuringTimeHorizon(scalar_t timeHorizon, + const GaitSchedule& gaitSchedule) { const auto modeSchedule = gaitSchedule.getModeSchedule(timeHorizon); - return std::all_of(modeSchedule.modeSequence.begin(), modeSchedule.modeSequence.end(), + return std::all_of(modeSchedule.modeSequence.begin(), + modeSchedule.modeSequence.end(), [](size_t mode) { return mode == ModeNumber::STANCE; }); } bool isStanding(const GaitSchedule& gaitSchedule) { const auto& modeSequence = gaitSchedule.getCurrentGait().modeSequence; - return std::all_of(modeSequence.begin(), modeSequence.end(), [](size_t mode) { return mode == ModeNumber::STANCE; }); + return std::all_of(modeSequence.begin(), modeSequence.end(), + [](size_t mode) { return mode == ModeNumber::STANCE; }); } } // namespace switched_model \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/ModeSequenceTemplate.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/ModeSequenceTemplate.cpp index e62942b11..e1f065faa 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/ModeSequenceTemplate.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/ModeSequenceTemplate.cpp @@ -34,21 +34,30 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace switched_model { -std::ostream& operator<<(std::ostream& stream, const ModeSequenceTemplate& modeSequenceTemplate) { - stream << "Template switching times: {" << ocs2::toDelimitedString(modeSequenceTemplate.switchingTimes) << "}\n"; - stream << "Template mode sequence: {" << ocs2::toDelimitedString(modeSequenceTemplate.modeSequence) << "}\n"; +std::ostream& operator<<(std::ostream& stream, + const ModeSequenceTemplate& ModeSequenceTemplate) { + stream << "Template switching times: {" + << ocs2::toDelimitedString(ModeSequenceTemplate.switchingTimes) + << "}\n"; + stream << "Template mode sequence: {" + << ocs2::toDelimitedString(ModeSequenceTemplate.modeSequence) << "}\n"; return stream; } -ModeSequenceTemplate loadModeSequenceTemplate(const std::string& filename, const std::string& topicName, bool verbose) { +ModeSequenceTemplate loadModeSequenceTemplate(const std::string& filename, + const std::string& topicName, + bool verbose) { std::vector switchingTimes; - ocs2::loadData::loadStdVector(filename, topicName + ".switchingTimes", switchingTimes, verbose); + ocs2::loadData::loadStdVector(filename, topicName + ".switchingTimes", + switchingTimes, verbose); std::vector modeSequenceString; - ocs2::loadData::loadStdVector(filename, topicName + ".modeSequence", modeSequenceString, verbose); + ocs2::loadData::loadStdVector(filename, topicName + ".modeSequence", + modeSequenceString, verbose); if (switchingTimes.empty() || modeSequenceString.empty()) { - throw std::runtime_error("[loadModeSequenceTemplate] failed to load : " + topicName + " from " + filename); + throw std::runtime_error("[loadModeSequenceTemplate] failed to load : " + + topicName + " from " + filename); } // convert the mode name to mode enum @@ -61,43 +70,59 @@ ModeSequenceTemplate loadModeSequenceTemplate(const std::string& filename, const return {switchingTimes, modeSequence}; } -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()); +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; } -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()); +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}; } -Gait toGait(const ModeSequenceTemplate& modeSequenceTemplate) { - const auto startTime = modeSequenceTemplate.switchingTimes.front(); - const auto endTime = modeSequenceTemplate.switchingTimes.back(); +Gait toGait(const ModeSequenceTemplate& ModeSequenceTemplate) { + const auto startTime = ModeSequenceTemplate.switchingTimes.front(); + const auto endTime = ModeSequenceTemplate.switchingTimes.back(); Gait gait; gait.duration = endTime - startTime; // Events: from time -> phase - gait.eventPhases.reserve(modeSequenceTemplate.switchingTimes.size()); - std::for_each(modeSequenceTemplate.switchingTimes.begin() + 1, modeSequenceTemplate.switchingTimes.end() - 1, - [&](scalar_t eventTime) { gait.eventPhases.push_back((eventTime - startTime) / gait.duration); }); + gait.eventPhases.reserve(ModeSequenceTemplate.switchingTimes.size()); + std::for_each( + ModeSequenceTemplate.switchingTimes.begin() + 1, + ModeSequenceTemplate.switchingTimes.end() - 1, [&](scalar_t eventTime) { + gait.eventPhases.push_back((eventTime - startTime) / gait.duration); + }); // Modes: - gait.modeSequence = modeSequenceTemplate.modeSequence; + gait.modeSequence = ModeSequenceTemplate.modeSequence; assert(isValidGait(gait)); return gait; } -ocs2::ModeSchedule loadModeSchedule(const std::string& filename, const std::string& topicName, bool verbose) { +ocs2::ModeSchedule loadModeSchedule(const std::string& filename, + const std::string& topicName, + bool verbose) { std::vector eventTimes; - ocs2::loadData::loadStdVector(filename, topicName + ".eventTimes", eventTimes, verbose); + ocs2::loadData::loadStdVector(filename, topicName + ".eventTimes", eventTimes, + verbose); std::vector modeSequenceString; - ocs2::loadData::loadStdVector(filename, topicName + ".modeSequence", modeSequenceString, verbose); + ocs2::loadData::loadStdVector(filename, topicName + ".modeSequence", + modeSequenceString, verbose); if (modeSequenceString.empty()) { - throw std::runtime_error("[loadModeSchedule] failed to load : " + topicName + " from " + filename); + throw std::runtime_error("[loadModeSchedule] failed to load : " + + topicName + " from " + filename); } // convert the mode name to mode enum diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/SingleLegLogic.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/SingleLegLogic.cpp index 66a87008a..cefd5f88c 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/SingleLegLogic.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/logic/SingleLegLogic.cpp @@ -8,21 +8,25 @@ namespace switched_model { -feet_array_t> extractContactTimingsPerLeg(const ocs2::ModeSchedule& modeSchedule) { +feet_array_t> extractContactTimingsPerLeg( + const ocs2::ModeSchedule& modeSchedule) { feet_array_t> contactTimingsPerLeg; // Convert mode sequence to a contact flag vector per leg - const auto contactSequencePerLeg = extractContactFlags(modeSchedule.modeSequence); + const auto contactSequencePerLeg = + extractContactFlags(modeSchedule.modeSequence); // Extract timings per leg for (size_t leg = 0; leg < NUM_CONTACT_POINTS; ++leg) { - contactTimingsPerLeg[leg] = extractContactTimings(modeSchedule.eventTimes, contactSequencePerLeg[leg]); + contactTimingsPerLeg[leg] = extractContactTimings( + modeSchedule.eventTimes, contactSequencePerLeg[leg]); } return contactTimingsPerLeg; } -scalar_t getTimeOfNextLiftOff(scalar_t currentTime, const std::vector& contactTimings) { +scalar_t getTimeOfNextLiftOff( + scalar_t currentTime, const std::vector& contactTimings) { for (const auto& contactPhase : contactTimings) { if (hasEndTime(contactPhase) && contactPhase.end > currentTime) { return contactPhase.end; @@ -31,7 +35,8 @@ scalar_t getTimeOfNextLiftOff(scalar_t currentTime, const std::vector& contactTimings) { +scalar_t getTimeOfNextTouchDown( + scalar_t currentTime, const std::vector& contactTimings) { for (const auto& contactPhase : contactTimings) { if (hasStartTime(contactPhase) && contactPhase.start > currentTime) { return contactPhase.start; @@ -40,7 +45,9 @@ scalar_t getTimeOfNextTouchDown(scalar_t currentTime, const std::vector extractContactTimings(const std::vector& eventTimes, const std::vector& contactFlags) { +std::vector extractContactTimings( + const std::vector& eventTimes, + const std::vector& contactFlags) { assert(eventTimes.size() + 1 == contactFlags.size()); const int numPhases = contactFlags.size(); @@ -58,7 +65,9 @@ std::vector extractContactTimings(const std::vector& ev } // Register start of the contact phase - const scalar_t startTime = (currentPhase == 0) ? std::numeric_limits::quiet_NaN() : eventTimes[currentPhase - 1]; + const scalar_t startTime = (currentPhase == 0) + ? std::numeric_limits::quiet_NaN() + : eventTimes[currentPhase - 1]; // Find when the contact phase ends while (currentPhase + 1 < numPhases && contactFlags[currentPhase + 1]) { @@ -66,7 +75,9 @@ std::vector extractContactTimings(const std::vector& ev } // Register end of the contact phase - const scalar_t endTime = (currentPhase + 1 >= numPhases) ? std::numeric_limits::quiet_NaN() : eventTimes[currentPhase]; + const scalar_t endTime = (currentPhase + 1 >= numPhases) + ? std::numeric_limits::quiet_NaN() + : eventTimes[currentPhase]; // Add to phases contactTimings.push_back({startTime, endTime}); @@ -75,11 +86,13 @@ std::vector extractContactTimings(const std::vector& ev return contactTimings; } -feet_array_t> extractContactFlags(const std::vector& modeSequence) { +feet_array_t> extractContactFlags( + const std::vector& modeSequence) { const size_t numPhases = modeSequence.size(); feet_array_t> contactFlagStock; - std::fill(contactFlagStock.begin(), contactFlagStock.end(), std::vector(numPhases)); + std::fill(contactFlagStock.begin(), contactFlagStock.end(), + std::vector(numPhases)); for (size_t i = 0; i < numPhases; i++) { const auto contactFlag = modeNumber2StanceLeg(modeSequence[i]); diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/ros_msg_conversions/RosMsgConversions.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/ros_msg_conversions/RosMsgConversions.cpp index 7f042be2b..5d5c51790 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/ros_msg_conversions/RosMsgConversions.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/src/ros_msg_conversions/RosMsgConversions.cpp @@ -10,37 +10,38 @@ namespace switched_model { namespace ros_msg_conversions { -ocs2_switched_model_msgs::gait toMessage(const Gait& gait) { - ocs2_switched_model_msgs::gait msg; +ocs2_switched_model_msgs::msg::Gait toMessage(const Gait& gait) { + ocs2_switched_model_msgs::msg::Gait msg; msg.duration = gait.duration; - msg.eventPhases.reserve(gait.eventPhases.size()); + msg.event_phases.reserve(gait.eventPhases.size()); for (const auto& phase : gait.eventPhases) { - msg.eventPhases.push_back(phase); + msg.event_phases.push_back(phase); } - msg.modeSequence.reserve(gait.modeSequence.size()); + msg.mode_sequence.reserve(gait.modeSequence.size()); for (const auto& mode : gait.modeSequence) { - msg.modeSequence.push_back(mode); + msg.mode_sequence.push_back(mode); } return msg; } -Gait fromMessage(const ocs2_switched_model_msgs::gait& msg) { +Gait fromMessage(const ocs2_switched_model_msgs::msg::Gait& msg) { Gait gait; gait.duration = msg.duration; - gait.eventPhases.reserve(msg.eventPhases.size()); - for (const auto& phase : msg.eventPhases) { + gait.eventPhases.reserve(msg.event_phases.size()); + for (const auto& phase : msg.event_phases) { gait.eventPhases.push_back(phase); } - gait.modeSequence.reserve(msg.modeSequence.size()); - for (const auto& mode : msg.modeSequence) { + gait.modeSequence.reserve(msg.mode_sequence.size()); + for (const auto& mode : msg.mode_sequence) { gait.modeSequence.push_back(mode); } assert(isValidGait(gait)); return gait; } -ocs2_switched_model_msgs::gait_sequence toMessage(const GaitSchedule::GaitSequence& gaitSequence) { - ocs2_switched_model_msgs::gait_sequence msg; +ocs2_switched_model_msgs::msg::GaitSequence toMessage( + const GaitSchedule::GaitSequence& gaitSequence) { + ocs2_switched_model_msgs::msg::GaitSequence msg; msg.gaits.reserve(gaitSequence.size()); for (const auto& gait : gaitSequence) { msg.gaits.emplace_back(toMessage(gait)); @@ -48,7 +49,8 @@ ocs2_switched_model_msgs::gait_sequence toMessage(const GaitSchedule::GaitSequen return msg; } -GaitSchedule::GaitSequence fromMessage(const ocs2_switched_model_msgs::gait_sequence& msg) { +GaitSchedule::GaitSequence fromMessage( + const ocs2_switched_model_msgs::msg::GaitSequence& msg) { GaitSchedule::GaitSequence gaitSequence; gaitSequence.reserve(msg.gaits.size()); for (const auto& gait : msg.gaits) { @@ -57,15 +59,17 @@ GaitSchedule::GaitSequence fromMessage(const ocs2_switched_model_msgs::gait_sequ return gaitSequence; } -ocs2_switched_model_msgs::scheduled_gait_sequence toMessage(scalar_t startTime, const GaitSchedule::GaitSequence& gaitSequence) { - ocs2_switched_model_msgs::scheduled_gait_sequence msg; - msg.startTime = startTime; - msg.gaitSequence = toMessage(gaitSequence); +ocs2_switched_model_msgs::msg::ScheduledGaitSequence toMessage( + scalar_t startTime, const GaitSchedule::GaitSequence& gaitSequence) { + ocs2_switched_model_msgs::msg::ScheduledGaitSequence msg; + msg.start_time = startTime; + msg.gait_sequence = toMessage(gaitSequence); return msg; } -std::pair fromMessage(const ocs2_switched_model_msgs::scheduled_gait_sequence& msg) { - return {msg.startTime, fromMessage(msg.gaitSequence)}; +std::pair fromMessage( + const ocs2_switched_model_msgs::msg::ScheduledGaitSequence& msg) { + return {msg.start_time, fromMessage(msg.gait_sequence)}; } } // namespace ros_msg_conversions diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testEarlyTouchDown.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testEarlyTouchDown.cpp index 6070ac855..67858ce19 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testEarlyTouchDown.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testEarlyTouchDown.cpp @@ -20,37 +20,52 @@ TEST(testEarlyTouchDown, testTrot) { Gait gait; gait.duration = 0.8; gait.eventPhases = {0.45, 0.5, 0.95}; - gait.modeSequence = {ModeNumber::LF_RH, ModeNumber::STANCE, ModeNumber::RF_LH, ModeNumber::STANCE}; + gait.modeSequence = {ModeNumber::LF_RH, ModeNumber::STANCE, ModeNumber::RF_LH, + ModeNumber::STANCE}; GaitSchedule gaitSchedule(0.0, gait); - gaitSchedule.setGaitAfterTime(gait, gaitSchedule.getCurrentTime() + 0.99 * gait.duration); + gaitSchedule.setGaitAfterTime( + gait, gaitSchedule.getCurrentTime() + 0.99 * gait.duration); // Early touch down gait adaptation RF and LH should be in touch down. feet_array_t earlyTouchDownPerLeg{true, true, true, true}; - auto earlyTouchDownGaitAdaptor = earlyTouchDownAdaptation(earlyTouchDownPerLeg); + auto earlyTouchDownGaitAdaptor = + earlyTouchDownAdaptation(earlyTouchDownPerLeg); gaitSchedule.adaptCurrentGait(earlyTouchDownGaitAdaptor); // Check. - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == ModeNumber::STANCE); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == ModeNumber::STANCE); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[2] == ModeNumber::RF_LH); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[3] == ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == + ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == + ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[2] == + ModeNumber::RF_LH); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[3] == + ModeNumber::STANCE); // Early touch down gait adaptation LF and RH should be in touch down. gaitSchedule.advanceToTime(gait.duration * gait.eventPhases[1]); gaitSchedule.adaptCurrentGait(earlyTouchDownGaitAdaptor); // Check. - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == ModeNumber::STANCE); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == ModeNumber::STANCE); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[2] == ModeNumber::STANCE); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[3] == ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == + ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == + ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[2] == + ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[3] == + ModeNumber::STANCE); // Check that the next gait was not adapted. gaitSchedule.advanceToTime(gait.duration); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == ModeNumber::LF_RH); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == ModeNumber::STANCE); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[2] == ModeNumber::RF_LH); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[3] == ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == + ModeNumber::LF_RH); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == + ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[2] == + ModeNumber::RF_LH); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[3] == + ModeNumber::STANCE); } TEST(testEarlyTouchDown, testOverhangingSwingPhases) { @@ -64,15 +79,18 @@ TEST(testEarlyTouchDown, testOverhangingSwingPhases) { nextGait.duration = 0.5; nextGait.eventPhases = {0.5}; nextGait.modeSequence = {ModeNumber::LH_RH, ModeNumber::LF_LH_RH}; - gaitSchedule.setGaitAfterTime(nextGait, gaitSchedule.getCurrentTime() + 0.99 * gait.duration); + gaitSchedule.setGaitAfterTime( + nextGait, gaitSchedule.getCurrentTime() + 0.99 * gait.duration); // Early touch down gait adaptation LF should be in touch down. feet_array_t earlyTouchDownPerLeg{true, true, false, false}; - auto earlyTouchDownGaitAdaptor = earlyTouchDownAdaptation(earlyTouchDownPerLeg); + auto earlyTouchDownGaitAdaptor = + earlyTouchDownAdaptation(earlyTouchDownPerLeg); gaitSchedule.adaptCurrentGait(earlyTouchDownGaitAdaptor); // Check. - const auto modeSchedule = gaitSchedule.getModeSchedule(gait.duration + nextGait.duration); + const auto modeSchedule = + gaitSchedule.getModeSchedule(gait.duration + nextGait.duration); ASSERT_TRUE(modeSchedule.modeSequence[0] == ModeNumber::STANCE); ASSERT_TRUE(modeSchedule.modeSequence[1] == ModeNumber::LF_LH_RH); ASSERT_EQ(modeSchedule.modeSequence.size(), 2); @@ -82,12 +100,18 @@ TEST(testEarlyTouchDown, testOverhangingSwingPhases) { gaitSchedule.adaptCurrentGait(earlyTouchDownGaitAdaptor); // Check. - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == ModeNumber::STANCE); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == + ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == + ModeNumber::STANCE); gaitSchedule.advanceToTime(gait.duration); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == ModeNumber::STANCE); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == + ModeNumber::STANCE); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == + ModeNumber::STANCE); gaitSchedule.advanceToTime(gait.duration + nextGait.duration); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == ModeNumber::LH_RH); - ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == ModeNumber::LF_LH_RH); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[0] == + ModeNumber::LH_RH); + ASSERT_TRUE(gaitSchedule.getCurrentGait().modeSequence[1] == + ModeNumber::LF_LH_RH); } \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testGait.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testGait.cpp index 899960557..4acd158fd 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testGait.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testGait.cpp @@ -56,9 +56,11 @@ TEST(TestPhaseWrap, wrapPhase) { // Wrap around edges ASSERT_DOUBLE_EQ(wrapPhase(0.0), 0.0); ASSERT_DOUBLE_EQ(wrapPhase(1.0), 0.0); - ASSERT_DOUBLE_EQ(wrapPhase(1.0 - std::numeric_limits::epsilon()), 1.0 - std::numeric_limits::epsilon()); + ASSERT_DOUBLE_EQ(wrapPhase(1.0 - std::numeric_limits::epsilon()), + 1.0 - std::numeric_limits::epsilon()); ASSERT_DOUBLE_EQ(wrapPhase(1.0), 0.0); - ASSERT_DOUBLE_EQ(wrapPhase(1.0 + std::numeric_limits::epsilon()), std::numeric_limits::epsilon()); + ASSERT_DOUBLE_EQ(wrapPhase(1.0 + std::numeric_limits::epsilon()), + std::numeric_limits::epsilon()); ASSERT_DOUBLE_EQ(wrapPhase(1.8), 0.8); ASSERT_DOUBLE_EQ(wrapPhase(-0.2), 0.8); } @@ -66,7 +68,9 @@ TEST(TestPhaseWrap, wrapPhase) { TEST(TestSingleModeGait, getModeFromPhase) { ASSERT_EQ(getModeFromPhase(0.0, singleModeGait), 1); ASSERT_EQ(getModeFromPhase(0.33, singleModeGait), 1); - ASSERT_EQ(getModeFromPhase(1.0 - std::numeric_limits::epsilon(), singleModeGait), 1); + ASSERT_EQ(getModeFromPhase(1.0 - std::numeric_limits::epsilon(), + singleModeGait), + 1); } TEST(TestMultiModeGait, getModeFromPhase) { @@ -75,22 +79,37 @@ TEST(TestMultiModeGait, getModeFromPhase) { ASSERT_EQ(getModeFromPhase(0.33, multiModeGait), 1); ASSERT_EQ(getModeFromPhase(0.42, multiModeGait), 1); ASSERT_EQ(getModeFromPhase(0.66, multiModeGait), 2); - ASSERT_EQ(getModeFromPhase(1.0 - std::numeric_limits::epsilon(), multiModeGait), 2); + ASSERT_EQ(getModeFromPhase(1.0 - std::numeric_limits::epsilon(), + multiModeGait), + 2); } TEST(TestMultiModeGait, timeLeft) { - ASSERT_DOUBLE_EQ(timeLeftInMode(0.0, multiModeGait), 0.33 * multiModeGait.duration); - ASSERT_DOUBLE_EQ(timeLeftInMode(0.1, multiModeGait), (0.33 - 0.1) * multiModeGait.duration); - ASSERT_DOUBLE_EQ(timeLeftInMode(0.33 - std::numeric_limits::epsilon(), multiModeGait), - std::numeric_limits::epsilon() * multiModeGait.duration); - ASSERT_DOUBLE_EQ(timeLeftInMode(0.33, multiModeGait), (0.66 - 0.33) * multiModeGait.duration); - ASSERT_DOUBLE_EQ(timeLeftInMode(0.42, multiModeGait), (0.66 - 0.42) * multiModeGait.duration); - ASSERT_DOUBLE_EQ(timeLeftInMode(0.66, multiModeGait), (1.0 - 0.66) * multiModeGait.duration); - ASSERT_DOUBLE_EQ(timeLeftInMode(1.0 - std::numeric_limits::epsilon(), multiModeGait), - std::numeric_limits::epsilon() * multiModeGait.duration); + ASSERT_DOUBLE_EQ(timeLeftInMode(0.0, multiModeGait), + 0.33 * multiModeGait.duration); + ASSERT_DOUBLE_EQ(timeLeftInMode(0.1, multiModeGait), + (0.33 - 0.1) * multiModeGait.duration); + ASSERT_DOUBLE_EQ( + timeLeftInMode(0.33 - std::numeric_limits::epsilon(), + multiModeGait), + std::numeric_limits::epsilon() * multiModeGait.duration); + ASSERT_DOUBLE_EQ(timeLeftInMode(0.33, multiModeGait), + (0.66 - 0.33) * multiModeGait.duration); + ASSERT_DOUBLE_EQ(timeLeftInMode(0.42, multiModeGait), + (0.66 - 0.42) * multiModeGait.duration); + ASSERT_DOUBLE_EQ(timeLeftInMode(0.66, multiModeGait), + (1.0 - 0.66) * multiModeGait.duration); + ASSERT_DOUBLE_EQ( + timeLeftInMode(1.0 - std::numeric_limits::epsilon(), + multiModeGait), + std::numeric_limits::epsilon() * multiModeGait.duration); - ASSERT_DOUBLE_EQ(timeLeftInGait(0.0, multiModeGait), 1.0 * multiModeGait.duration); - ASSERT_DOUBLE_EQ(timeLeftInGait(0.5, multiModeGait), 0.5 * multiModeGait.duration); - ASSERT_DOUBLE_EQ(timeLeftInGait(1.0 - std::numeric_limits::epsilon(), multiModeGait), - std::numeric_limits::epsilon() * multiModeGait.duration); + ASSERT_DOUBLE_EQ(timeLeftInGait(0.0, multiModeGait), + 1.0 * multiModeGait.duration); + ASSERT_DOUBLE_EQ(timeLeftInGait(0.5, multiModeGait), + 0.5 * multiModeGait.duration); + ASSERT_DOUBLE_EQ( + timeLeftInGait(1.0 - std::numeric_limits::epsilon(), + multiModeGait), + std::numeric_limits::epsilon() * multiModeGait.duration); } \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testGaitSchedule.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testGaitSchedule.cpp index cfe64756d..39883985f 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testGaitSchedule.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_interface/test/logic/testGaitSchedule.cpp @@ -39,14 +39,18 @@ TEST(TestGaitSchedule, advanceTwoGaits) { TEST(TestGaitSchedule, setNextGaitSchedule) { const double t0 = 11.0; const double timeHorizon = 5.0; - const std::deque gaitSequence = {Gait{1.0, {}, {42}}, Gait{1.0, {}, {21}}, Gait{0.5, {0.66}, {0, 1}}}; + const std::deque gaitSequence = { + Gait{1.0, {}, {42}}, Gait{1.0, {}, {21}}, Gait{0.5, {0.66}, {0, 1}}}; // Add the gait schedule in two parts GaitSchedule gaitSchedule(t0, gaitSequence[0]); - gaitSchedule.setGaitSequenceAfterCurrentGait({gaitSequence[1], gaitSequence[2]}); + gaitSchedule.setGaitSequenceAfterCurrentGait( + {gaitSequence[1], gaitSequence[2]}); // Check that the resulting mode schedule is equal - auto checkModeSchedule = getModeSchedule(gaitSchedule.getCurrentPhase(), t0, timeHorizon, gaitSequence.begin(), gaitSequence.end()); + auto checkModeSchedule = + getModeSchedule(gaitSchedule.getCurrentPhase(), t0, timeHorizon, + gaitSequence.begin(), gaitSequence.end()); auto modeSchedule = gaitSchedule.getModeSchedule(timeHorizon); ASSERT_EQ(modeSchedule.eventTimes, checkModeSchedule.eventTimes); ASSERT_EQ(modeSchedule.modeSequence, checkModeSchedule.modeSequence); @@ -165,7 +169,8 @@ TEST(TestGaitSchedule, setGaitScheduleAfterTimeWithNonzeroPhase) { auto modeSchedule = gaitSchedule.getModeSchedule(2.0); ASSERT_DOUBLE_EQ(modeSchedule.eventTimes.size(), 1); - ASSERT_DOUBLE_EQ(modeSchedule.eventTimes.front(), 3.0); // First insert opportunity + ASSERT_DOUBLE_EQ(modeSchedule.eventTimes.front(), + 3.0); // First insert opportunity ASSERT_EQ(modeSchedule.modeSequence.front(), 0); ASSERT_EQ(modeSchedule.modeSequence.back(), 1); } \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/CMakeLists.txt index ae4bd5689..bde07f598 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/CMakeLists.txt @@ -1,35 +1,19 @@ -cmake_minimum_required(VERSION 3.0.0) +cmake_minimum_required(VERSION 3.5) project(ocs2_switched_model_msgs) -set(CATKIN_PACKAGE_DEPENDENCIES - std_msgs - ocs2_msgs -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} - message_generation -) +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(ocs2_msgs REQUIRED) -# Declare the message files to be built -add_message_files(DIRECTORY msg FILES - gait.msg - gait_sequence.msg - scheduled_gait_sequence.msg +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Gait.msg" + "msg/GaitSequence.msg" + "msg/ScheduledGaitSequence.msg" + "srv/TrajectoryRequest.srv" + DEPENDENCIES + ocs2_msgs ) -# Declare the service files to be built -add_service_files(DIRECTORY srv FILES - trajectory_request.srv -) +ament_export_dependencies(rosidl_default_runtime) -# Actually generate the language-specific message and service files -generate_messages(DEPENDENCIES - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -# Declare that this catkin package's runtime dependencies -catkin_package(CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - message_runtime -) +ament_package() diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/Gait.msg b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/Gait.msg new file mode 100644 index 000000000..5ce021b7c --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/Gait.msg @@ -0,0 +1,5 @@ +# Gait message + +float32[] event_phases # event phases: length equal to modesequence's - 1. Values within [0,1] +int8[] mode_sequence # mode sequence: e.g., for a quadrupedal robot, it is in the set {0, 1,..., 15} +float32 duration diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/gait_sequence.msg b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/GaitSequence.msg similarity index 61% rename from ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/gait_sequence.msg rename to ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/GaitSequence.msg index a52dbf80e..5dda90420 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/gait_sequence.msg +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/GaitSequence.msg @@ -1,3 +1,3 @@ # Sequence of gaits -gait[] gaits +Gait[] gaits diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/ScheduledGaitSequence.msg b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/ScheduledGaitSequence.msg new file mode 100644 index 000000000..d29139fc3 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/ScheduledGaitSequence.msg @@ -0,0 +1,4 @@ +# Scheduled list of gaits + +float32 start_time +GaitSequence gait_sequence diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/gait.msg b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/gait.msg deleted file mode 100644 index 38db64176..000000000 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/gait.msg +++ /dev/null @@ -1,5 +0,0 @@ -# Gait message - -float32[] eventPhases # event phases: length equal to modesequence's - 1. Values within [0,1] -int8[] modeSequence # mode sequence: e.g., for a quadrupedal robot, it is in the set {0, 1,..., 15} -float32 duration diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/scheduled_gait_sequence.msg b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/scheduled_gait_sequence.msg deleted file mode 100644 index 7bf2458ba..000000000 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/msg/scheduled_gait_sequence.msg +++ /dev/null @@ -1,4 +0,0 @@ -# Scheduled list of gaits - -float32 startTime -gait_sequence gaitSequence diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/package.xml b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/package.xml index 85bdd0121..4e7373b73 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/package.xml +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/package.xml @@ -1,4 +1,5 @@ - + + ocs2_switched_model_msgs 0.0.1 Provides ocs2_switched_model_msgs @@ -10,10 +11,16 @@ TODO - catkin - message_generation - std_msgs + ament_cmake + rosidl_default_generators + ocs2_msgs - message_runtime + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/srv/TrajectoryRequest.srv b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/srv/TrajectoryRequest.srv new file mode 100644 index 000000000..69d1b7031 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/srv/TrajectoryRequest.srv @@ -0,0 +1,6 @@ +# Trajectory request service + +ocs2_msgs/MpcTargetTrajectories trajectory # The trajectory to track +GaitSequence gait_sequence # The gait +--- +bool success # 1 if successful 0 if trajectory couldn't be started. diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/srv/trajectory_request.srv b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/srv/trajectory_request.srv deleted file mode 100644 index cb311f595..000000000 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_switched_model_msgs/srv/trajectory_request.srv +++ /dev/null @@ -1,6 +0,0 @@ -# Trajectory request service - -ocs2_msgs/mpc_target_trajectories trajectory # The trajectory to track -gait_sequence gaitSequence # The gait ---- -bool success # 1 if successful 0 if trajectory couldn't be started. diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/CMakeLists.txt b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/CMakeLists.txt index 6a5454e26..1dcf18859 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/CMakeLists.txt @@ -1,8 +1,8 @@ cmake_minimum_required(VERSION 3.10) project(segmented_planes_terrain_model) -# Catkin dependencies -set(CATKIN_PACKAGE_DEPENDENCIES +# ament_cmake dependencies +set(dependencies convex_plane_decomposition convex_plane_decomposition_msgs convex_plane_decomposition_ros @@ -11,36 +11,43 @@ set(CATKIN_PACKAGE_DEPENDENCIES grid_map_sdf ocs2_ros_interfaces ocs2_switched_model_interface - roscpp + rclcpp sensor_msgs visualization_msgs ) -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} +find_package(ament_cmake REQUIRED) +find_package(convex_plane_decomposition REQUIRED) +find_package(convex_plane_decomposition_msgs REQUIRED) +find_package(convex_plane_decomposition_ros REQUIRED) +find_package(grid_map_filters_rsl REQUIRED) +find_package(grid_map_ros REQUIRED) +find_package(grid_map_sdf REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_switched_model_interface REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(PCL REQUIRED) + +find_package(Boost REQUIRED COMPONENTS + system + filesystem + log_setup + log ) # Cpp standard version -set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS -) - ########### ## Build ## ########### include_directories( include - ${catkin_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} ) add_library(${PROJECT_NAME} @@ -48,11 +55,8 @@ add_library(${PROJECT_NAME} src/SegmentedPlanesTerrainModelRos.cpp src/SegmentedPlanesTerrainVisualization.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} - ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) target_compile_options(${PROJECT_NAME} PUBLIC -DCGAL_HAS_THREADS @@ -62,15 +66,17 @@ target_compile_options(${PROJECT_NAME} ## Install ## ############# -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - ) +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include/${PROJECT_NAME} +) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) -############# -## Testing ## -############# +ament_export_dependencies(${dependencies}) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesSignedDistanceField.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesSignedDistanceField.h index 254d786fc..ef1170eb8 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesSignedDistanceField.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesSignedDistanceField.h @@ -11,37 +11,52 @@ namespace switched_model { /** - * Simple wrapper class to implement the switched_model::SignedDistanceField interface. - * See the forwarded function for documentation. + * Simple wrapper class to implement the switched_model::SignedDistanceField + * interface. See the forwarded function for documentation. */ class SegmentedPlanesSignedDistanceField : public SignedDistanceField { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - SegmentedPlanesSignedDistanceField(const grid_map::GridMap& gridMap, const std::string& elevationLayer, double minHeight, - double maxHeight) - : sdf_(gridMap, elevationLayer, minHeight, maxHeight) {} + SegmentedPlanesSignedDistanceField(const grid_map::GridMap& gridMap, + const std::string& elevationLayer, + double minHeight, double maxHeight) + : gridMap_(gridMap) { + sdf_.calculateSignedDistanceField(gridMap_, elevationLayer, maxHeight); + } ~SegmentedPlanesSignedDistanceField() override = default; - SegmentedPlanesSignedDistanceField* clone() const override { return new SegmentedPlanesSignedDistanceField(*this); }; - - switched_model::scalar_t value(const switched_model::vector3_t& position) const override { return sdf_.value(position); } + SegmentedPlanesSignedDistanceField* clone() const override { + return new SegmentedPlanesSignedDistanceField(*this); + }; - switched_model::vector3_t derivative(const switched_model::vector3_t& position) const override { return sdf_.derivative(position); } + switched_model::scalar_t value( + const switched_model::vector3_t& position) const override { + return sdf_.getDistanceAt(position); + } - std::pair valueAndDerivative( + switched_model::vector3_t derivative( const switched_model::vector3_t& position) const override { - return sdf_.valueAndDerivative(position); + return sdf_.getDistanceGradientAt(position); + } + + std::pair + valueAndDerivative(const switched_model::vector3_t& position) const override { + return {value(position), derivative(position)}; } grid_map::SignedDistanceField& asGridmapSdf() { return sdf_; } const grid_map::SignedDistanceField& asGridmapSdf() const { return sdf_; } + const grid_map::GridMap& gridMap() const {return gridMap_; } protected: - SegmentedPlanesSignedDistanceField(const SegmentedPlanesSignedDistanceField& other) : sdf_(other.sdf_){}; + SegmentedPlanesSignedDistanceField( + const SegmentedPlanesSignedDistanceField& other) + : sdf_(other.sdf_){}; private: grid_map::SignedDistanceField sdf_; + grid_map::GridMap gridMap_; }; } // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesTerrainModelRos.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesTerrainModelRos.h index e3c75cea5..0b7fa15d0 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesTerrainModelRos.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesTerrainModelRos.h @@ -4,40 +4,48 @@ #pragma once -#include - -#include - -#include - #include -#include +#include +#include +#include #include "SegmentedPlanesTerrainModel.h" +#include "rclcpp/rclcpp.hpp" namespace switched_model { class SegmentedPlanesTerrainModelRos { public: - SegmentedPlanesTerrainModelRos(ros::NodeHandle& nodehandle); + SegmentedPlanesTerrainModelRos(const rclcpp::Node::SharedPtr& node); ~SegmentedPlanesTerrainModelRos(); /// Extract the latest terrain model. Resets internal model to a nullptr std::unique_ptr getTerrainModel(); - void createSignedDistanceBetween(const Eigen::Vector3d& minCoordinates, const Eigen::Vector3d& maxCoordinates); + void createSignedDistanceBetween(const Eigen::Vector3d& minCoordinates, + const Eigen::Vector3d& maxCoordinates); void publish(); + static void toPointCloud( + const SegmentedPlanesSignedDistanceField& segmentedPlanesSignedDistanceField, + sensor_msgs::msg::PointCloud2& pointCloud, size_t decimation, + const std::function& condition); private: - void callback(const convex_plane_decomposition_msgs::PlanarTerrain::ConstPtr& msg); + void callback( + const convex_plane_decomposition_msgs::msg::PlanarTerrain::ConstSharedPtr& + msg); - std::pair getSignedDistanceRange(const grid_map::GridMap& gridMap, const std::string& elevationLayer); + std::pair getSignedDistanceRange( + const grid_map::GridMap& gridMap, const std::string& elevationLayer); - ros::Subscriber terrainSubscriber_; - ros::Publisher distanceFieldPublisher_; + rclcpp::Node::SharedPtr node_; + rclcpp::Subscription:: + SharedPtr terrainSubscriber_; + rclcpp::Publisher::SharedPtr + distanceFieldPublisher_; std::mutex updateMutex_; std::atomic_bool terrainUpdated_; @@ -49,7 +57,7 @@ class SegmentedPlanesTerrainModelRos { bool externalCoordinatesGiven_; std::mutex pointCloudMutex_; - std::unique_ptr pointCloud2MsgPtr_; + std::unique_ptr pointCloud2MsgPtr_; ocs2::benchmark::RepeatedTimer callbackTimer_; }; diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesTerrainVisualization.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesTerrainVisualization.h index 6c8acf6b4..afa4ccbff 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesTerrainVisualization.h +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/include/segmented_planes_terrain_model/SegmentedPlanesTerrainVisualization.h @@ -4,15 +4,15 @@ #pragma once -#include -#include +#include +#include #include #include namespace switched_model { -visualization_msgs::MarkerArray getConvexTerrainMarkers(const ConvexTerrain& convexTerrain, ocs2::Color color, double linewidth, +visualization_msgs::msg::MarkerArray getConvexTerrainMarkers(const ConvexTerrain& convexTerrain, ocs2::Color color, double linewidth, double normalLength); } // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/package.xml b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/package.xml index c9b7fb7c4..dd60f3f85 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/package.xml +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/package.xml @@ -8,7 +8,7 @@ TODO - catkin + ament_cmake convex_plane_decomposition convex_plane_decomposition_msgs convex_plane_decomposition_ros @@ -17,8 +17,11 @@ grid_map_sdf ocs2_ros_interfaces ocs2_switched_model_interface - roscpp + rclcpp sensor_msgs visualization_msgs + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/src/SegmentedPlanesTerrainModelRos.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/src/SegmentedPlanesTerrainModelRos.cpp index fa83f1da1..d51198d73 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/src/SegmentedPlanesTerrainModelRos.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/src/SegmentedPlanesTerrainModelRos.cpp @@ -4,41 +4,52 @@ #include "segmented_planes_terrain_model/SegmentedPlanesTerrainModelRos.h" -#include - #include #include +#include namespace switched_model { -SegmentedPlanesTerrainModelRos::SegmentedPlanesTerrainModelRos(ros::NodeHandle& nodehandle) - : terrainUpdated_(false), +SegmentedPlanesTerrainModelRos::SegmentedPlanesTerrainModelRos( + const rclcpp::Node::SharedPtr& node) + : node_(node), + terrainUpdated_(false), minCoordinates_(Eigen::Vector3d::Zero()), maxCoordinates_(Eigen::Vector3d::Zero()), externalCoordinatesGiven_(false) { - terrainSubscriber_ = - nodehandle.subscribe("/convex_plane_decomposition_ros/planar_terrain", 1, &SegmentedPlanesTerrainModelRos::callback, this); + terrainSubscriber_ = node->create_subscription< + convex_plane_decomposition_msgs::msg::PlanarTerrain>( + "/convex_plane_decomposition_ros/planar_terrain", 1, + std::bind(&SegmentedPlanesTerrainModelRos::callback, this, + std::placeholders::_1)); distanceFieldPublisher_ = - nodehandle.advertise("/convex_plane_decomposition_ros/signed_distance_field", 1, true); + node->create_publisher( + "/convex_plane_decomposition_ros/signed_distance_field", 1); } SegmentedPlanesTerrainModelRos::~SegmentedPlanesTerrainModelRos() { if (callbackTimer_.getNumTimedIntervals() > 0) { - std::cout << "[SegmentedPlanesTerrainModelRos] Benchmarking terrain Callback\n" - << "\tStatistics computed over " << callbackTimer_.getNumTimedIntervals() << " iterations. \n" - << "\tAverage time [ms] " << callbackTimer_.getAverageInMilliseconds() << "\n" - << "\tMaximum time [ms] " << callbackTimer_.getMaxIntervalInMilliseconds() << std::endl; + std::cout + << "[SegmentedPlanesTerrainModelRos] Benchmarking terrain Callback\n" + << "\tStatistics computed over " + << callbackTimer_.getNumTimedIntervals() << " iterations. \n" + << "\tAverage time [ms] " << callbackTimer_.getAverageInMilliseconds() + << "\n" + << "\tMaximum time [ms] " + << callbackTimer_.getMaxIntervalInMilliseconds() << std::endl; } } -std::unique_ptr SegmentedPlanesTerrainModelRos::getTerrainModel() { +std::unique_ptr +SegmentedPlanesTerrainModelRos::getTerrainModel() { std::lock_guard lock(updateMutex_); return std::move(terrainPtr_); } -void SegmentedPlanesTerrainModelRos::createSignedDistanceBetween(const Eigen::Vector3d& minCoordinates, - const Eigen::Vector3d& maxCoordinates) { +void SegmentedPlanesTerrainModelRos::createSignedDistanceBetween( + const Eigen::Vector3d& minCoordinates, + const Eigen::Vector3d& maxCoordinates) { std::lock_guard lock(updateCoordinatesMutex_); minCoordinates_ = minCoordinates; maxCoordinates_ = maxCoordinates; @@ -47,7 +58,7 @@ void SegmentedPlanesTerrainModelRos::createSignedDistanceBetween(const Eigen::Ve void SegmentedPlanesTerrainModelRos::publish() { // Extract point cloud. - std::unique_ptr pointCloud2MsgPtr; + std::unique_ptr pointCloud2MsgPtr; { std::lock_guard lock(pointCloudMutex_); pointCloud2MsgPtr.swap(pointCloud2MsgPtr_); @@ -55,29 +66,34 @@ void SegmentedPlanesTerrainModelRos::publish() { // Publish. if (pointCloud2MsgPtr != nullptr) { - distanceFieldPublisher_.publish(*pointCloud2MsgPtr); + distanceFieldPublisher_->publish(*pointCloud2MsgPtr); } } -void SegmentedPlanesTerrainModelRos::callback(const convex_plane_decomposition_msgs::PlanarTerrain::ConstPtr& msg) { +void SegmentedPlanesTerrainModelRos::callback( + const convex_plane_decomposition_msgs::msg::PlanarTerrain::ConstSharedPtr& + msg) { callbackTimer_.startTimer(); // Read terrain - auto terrainPtr = std::make_unique(convex_plane_decomposition::fromMessage(*msg)); + auto terrainPtr = std::make_unique( + convex_plane_decomposition::fromMessage(*msg)); // Create SDF const std::string elevationLayer = "elevation"; if (terrainPtr->planarTerrain().gridMap.exists(elevationLayer)) { - const auto sdfRange = getSignedDistanceRange(terrainPtr->planarTerrain().gridMap, elevationLayer); + const auto sdfRange = getSignedDistanceRange( + terrainPtr->planarTerrain().gridMap, elevationLayer); terrainPtr->createSignedDistanceBetween(sdfRange.first, sdfRange.second); } // Create pointcloud for visualization const auto* sdfPtr = terrainPtr->getSignedDistanceField(); if (sdfPtr != nullptr) { - const auto& sdf = sdfPtr->asGridmapSdf(); - std::unique_ptr pointCloud2MsgPtr(new sensor_msgs::PointCloud2()); - grid_map::GridMapRosConverter::toPointCloud(sdf, *pointCloud2MsgPtr, 1, [](float val) { return -0.05F <= val && val <= 0.0F; }); + std::unique_ptr pointCloud2MsgPtr( + new sensor_msgs::msg::PointCloud2()); + toPointCloud(*sdfPtr, *pointCloud2MsgPtr, 1, + [](float val) { return -0.05F <= val && val <= 0.0F; }); std::lock_guard lock(pointCloudMutex_); pointCloud2MsgPtr_.swap(pointCloud2MsgPtr); } @@ -90,8 +106,9 @@ void SegmentedPlanesTerrainModelRos::callback(const convex_plane_decomposition_m callbackTimer_.endTimer(); } -std::pair SegmentedPlanesTerrainModelRos::getSignedDistanceRange(const grid_map::GridMap& gridMap, - const std::string& elevationLayer) { +std::pair +SegmentedPlanesTerrainModelRos::getSignedDistanceRange( + const grid_map::GridMap& gridMap, const std::string& elevationLayer) { // Extract coordinates for signed distance field Eigen::Vector3d minCoordinates; Eigen::Vector3d maxCoordinates; @@ -110,9 +127,11 @@ std::pair SegmentedPlanesTerrainModelRos::getS const float minValue = elevationData.minCoeffOfFinites() - heightMargin; const float maxValue = elevationData.maxCoeffOfFinites() + heightMargin; auto minXY = grid_map::lookup::projectToMapWithMargin( - gridMap, grid_map::Position(std::numeric_limits::lowest(), std::numeric_limits::lowest())); + gridMap, grid_map::Position(std::numeric_limits::lowest(), + std::numeric_limits::lowest())); auto maxXY = grid_map::lookup::projectToMapWithMargin( - gridMap, grid_map::Position(std::numeric_limits::max(), std::numeric_limits::max())); + gridMap, grid_map::Position(std::numeric_limits::max(), + std::numeric_limits::max())); minCoordinates = {minXY.x(), minXY.y(), minValue}; maxCoordinates = {maxXY.x(), maxXY.y(), maxValue}; }; @@ -120,4 +139,64 @@ std::pair SegmentedPlanesTerrainModelRos::getS return {minCoordinates, maxCoordinates}; } +void SegmentedPlanesTerrainModelRos::toPointCloud( + const SegmentedPlanesSignedDistanceField& + segmentedPlanesSignedDistanceField, + sensor_msgs::msg::PointCloud2& pointCloud, size_t decimation, + const std::function& condition) { + const auto& gridMap = segmentedPlanesSignedDistanceField.gridMap(); + pcl::PointCloud points; + const grid_map::SignedDistanceField& signedDistanceField = + segmentedPlanesSignedDistanceField.asGridmapSdf(); + signedDistanceField.convertToPointCloud(points); + + pointCloud.header.stamp = rclcpp::Time(static_cast(gridMap.getTimestamp())); + pointCloud.header.frame_id = gridMap.getFrameId(); + + // Fields: Store each point analogous to pcl::PointXYZI + const std::vector fieldNames{"x", "y", "z", "intensity"}; + pointCloud.fields.clear(); + pointCloud.fields.reserve(fieldNames.size()); + size_t offset = 0; + for (const auto& name : fieldNames) { + sensor_msgs::msg::PointField pointField; + pointField.name = name; + pointField.count = 1; + pointField.datatype = sensor_msgs::msg::PointField::FLOAT32; + pointField.offset = offset; + pointCloud.fields.push_back(pointField); + offset += pointField.count * sizeof(float); + } + + // Resize + const size_t pointCloudMaxSize{points.size()}; + pointCloud.height = 1; + pointCloud.width = pointCloudMaxSize; + pointCloud.point_step = offset; + pointCloud.row_step = pointCloud.width * pointCloud.point_step; + pointCloud.data.resize(pointCloud.height * pointCloud.row_step); + + // Fill data + size_t addedPoints = 0; + std::vector filterPoints; + for (auto it = points.begin(); it != points.end(); it++) { + const auto sdfValue = it->intensity; + if (condition(sdfValue)) { + filterPoints.push_back(it->x); + filterPoints.push_back(it->y); + filterPoints.push_back(it->z); + filterPoints.push_back(sdfValue); + ++addedPoints; + } + } + + if (addedPoints != pointCloudMaxSize) { + pointCloud.width = addedPoints; + pointCloud.row_step = pointCloud.width * pointCloud.point_step; + const auto size = pointCloud.height * pointCloud.row_step; + pointCloud.data.resize(size); + memcpy(pointCloud.data.data(), filterPoints.data(), size); + } +} + } // namespace switched_model diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/src/SegmentedPlanesTerrainVisualization.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/src/SegmentedPlanesTerrainVisualization.cpp index 1f20ffd0e..b10ba3f10 100644 --- a/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/src/SegmentedPlanesTerrainVisualization.cpp +++ b/ocs2_robotic_examples/ocs2_perceptive_anymal/segmented_planes_terrain_model/src/SegmentedPlanesTerrainVisualization.cpp @@ -8,9 +8,9 @@ namespace switched_model { -visualization_msgs::MarkerArray getConvexTerrainMarkers(const ConvexTerrain& convexTerrain, ocs2::Color color, double linewidth, +visualization_msgs::msg::MarkerArray getConvexTerrainMarkers(const ConvexTerrain& convexTerrain, ocs2::Color color, double linewidth, double normalLength) { - visualization_msgs::MarkerArray markerArray; + visualization_msgs::msg::MarkerArray markerArray; markerArray.markers.reserve(2); // Mark the surface normal @@ -19,7 +19,7 @@ visualization_msgs::MarkerArray getConvexTerrainMarkers(const ConvexTerrain& con // Polygon message if (!convexTerrain.boundary.empty()) { - std::vector boundary; + std::vector boundary; boundary.reserve(convexTerrain.boundary.size() + 1); for (const auto& point : convexTerrain.boundary) { const auto& pointInWorldFrame = positionInWorldFrameFromPositionInTerrain({point.x(), point.y(), 0.0}, convexTerrain.plane); diff --git a/ocs2_robotic_examples/ocs2_quadrotor/CMakeLists.txt b/ocs2_robotic_examples/ocs2_quadrotor/CMakeLists.txt index dece8bae4..a1ea5ac5f 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_quadrotor/CMakeLists.txt @@ -1,143 +1,123 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_quadrotor) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -set(CATKIN_PACKAGE_DEPENDENCIES - pybind11_catkin - ocs2_mpc - ocs2_ddp - ocs2_robotic_tools - ocs2_robotic_assets - ocs2_python_interface -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(Python3 REQUIRED COMPONENTS Interpreter Development.Module) +find_package(pybind11 REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(ocs2_python_interface REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost - LIBRARIES - ${PROJECT_NAME} -) - -########### -## Build ## -########### +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) # Resolve for the package path at compile time. -configure_file ( +configure_file( "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h.in" - "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY -) - -# Add directories for all targets -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} + "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" + @ONLY ) -# Quadrotor interface library add_library(${PROJECT_NAME} src/QuadrotorInterface.cpp src/QuadrotorSystemDynamics.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_mpc + ocs2_ddp + ocs2_robotic_tools + ocs2_robotic_assets + ocs2_python_interface ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} + Boost::filesystem + Boost::system dl ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) # python bindings -pybind11_add_module(QuadrotorPyBindings SHARED +pybind11_add_module(QuadrotorPyBindings src/pyBindModule.cpp ) -add_dependencies(QuadrotorPyBindings - ${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(QuadrotorPyBindings PRIVATE ${PROJECT_NAME} - ${catkin_LIBRARIES} -) -set_target_properties(QuadrotorPyBindings - PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} ) -catkin_python_setup() - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target ocs2_quadrotor") - add_clang_tooling( - TARGETS - ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) +ament_python_install_package(${PROJECT_NAME} + PACKAGE_DIR src/${PROJECT_NAME} +) ############# ## Install ## ############# 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} + DESTINATION include/${PROJECT_NAME} ) install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) install(TARGETS QuadrotorPyBindings - ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} + LIBRARY DESTINATION ${PYTHON_INSTALL_DIR}/${PROJECT_NAME} ) -############# -## Testing ## -############# - -catkin_add_gtest(${PROJECT_NAME}_PyBindingsTest - test/PyBindingsTest.cpp +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -target_include_directories(${PROJECT_NAME}_PyBindingsTest - PRIVATE ${PROJECT_BINARY_DIR}/include + +ament_export_include_directories( + include + ${PROJECT_BINARY_DIR}/include + ${EIGEN3_INCLUDE_DIRS} ) -target_link_libraries(${PROJECT_NAME}_PyBindingsTest - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_mpc + ocs2_ddp + ocs2_robotic_tools + ocs2_robotic_assets + ocs2_python_interface + Eigen3 + Boost + pybind11_vendor ) + +############ +# Testing ## +############ + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(${PROJECT_NAME}_PyBindingsTest + test/PyBindingsTest.cpp + ) + target_include_directories(${PROJECT_NAME}_PyBindingsTest + PRIVATE ${PROJECT_BINARY_DIR}/include + ) + target_link_libraries(${PROJECT_NAME}_PyBindingsTest + ${PROJECT_NAME} + ) +endif() + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_quadrotor/package.xml b/ocs2_robotic_examples/ocs2_quadrotor/package.xml index ac89ffc34..4471e125e 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor/package.xml +++ b/ocs2_robotic_examples/ocs2_quadrotor/package.xml @@ -1,5 +1,5 @@ - + ocs2_quadrotor 0.0.0 The ocs2_quadrotor package @@ -10,15 +10,22 @@ TODO - catkin + ament_cmake + ament_cmake_python - roslib - pybind11_catkin + pybind11_vendor ocs2_mpc ocs2_ddp ocs2_robotic_tools ocs2_robotic_assets ocs2_python_interface + eigen3_cmake_module + boost + + ament_cmake_gtest + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_quadrotor_ros/CMakeLists.txt index d85fb3d68..9619c51a8 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/CMakeLists.txt @@ -1,122 +1,103 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_quadrotor_ros) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) -set(CATKIN_PACKAGE_DEPENDENCIES - tf - ocs2_mpc - ocs2_ros_interfaces - ocs2_robotic_tools - ocs2_robotic_assets - ocs2_quadrotor -) +find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(geometry_msgs REQUIRED) -find_package(catkin REQUIRED COMPONENTS - roslib - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(ocs2_quadrotor REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include +add_library(${PROJECT_NAME} + src/QuadrotorDummyVisualization.cpp +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost ) - -########### -## Build ## -########### - -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} +ament_target_dependencies(${PROJECT_NAME} + ament_index_cpp + rclcpp + tf2_ros + geometry_msgs + ocs2_ddp + ocs2_mpc + ocs2_ros_interfaces + ocs2_robotic_tools + ocs2_robotic_assets + ocs2_quadrotor ) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -# MPC node -add_executable(quadrotor_mpc - src/QuadrotorMpcNode.cpp -) -add_dependencies(quadrotor_mpc - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(quadrotor_mpc - ${catkin_LIBRARIES} -) +add_executable(quadrotor_mpc src/QuadrotorMpcNode.cpp) +target_link_libraries(quadrotor_mpc ${PROJECT_NAME}) +target_compile_options(quadrotor_mpc PRIVATE ${OCS2_CXX_FLAGS}) -# Dummy node -add_executable(quadrotor_dummy_test - src/DummyQuadrotorNode.cpp - src/QuadrotorDummyVisualization.cpp -) -add_dependencies(quadrotor_dummy_test - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(quadrotor_dummy_test - ${catkin_LIBRARIES} -) +add_executable(quadrotor_dummy_test src/DummyQuadrotorNode.cpp) +target_link_libraries(quadrotor_dummy_test ${PROJECT_NAME}) +target_compile_options(quadrotor_dummy_test PRIVATE ${OCS2_CXX_FLAGS}) -# Target node -add_executable(quadrotor_target - src/QuadrotorTargetPoseCommand.cpp -) -add_dependencies(quadrotor_target - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(quadrotor_target - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} -) +add_executable(quadrotor_target src/QuadrotorTargetPoseCommand.cpp) +target_link_libraries(quadrotor_target ${PROJECT_NAME}) target_compile_options(quadrotor_target PRIVATE ${OCS2_CXX_FLAGS}) -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target ocs2_quadrotor") - add_clang_tooling( - TARGETS - quadrotor_mpc - quadrotor_dummy_test - quadrotor_target - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) - ############# ## Install ## ############# +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(TARGETS quadrotor_mpc quadrotor_dummy_test quadrotor_target + RUNTIME DESTINATION lib/${PROJECT_NAME} +) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} +) +install(DIRECTORY launch rviz + DESTINATION share/${PROJECT_NAME} ) + install( - TARGETS - quadrotor_mpc - quadrotor_dummy_test - quadrotor_target - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) -install(DIRECTORY launch rviz - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ament_index_cpp + rclcpp + tf2_ros + geometry_msgs + ocs2_ddp + ocs2_mpc + ocs2_ros_interfaces + ocs2_robotic_tools + ocs2_robotic_assets + ocs2_quadrotor + Eigen3 + Boost ) + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/include/ocs2_quadrotor_ros/QuadrotorDummyVisualization.h b/ocs2_robotic_examples/ocs2_quadrotor_ros/include/ocs2_quadrotor_ros/QuadrotorDummyVisualization.h index 1de396312..c02a9aa09 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/include/ocs2_quadrotor_ros/QuadrotorDummyVisualization.h +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/include/ocs2_quadrotor_ros/QuadrotorDummyVisualization.h @@ -29,9 +29,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include - #include +#include + +#include #include "ocs2_quadrotor/definitions.h" @@ -40,12 +41,17 @@ namespace quadrotor { class QuadrotorDummyVisualization final : public DummyObserver { public: + explicit QuadrotorDummyVisualization(const rclcpp::Node::SharedPtr& node) + : node_(node), tfBroadcaster_(node) {} ~QuadrotorDummyVisualization() override = default; - void update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) override; + void update(const SystemObservation& observation, + const PrimalSolution& policy, + const CommandData& command) override; private: - tf::TransformBroadcaster tfBroadcaster_; + rclcpp::Node::SharedPtr node_; + tf2_ros::TransformBroadcaster tfBroadcaster_; }; } // namespace quadrotor diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/multiplot.launch.py b/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/multiplot.launch.py new file mode 100644 index 000000000..a7bbdf6d2 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/multiplot.launch.py @@ -0,0 +1,25 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_ros_interfaces'), 'launch/performance_indices.launch.py') + ), + launch_arguments={ + 'mpc_policy_topic_name': 'quadrotor_mpc_policy' + }.items() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/quadrotor.launch.py b/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/quadrotor.launch.py new file mode 100644 index 000000000..ab410719b --- /dev/null +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/quadrotor.launch.py @@ -0,0 +1,62 @@ +import os + +import launch +import launch_ros.actions + +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', + default_value='true' + ), + launch.actions.DeclareLaunchArgument( + name='task_name', + default_value='mpc' + ), + launch.actions.DeclareLaunchArgument( + name='terminal_prefix', + default_value='' + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'ocs2_quadrotor_ros'), 'launch/visualize.launch.py') + ), + launch_arguments={ + 'use_joint_state_publisher': 'false' + }.items() + ), + launch_ros.actions.Node( + package='ocs2_quadrotor_ros', + executable='quadrotor_mpc', + name='quadrotor_mpc', + arguments=[LaunchConfiguration('task_name')], + prefix= "", + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_quadrotor_ros', + executable='quadrotor_dummy_test', + name='quadrotor_dummy_test', + arguments=[LaunchConfiguration('task_name')], + prefix=LaunchConfiguration('terminal_prefix'), + output='screen' + ), + launch_ros.actions.Node( + package='ocs2_quadrotor_ros', + executable='quadrotor_target', + name='quadrotor_target', + arguments=[LaunchConfiguration('task_name')], + prefix=LaunchConfiguration('terminal_prefix'), + output='screen' + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/visualize.launch.py b/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/visualize.launch.py new file mode 100644 index 000000000..82be1a8ed --- /dev/null +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/launch/visualize.launch.py @@ -0,0 +1,49 @@ +import os +import launch +import launch_ros.actions +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +def generate_launch_description(): + rviz_config_file = get_package_share_directory('ocs2_quadrotor_ros') + "/rviz/quadrotor.rviz" + urdf_dir = get_package_share_directory("ocs2_robotic_assets") + urdf_model_path = os.path.join(urdf_dir, "resources/quadrotor/urdf", "quadrotor.urdf") + use_joint_state_publisher_argument = launch.actions.DeclareLaunchArgument( + name='use_joint_state_publisher', + default_value='true' + ) + use_joint_state_publisher = LaunchConfiguration("use_joint_state_publisher") + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + arguments=[urdf_model_path], + ) + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + arguments=[urdf_model_path], + condition=IfCondition(use_joint_state_publisher), + ) + + rviz_node = launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='quadrotor', + output='screen', + arguments=["-d", rviz_config_file] + ) + + ld = launch.LaunchDescription([ + use_joint_state_publisher_argument, + robot_state_publisher_node, + joint_state_publisher_node, + rviz_node + ]) + return ld + +if __name__ == '__main__': + generate_launch_description() diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/package.xml b/ocs2_robotic_examples/ocs2_quadrotor_ros/package.xml index 498084aa3..bbff37463 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/package.xml @@ -1,5 +1,5 @@ - + ocs2_quadrotor_ros 0.0.0 The ocs2_quadrotor_ros package @@ -10,15 +10,24 @@ TODO - catkin + ament_cmake - roslib - tf + ament_index_cpp + rclcpp + tf2_ros + geometry_msgs + ocs2_ddp ocs2_mpc ocs2_ros_interfaces ocs2_robotic_tools ocs2_robotic_assets ocs2_quadrotor + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/rviz/quadrotor.rviz b/ocs2_robotic_examples/ocs2_quadrotor_ros/rviz/quadrotor.rviz index 6ac5ccac0..de5f614a8 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/rviz/quadrotor.rviz +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/rviz/quadrotor.rviz @@ -1,5 +1,5 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: @@ -8,37 +8,33 @@ Panels: - /Status1 - /Grid1 - /RobotModel1 + - /RobotModel1/Description Topic1 Splitter Ratio: 0.5 - Tree Height: 851 - - Class: rviz/Selection + Tree Height: 291 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -55,8 +51,16 @@ Visualization Manager: Reference Frame: Value: true - Alpha: 1 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description Enabled: true Links: All Links Enabled: true @@ -69,8 +73,10 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + Mass Properties: + Inertia: false + Mass: false Name: RobotModel - Robot Description: robot_description TF Prefix: "" Update Interval: 0 Value: true @@ -78,31 +84,49 @@ Visualization Manager: Enabled: true Global Options: Background Color: 255; 255; 255 - Default Light: true Fixed Frame: world Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit + Class: rviz_default_plugins/Orbit Distance: 18.255809783935547 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -125,11 +149,11 @@ Visualization Manager: Saved: ~ Window Geometry: Displays: - collapsed: true + collapsed: false Height: 588 - Hide Left Dock: true + Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001de000003defc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000003de000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000001aefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000001ae000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003540000003efc0100000002fb0000000800540069006d0065010000000000000354000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000001ae00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001de000001aefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001ae000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000001aefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000001ae000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003540000003efc0100000002fb0000000800540069006d0065010000000000000354000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000170000001ae00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -139,5 +163,5 @@ Window Geometry: Views: collapsed: true Width: 852 - X: 2988 + X: 428 Y: 51 diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/DummyQuadrotorNode.cpp b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/DummyQuadrotorNode.cpp index bc8adf82c..76355405c 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/DummyQuadrotorNode.cpp +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/DummyQuadrotorNode.cpp @@ -27,48 +27,55 @@ 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 #include -#include -#include +#include + +#include #include "ocs2_quadrotor_ros/QuadrotorDummyVisualization.h" +#include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { const std::string robotName = "quadrotor"; // task file - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName = std::string(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mrt"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mrt"); // Robot interface - const std::string taskFile = ros::package::getPath("ocs2_quadrotor") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_quadrotor") + "/auto_generated"; + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_quadrotor") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_quadrotor_auto_generated"); ocs2::quadrotor::QuadrotorInterface quadrotorInterface(taskFile, libFolder); // MRT ocs2::MRT_ROS_Interface mrt(robotName); mrt.initRollout(&quadrotorInterface.getRollout()); - mrt.launchNodes(nodeHandle); + mrt.launchNodes(node); // Visualization - auto quadrotorDummyVisualization = std::make_shared(); + auto quadrotorDummyVisualization = + std::make_shared(node); // Dummy loop - ocs2::MRT_ROS_Dummy_Loop dummyQuadrotor(mrt, quadrotorInterface.mpcSettings().mrtDesiredFrequency_, - quadrotorInterface.mpcSettings().mpcDesiredFrequency_); + ocs2::MRT_ROS_Dummy_Loop dummyQuadrotor( + mrt, quadrotorInterface.mpcSettings().mrtDesiredFrequency_, + quadrotorInterface.mpcSettings().mpcDesiredFrequency_); dummyQuadrotor.subscribeObservers({quadrotorDummyVisualization}); // initial state @@ -78,7 +85,8 @@ int main(int argc, char** argv) { initObservation.time = 0.0; // initial command - const ocs2::TargetTrajectories initTargetTrajectories({initObservation.time}, {initObservation.state}, {initObservation.input}); + const ocs2::TargetTrajectories initTargetTrajectories( + {initObservation.time}, {initObservation.state}, {initObservation.input}); // Run dummy (loops while ros is ok) dummyQuadrotor.run(initObservation, initTargetTrajectories); diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorDummyVisualization.cpp b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorDummyVisualization.cpp index d8f7cfcb6..52253d819 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorDummyVisualization.cpp +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorDummyVisualization.cpp @@ -29,39 +29,64 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_quadrotor_ros/QuadrotorDummyVisualization.h" +#include + namespace ocs2 { namespace quadrotor { -void QuadrotorDummyVisualization::update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) { +void QuadrotorDummyVisualization::update(const SystemObservation& observation, + const PrimalSolution& policy, + const CommandData& command) { const auto& targetTrajectories = command.mpcTargetTrajectories_; // publish command transform - const Eigen::Vector3d desiredPositionWorldToTarget(targetTrajectories.stateTrajectory.back()(0), - targetTrajectories.stateTrajectory.back()(1), - targetTrajectories.stateTrajectory.back()(2)); + const Eigen::Vector3d desiredPositionWorldToTarget( + targetTrajectories.stateTrajectory.back()(0), + targetTrajectories.stateTrajectory.back()(1), + targetTrajectories.stateTrajectory.back()(2)); const Eigen::Quaterniond desiredQuaternionBaseToWorld = - Eigen::AngleAxisd{targetTrajectories.stateTrajectory.back()(3), Eigen::Vector3d::UnitZ()} * - Eigen::AngleAxisd{targetTrajectories.stateTrajectory.back()(4), Eigen::Vector3d::UnitY()} * - Eigen::AngleAxisd{targetTrajectories.stateTrajectory.back()(5), Eigen::Vector3d::UnitX()}; - ros::Time timeMsg = ros::Time::now(); - geometry_msgs::TransformStamped command_frame_transform; - command_frame_transform.header.stamp = timeMsg; + Eigen::AngleAxisd{targetTrajectories.stateTrajectory.back()(3), + Eigen::Vector3d::UnitZ()} * + Eigen::AngleAxisd{targetTrajectories.stateTrajectory.back()(4), + Eigen::Vector3d::UnitY()} * + Eigen::AngleAxisd{targetTrajectories.stateTrajectory.back()(5), + Eigen::Vector3d::UnitX()}; + geometry_msgs::msg::TransformStamped command_frame_transform; + command_frame_transform.header.stamp = node_->get_clock()->now(); command_frame_transform.header.frame_id = "odom"; command_frame_transform.child_frame_id = "command"; - command_frame_transform.transform.translation.x = desiredPositionWorldToTarget.x(); - command_frame_transform.transform.translation.y = desiredPositionWorldToTarget.y(); - command_frame_transform.transform.translation.z = desiredPositionWorldToTarget.z(); - command_frame_transform.transform.rotation.w = desiredQuaternionBaseToWorld.w(); - command_frame_transform.transform.rotation.x = desiredQuaternionBaseToWorld.x(); - command_frame_transform.transform.rotation.y = desiredQuaternionBaseToWorld.y(); - command_frame_transform.transform.rotation.z = desiredQuaternionBaseToWorld.z(); + command_frame_transform.transform.translation.x = + desiredPositionWorldToTarget.x(); + command_frame_transform.transform.translation.y = + desiredPositionWorldToTarget.y(); + command_frame_transform.transform.translation.z = + desiredPositionWorldToTarget.z(); + command_frame_transform.transform.rotation.w = + desiredQuaternionBaseToWorld.w(); + command_frame_transform.transform.rotation.x = + desiredQuaternionBaseToWorld.x(); + command_frame_transform.transform.rotation.y = + desiredQuaternionBaseToWorld.y(); + command_frame_transform.transform.rotation.z = + desiredQuaternionBaseToWorld.z(); tfBroadcaster_.sendTransform(command_frame_transform); - tf::Transform transform; - transform.setOrigin(tf::Vector3(observation.state(0), observation.state(1), observation.state(2))); - tf::Quaternion q = tf::createQuaternionFromRPY(observation.state(3), observation.state(4), observation.state(5)); - transform.setRotation(q); - tfBroadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "base")); + const Eigen::Quaterniond q = + Eigen::AngleAxisd{observation.state(3), Eigen::Vector3d::UnitZ()} * + Eigen::AngleAxisd{observation.state(4), Eigen::Vector3d::UnitY()} * + Eigen::AngleAxisd{observation.state(5), Eigen::Vector3d::UnitX()}; + geometry_msgs::msg::TransformStamped base_frame_transform; + base_frame_transform.header.stamp = node_->get_clock()->now(); + base_frame_transform.header.frame_id = "world"; + base_frame_transform.child_frame_id = "base"; + base_frame_transform.transform.translation.x = observation.state(0); + base_frame_transform.transform.translation.y = observation.state(1); + base_frame_transform.transform.translation.z = observation.state(2); + base_frame_transform.transform.rotation.w = q.w(); + base_frame_transform.transform.rotation.x = q.x(); + base_frame_transform.transform.rotation.y = q.y(); + base_frame_transform.transform.rotation.z = q.z(); + tfBroadcaster_.sendTransform(base_frame_transform); } } // namespace quadrotor diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorMpcNode.cpp b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorMpcNode.cpp index 485588d97..a8bd6394c 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorMpcNode.cpp @@ -27,46 +27,57 @@ 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 + +#include + +#include + #include "ocs2_quadrotor/QuadrotorInterface.h" +#include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { const std::string robotName = "quadrotor"; // task file - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); + if (programArgs.size() <= 1) { throw std::runtime_error("No task file specified. Aborting."); } std::string taskFileFolderName(programArgs[1]); // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(robotName + "_mpc"); // Robot interface - const std::string taskFile = ros::package::getPath("ocs2_quadrotor") + "/config/" + taskFileFolderName + "/task.info"; - const std::string libFolder = ros::package::getPath("ocs2_quadrotor") + "/auto_generated"; + const std::string taskFile = + ament_index_cpp::get_package_share_directory("ocs2_quadrotor") + + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = node->declare_parameter( + "libFolder", "/tmp/ocs2_quadrotor_auto_generated"); ocs2::quadrotor::QuadrotorInterface quadrotorInterface(taskFile, libFolder); // ROS ReferenceManager - auto rosReferenceManagerPtr = std::make_shared(robotName, quadrotorInterface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); + auto rosReferenceManagerPtr = std::make_shared( + robotName, quadrotorInterface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(node); // MPC - ocs2::GaussNewtonDDP_MPC mpc(quadrotorInterface.mpcSettings(), quadrotorInterface.ddpSettings(), quadrotorInterface.getRollout(), - quadrotorInterface.getOptimalControlProblem(), quadrotorInterface.getInitializer()); + ocs2::GaussNewtonDDP_MPC mpc(quadrotorInterface.mpcSettings(), + quadrotorInterface.ddpSettings(), + quadrotorInterface.getRollout(), + quadrotorInterface.getOptimalControlProblem(), + quadrotorInterface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // Launch MPC ROS node ocs2::MPC_ROS_Interface mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); // Successful exit return 0; diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorTargetPoseCommand.cpp b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorTargetPoseCommand.cpp index a49e476aa..82d762b17 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorTargetPoseCommand.cpp +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorTargetPoseCommand.cpp @@ -38,7 +38,8 @@ using namespace quadrotor; * @param [in] commadLineTarget : [deltaX, deltaY, deltaZ, deltaYaw] * @param [in] observation : the current observation */ -TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTarget, const SystemObservation& observation) { +TargetTrajectories commandLineToTargetTrajectories( + const vector_t& commadLineTarget, const SystemObservation& observation) { const vector_t targetState = [&]() { vector_t targetState = vector_t::Zero(STATE_DIM); targetState(0) = observation.state(0) + commadLineTarget(0); @@ -46,14 +47,16 @@ TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTar targetState(2) = observation.state(2) + commadLineTarget(2); targetState(3) = observation.state(3); targetState(4) = observation.state(4); - targetState(5) = observation.state(5) + commadLineTarget(3) * M_PI / 180.0; // deg2rad + targetState(5) = + observation.state(5) + commadLineTarget(3) * M_PI / 180.0; // deg2rad return targetState; }(); // target reaching duration constexpr scalar_t averageSpeed = 2.0; const vector_t deltaPose = (targetState - observation.state).head<6>(); - const scalar_t targetTime = observation.time + deltaPose.norm() / averageSpeed; + const scalar_t targetTime = + observation.time + deltaPose.norm() / averageSpeed; // Desired time trajectory const scalar_array_t timeTrajectory{observation.time, targetTime}; @@ -69,14 +72,17 @@ TargetTrajectories commandLineToTargetTrajectories(const vector_t& commadLineTar int main(int argc, char* argv[]) { const std::string robotName = "quadrotor"; - ::ros::init(argc, argv, robotName + "_target"); - ::ros::NodeHandle nodeHandle; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared(robotName + "_target"); // goalPose: [deltaX, deltaY, deltaZ, deltaYaw] const scalar_array_t relativeStateLimit{10.0, 10.0, 10.0, 360.0}; - TargetTrajectoriesKeyboardPublisher targetPoseCommand(nodeHandle, robotName, relativeStateLimit, &commandLineToTargetTrajectories); + TargetTrajectoriesKeyboardPublisher targetPoseCommand( + node, robotName, relativeStateLimit, &commandLineToTargetTrajectories); - const std::string commadMsg = "Enter XYZ, and Yaw (deg) displacements, separated by spaces"; + const std::string commadMsg = + "Enter XYZ, and Yaw (deg) displacements, separated by spaces"; targetPoseCommand.publishKeyboardCommand(commadMsg); // Successful exit diff --git a/ocs2_robotic_examples/ocs2_robotic_examples/CMakeLists.txt b/ocs2_robotic_examples/ocs2_robotic_examples/CMakeLists.txt index b0bbf2530..6328a438b 100644 --- a/ocs2_robotic_examples/ocs2_robotic_examples/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_robotic_examples/CMakeLists.txt @@ -1,4 +1,6 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_robotic_examples) -find_package(catkin REQUIRED) -catkin_metapackage() + +find_package(ament_cmake REQUIRED) + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_robotic_examples/package.xml b/ocs2_robotic_examples/ocs2_robotic_examples/package.xml index 27cc79f14..d14edb4b4 100644 --- a/ocs2_robotic_examples/ocs2_robotic_examples/package.xml +++ b/ocs2_robotic_examples/ocs2_robotic_examples/package.xml @@ -1,5 +1,5 @@ - + ocs2_robotic_examples 0.0.0 The ocs2_robotic_examples metapackage @@ -10,25 +10,25 @@ TODO - catkin + ament_cmake - ocs2_ballbot - ocs2_ballbot_ros - ocs2_cartpole - ocs2_cartpole_ros - ocs2_double_integrator - ocs2_double_integrator_ros - ocs2_quadrotor - ocs2_quadrotor_ros - ocs2_mobile_manipulator - ocs2_mobile_manipulator_ros - ocs2_anymal - ocs2_legged_robot - ocs2_legged_robot_ros - xacro + ocs2_ballbot + ocs2_ballbot_ros + ocs2_cartpole + ocs2_cartpole_ros + ocs2_double_integrator + ocs2_double_integrator_ros + ocs2_quadrotor + ocs2_quadrotor_ros + ocs2_mobile_manipulator + ocs2_mobile_manipulator_ros + ocs2_anymal + ocs2_legged_robot + ocs2_legged_robot_ros + xacro - + ament_cmake - \ No newline at end of file + diff --git a/ocs2_robotic_tools/CMakeLists.txt b/ocs2_robotic_tools/CMakeLists.txt index 6c560fda9..729c64325 100644 --- a/ocs2_robotic_tools/CMakeLists.txt +++ b/ocs2_robotic_tools/CMakeLists.txt @@ -1,70 +1,36 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_robotic_tools) -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) -find_package(catkin REQUIRED COMPONENTS - ocs2_core - ocs2_oc -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc 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_setup log) +find_package(OpenMP REQUIRED) +find_package(Threads REQUIRED) ########### ## 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 + $ + $ + ${EIGEN3_INCLUDE_DIRS} ) -target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) - - -add_executable(${PROJECT_NAME}_lintTarget - src/lintTarget.cpp +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_oc ) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}_lintTarget) - add_clang_tooling( - TARGETS ${PROJECT_NAME}_lintTarget - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -endif(cmake_clang_tools_FOUND) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) ############# ## Install ## @@ -72,15 +38,34 @@ 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( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} +) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_oc + Eigen3 + Boost + OpenMP + Threads +) ############# ## Testing ## @@ -92,13 +77,18 @@ install(DIRECTORY ## to see the summary of unit test results run ## $ catkin_test_results ../../../build/ocs2_robotic_tools -catkin_add_gtest(rotation_transform_tests +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +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}) +endif() + +ament_package() diff --git a/ocs2_robotic_tools/package.xml b/ocs2_robotic_tools/package.xml index b73cabd70..c6692d701 100644 --- a/ocs2_robotic_tools/package.xml +++ b/ocs2_robotic_tools/package.xml @@ -1,5 +1,5 @@ - - + + ocs2_robotic_tools 0.0.0 The ocs2_robotic_tools package @@ -10,12 +10,15 @@ TODO - catkin + ament_cmake - cmake_clang_tools - ocs2_core - ocs2_oc - ocs2_core - ocs2_oc + ocs2_core + ocs2_oc + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_ros_interfaces/CMakeLists.txt b/ocs2_ros_interfaces/CMakeLists.txt index 0fb39c4c2..b5722a484 100644 --- a/ocs2_ros_interfaces/CMakeLists.txt +++ b/ocs2_ros_interfaces/CMakeLists.txt @@ -1,56 +1,20 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_ros_interfaces) -set(CATKIN_PACKAGE_DEPENDENCIES - roscpp - ocs2_msgs - ocs2_core - 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(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 -) - -########### -## Build ## -########### - -## Specify additional locations of header files -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(ocs2_msgs REQUIRED) +find_package(ocs2_core 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(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) add_library(${PROJECT_NAME} src/command/TargetTrajectoriesRosPublisher.cpp @@ -67,85 +31,89 @@ 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 -) -add_dependencies(test_custom_callback_queue - ${catkin_EXPORTED_TARGETS} +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} ) -target_link_libraries(test_custom_callback_queue - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} + rclcpp + ocs2_msgs + ocs2_core + ocs2_mpc + std_msgs + visualization_msgs + geometry_msgs + interactive_markers ) -target_compile_options(test_custom_callback_queue PRIVATE ${OCS2_CXX_FLAGS}) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) # multiplot remap node add_executable(multiplot_remap src/multiplot/MultiplotRemap.cpp ) -add_dependencies(multiplot_remap - ${catkin_EXPORTED_TARGETS} -) 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_link_libraries(${PROJECT_NAME}_lintTarget - ${catkin_LIBRARIES} -) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}_lintTarget) - add_clang_tooling( - TARGETS ${PROJECT_NAME}_lintTarget - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR -) -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} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(TARGETS multiplot_remap + RUNTIME DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) -install(TARGETS multiplot_remap - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + DESTINATION include/${PROJECT_NAME} ) install(DIRECTORY launch multiplot - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${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 +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + rclcpp + ocs2_msgs + ocs2_core + ocs2_mpc + std_msgs + visualization_msgs + geometry_msgs + interactive_markers + Eigen3 + Boost +) + +############ +# Testing ## +############ + +if(BUILD_TESTING) + add_executable(test_custom_callback_queue + test/test_custom_callback_queue.cpp + ) + target_link_libraries(test_custom_callback_queue + ${PROJECT_NAME} + ) + target_compile_options(test_custom_callback_queue PRIVATE ${OCS2_CXX_FLAGS}) +endif() + +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..29ae60cba 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesInteractiveMarker.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesInteractiveMarker.h @@ -29,16 +29,15 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include +#include + #include +#include +#include #include #include -#include -#include - -#include -#include - namespace ocs2 { /** @@ -47,36 +46,45 @@ namespace ocs2 { class TargetTrajectoriesInteractiveMarker final { public: using GaolPoseToTargetTrajectories = std::function; + const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation, + const SystemObservation& observation)>; /** * Constructor * - * @param [in] nodeHandle: ROS node handle. - * @param [in] topicPrefix: The TargetTrajectories will be published on "topicPrefix_mpc_target" topic. Moreover, the latest - * observation is be expected on "topicPrefix_mpc_observation" topic. - * @param [in] gaolPoseToTargetTrajectories: A function which transforms the commanded pose to TargetTrajectories. + * @param [in] node: ROS node handle. + * @param [in] topicPrefix: The TargetTrajectories will be published on + * "topicPrefix_mpc_target" topic. Moreover, the latest 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, - GaolPoseToTargetTrajectories gaolPoseToTargetTrajectories); + TargetTrajectoriesInteractiveMarker( + const 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); + rclcpp::Node::SharedPtr node_; interactive_markers::MenuHandler menuHandler_; interactive_markers::InteractiveMarkerServer server_; GaolPoseToTargetTrajectories gaolPoseToTargetTrajectories_; - std::unique_ptr targetTrajectoriesPublisherPtr_; + std::unique_ptr + targetTrajectoriesPublisherPtr_; - ::ros::Subscriber observationSubscriber_; + rclcpp::Subscription::SharedPtr + observationSubscriber_; mutable std::mutex latestObservationMutex_; SystemObservation latestObservation_; }; 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..f1e0f52cd 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesKeyboardPublisher.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesKeyboardPublisher.h @@ -29,59 +29,68 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include +#include + #include #include #include -#include - -#include -#include +#include "rclcpp/rclcpp.hpp" namespace ocs2 { /** * This class lets the user to insert robot command form command line. */ -class TargetTrajectoriesKeyboardPublisher final { +class TargetTrajectoriesKeyboardPublisher { public: - using CommandLineToTargetTrajectories = - std::function; + using CommandLineToTargetTrajectories = std::function; /** * Constructor * - * @param [in] nodeHandle: ROS node handle. - * @param [in] topicPrefix: The TargetTrajectories will be published on "topicPrefix_mpc_target" topic. Moreover, the latest - * observation is be expected on "topicPrefix_mpc_observation" topic. - * @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. + * @param [in] node: ROS node handle. + * @param [in] topicPrefix: The TargetTrajectories will be published on + * "topicPrefix_mpc_target" topic. Moreover, the latest observation is be + * expected on "topicPrefix_mpc_observation" topic. + * @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, - const scalar_array_t& targetCommandLimits, - CommandLineToTargetTrajectories commandLineToTargetTrajectoriesFun); + TargetTrajectoriesKeyboardPublisher( + const rclcpp::Node::SharedPtr& node, const std::string& topicPrefix, + const scalar_array_t& targetCommandLimits, + CommandLineToTargetTrajectories commandLineToTargetTrajectoriesFun); /** Gets the command vector size. */ size_t targetCommandSize() const { return targetCommandLimits_.size(); } /** - * Publishes command line input. If the input command is shorter than the expected command - * size (targetCommandSize), the method will set the rest of the command to zero. + * Publishes command line input. If the input command is shorter than the + * expected command size (targetCommandSize), the method will set the rest of + * the command to zero. * * @param [in] commadMsg: Message to be displayed on screen. */ - void publishKeyboardCommand(const std::string& commadMsg = "Enter command, separated by space"); + void publishKeyboardCommand( + const std::string& commadMsg = "Enter command, separated by space"); private: /** Gets the target from command line. */ vector_t getCommandLine(); + rclcpp::Node::SharedPtr node_; const vector_t targetCommandLimits_; CommandLineToTargetTrajectories commandLineToTargetTrajectoriesFun_; - std::unique_ptr targetTrajectoriesPublisherPtr_; + std::unique_ptr + targetTrajectoriesPublisherPtr_; - ::ros::Subscriber observationSubscriber_; + rclcpp::Subscription::SharedPtr + observationSubscriber_; mutable std::mutex latestObservationMutex_; SystemObservation latestObservation_; }; 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..944e38030 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesRosPublisher.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesRosPublisher.h @@ -29,29 +29,31 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include -#include - -#include - #include #include +#include +#include +#include + #include "ocs2_ros_interfaces/common/RosMsgConversions.h" +#include "rclcpp/rclcpp.hpp" namespace ocs2 { /** * This class provides a ROS publisher for the TargetTrajectories. */ -class TargetTrajectoriesRosPublisher final { +class TargetTrajectoriesRosPublisher { public: /** * Constructor. - * @param [in] nodeHandle: ROS node handle. - * @param [in] topicPrefix: The TargetTrajectories will be published on "topicPrefix_mpc_target" topic. + * @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 node, + const std::string& topicPrefix = "anonymousRobot"); /** Destructor. */ ~TargetTrajectoriesRosPublisher(); @@ -60,7 +62,9 @@ class TargetTrajectoriesRosPublisher final { void publishTargetTrajectories(const TargetTrajectories& targetTrajectories); private: - ::ros::Publisher targetTrajectoriesPublisher_; + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr + publisher_; }; } // 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 e390d0241..1c59c8835 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h @@ -38,34 +38,40 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include // MPC messages -#include -#include -#include -#include -#include -#include -#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. @@ -74,19 +80,24 @@ 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); /** Creates constraint message. */ -ocs2_msgs::constraint createConstraintMsg(scalar_t time, const vector_t& constraint); +ocs2_msgs::msg::Constraint createConstraintMsg(scalar_t time, + const vector_t& constraint); /** Creates lagrangian_metrics message. */ -ocs2_msgs::lagrangian_metrics createLagrangianMetricsMsg(scalar_t time, LagrangianMetricsConstRef metrics); +ocs2_msgs::msg::LagrangianMetrics createLagrangianMetricsMsg( + scalar_t time, LagrangianMetricsConstRef metrics); /** Creates multiplier message. */ -ocs2_msgs::multiplier createMultiplierMsg(scalar_t time, MultiplierConstRef multiplier); +ocs2_msgs::msg::Multiplier createMultiplierMsg(scalar_t time, + MultiplierConstRef multiplier); } // 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..222193a18 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgHelpers.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgHelpers.h @@ -29,30 +29,35 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include - #include #include +#include +#include +#include +#include +#include -#include -#include -#include -#include +#include "rclcpp/rclcpp.hpp" 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..38982fc08 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 @@ -29,32 +29,29 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include +#include +#include +#include +#include +#include +#include + #include #include #include #include #include +#include +#include +#include +#include +#include #include #include #include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include +#include "rclcpp/rclcpp.hpp" #define PUBLISH_THREAD @@ -71,7 +68,8 @@ class MPC_ROS_Interface { * @param [in] mpc: The underlying MPC class to be used. * @param [in] topicPrefix: The robot's name. */ - explicit MPC_ROS_Interface(MPC_BASE& mpc, std::string topicPrefix = "anonymousRobot"); + explicit MPC_ROS_Interface(MPC_BASE& mpc, + std::string topicPrefix = "anonymousRobot"); /** * Destructor. @@ -96,11 +94,12 @@ class MPC_ROS_Interface { 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. + * 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(const rclcpp::Node::SharedPtr& node); protected: /** @@ -109,7 +108,9 @@ class MPC_ROS_Interface { * @param req: Service request. * @param res: Service response. */ - bool resetMpcCallback(ocs2_msgs::reset::Request& req, ocs2_msgs::reset::Response& res); + void resetMpcCallback( + const std::shared_ptr req, + std::shared_ptr res); /** * Creates MPC Policy message. @@ -119,8 +120,9 @@ 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, - const PerformanceIndex& performanceIndices); + static ocs2_msgs::msg::MpcFlattenedController createMpcPolicyMsg( + const PrimalSolution& primalSolution, const CommandData& commandData, + const PerformanceIndex& performanceIndices); /** * Handles ROS publishing thread. @@ -128,19 +130,21 @@ class MPC_ROS_Interface { void publisherWorker(); /** - * Updates the buffer variables from the MPC object. This method is automatically called by advanceMpc() + * Updates the buffer variables from the MPC object. This method is + * automatically called by advanceMpc() * * @param [in] mpcInitObservation: The observation used to run the MPC. */ void copyToBuffer(const SystemObservation& mpcInitObservation); /** - * The callback method which receives the current observation, invokes the MPC algorithm, - * and finally publishes the optimized policy. + * The callback method which receives the current observation, invokes the MPC + * algorithm, and finally publishes the optimized policy. * * @param [in] msg: The observation message. */ - void mpcObservationCallback(const ocs2_msgs::mpc_observation::ConstPtr& msg); + void mpcObservationCallback( + const ocs2_msgs::msg::MpcObservation::ConstSharedPtr& msg); protected: /* @@ -150,13 +154,16 @@ 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_; @@ -165,7 +172,8 @@ class MPC_ROS_Interface { std::unique_ptr bufferPerformanceIndicesPtr_; std::unique_ptr publisherPerformanceIndicesPtr_; - mutable std::mutex bufferMutex_; // for policy variables with prefix (buffer*) + mutable std::mutex + bufferMutex_; // for policy variables with prefix (buffer*) // multi-threading for publishers std::atomic_bool terminateThread_{false}; diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/DummyObserver.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/DummyObserver.h index f1270bf40..c8f34f32f 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/DummyObserver.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/DummyObserver.h @@ -36,7 +36,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { /** - * This class can be used to observe the dummy loop. Every loop of the dummy, the update method is called on all subscribed observers. + * This class can be used to observe the dummy loop. Every loop of the dummy, + * the update method is called on all subscribed observers. */ class DummyObserver { public: @@ -49,6 +50,8 @@ class DummyObserver { * @param primalSolution : latest MPC primal solution * @param command : latest command on which the MPC solution is based */ - virtual void update(const SystemObservation& observation, const PrimalSolution& primalSolution, const CommandData& command) = 0; + virtual void update(const SystemObservation& observation, + const PrimalSolution& primalSolution, + const CommandData& command) = 0; }; } // namespace ocs2 diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/LoopshapingDummyObserver.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/LoopshapingDummyObserver.h index fbe132156..3d5add435 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/LoopshapingDummyObserver.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/LoopshapingDummyObserver.h @@ -35,18 +35,24 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { /** - * This wraps dummy observers and applies the loopshaping conversion before forwarding the update call. + * This wraps dummy observers and applies the loopshaping conversion before + * forwarding the update call. */ class LoopshapingDummyObserver : public DummyObserver { public: - LoopshapingDummyObserver(std::shared_ptr loopshapingDefinitionPtr, - std::vector> observersPtrArray); + LoopshapingDummyObserver( + std::shared_ptr loopshapingDefinitionPtr, + std::vector> observersPtrArray); ~LoopshapingDummyObserver() override = default; - void update(const SystemObservation& observation, const PrimalSolution& primalSolution, const CommandData& command) override; + void update(const SystemObservation& observation, + const PrimalSolution& primalSolution, + const CommandData& command) override; - void add(std::shared_ptr observer) { observersPtrArray_.push_back(std::move(observer)); } + void add(std::shared_ptr observer) { + observersPtrArray_.push_back(std::move(observer)); + } private: std::shared_ptr loopshapingDefinitionPtr_; diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h index d1c222f21..aa055f8d4 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h @@ -35,20 +35,25 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { /** - * This class implements a loop to test MPC-MRT communication interface using ROS. + * This class implements a loop to test MPC-MRT communication interface using + * ROS. */ class MRT_ROS_Dummy_Loop { public: /** * Constructor. * - * @param [in] mrt: The underlying MRT class to be used. If MRT contains a rollout object, the dummy will roll out - * the received controller using the MRT::rolloutPolicy() method instead of just sending back a planned state. - * @param [in] mrtDesiredFrequency: MRT loop frequency in Hz. This should always set to a positive number. - * @param [in] mpcDesiredFrequency: MPC loop frequency in Hz. If set to a positive number, MPC loop - * will be simulated to run by this frequency. Note that this might not be the MPC's real-time frequency. + * @param [in] mrt: The underlying MRT class to be used. If MRT contains a + * rollout object, the dummy will roll out the received controller using the + * MRT::rolloutPolicy() method instead of just sending back a planned state. + * @param [in] mrtDesiredFrequency: MRT loop frequency in Hz. This should + * always set to a positive number. + * @param [in] mpcDesiredFrequency: MPC loop frequency in Hz. If set to a + * positive number, MPC loop will be simulated to run by this frequency. Note + * that this might not be the MPC's real-time frequency. */ - MRT_ROS_Dummy_Loop(MRT_ROS_Interface& mrt, scalar_t mrtDesiredFrequency, scalar_t mpcDesiredFrequency = -1); + MRT_ROS_Dummy_Loop(MRT_ROS_Interface& mrt, scalar_t mrtDesiredFrequency, + scalar_t mpcDesiredFrequency = -1); /** * Destructor. @@ -61,15 +66,20 @@ class MRT_ROS_Dummy_Loop { * @param [in] initObservation: The initial observation. * @param [in] initTargetTrajectories: The initial TargetTrajectories. */ - void run(const SystemObservation& initObservation, const TargetTrajectories& initTargetTrajectories); + void run(const SystemObservation& initObservation, + const TargetTrajectories& initTargetTrajectories); /** - * Subscribe a set of observers to the dummy loop. Observers are updated in the provided order at the end of each timestep. - * The previous list of observers is overwritten. + * Subscribe a set of observers to the dummy loop. Observers are updated in + * the provided order at the end of each timestep. The previous list of + * observers is overwritten. * * @param observers : vector of observers. */ - void subscribeObservers(const std::vector>& observers) { observers_ = observers; } + void subscribeObservers( + const std::vector>& observers) { + observers_ = observers; + } protected: /** @@ -81,18 +91,23 @@ class MRT_ROS_Dummy_Loop { private: /** - * Runs a loop where mpc optimizations are synchronized with the forward simulation of the system + * Runs a loop where mpc optimizations are synchronized with the forward + * simulation of the system */ - void synchronizedDummyLoop(const SystemObservation& initObservation, const TargetTrajectories& initTargetTrajectories); + void synchronizedDummyLoop(const SystemObservation& initObservation, + const TargetTrajectories& initTargetTrajectories); /** - * Runs a loop where mpc optimizations and simulation of the system are asynchronous. - * The simulation runs as the specified mrtFrequency, and the MPC runs as fast as possible. + * Runs a loop where mpc optimizations and simulation of the system are + * asynchronous. The simulation runs as the specified mrtFrequency, and the + * MPC runs as fast as possible. */ - void realtimeDummyLoop(const SystemObservation& initObservation, const TargetTrajectories& initTargetTrajectories); + void realtimeDummyLoop(const SystemObservation& initObservation, + const TargetTrajectories& initTargetTrajectories); /** Forward simulates the system from current observation*/ - SystemObservation forwardSimulation(const SystemObservation& currentObservation); + SystemObservation forwardSimulation( + const SystemObservation& currentObservation); MRT_ROS_Interface& mrt_; std::vector> observers_; 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..217437167 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,16 +37,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include -#include -#include +#include "rclcpp/rclcpp.hpp" // MPC messages -#include -#include - #include +#include +#include + #include "ocs2_ros_interfaces/common/RosMsgConversions.h" #define PUBLISH_THREAD @@ -54,25 +52,31 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { /** - * This class implements MRT (Model Reference Tracking) communication interface using ROS. + * This class implements MRT (Model Reference Tracking) communication interface + * using ROS. */ class MRT_ROS_Interface : public MRT_BASE { public: /** * Constructor * - * @param [in] topicPrefix: The prefix defines the names for: observation's publishing topic "topicPrefix_mpc_observation", - * policy's receiving topic "topicPrefix_mpc_policy", and MPC reset service "topicPrefix_mpc_reset". + * @param [in] topicPrefix: The prefix defines the names for: observation's + * publishing topic "topicPrefix_mpc_observation", 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 */ ~MRT_ROS_Interface() override; + /** + * @note Must be called BEFORE the node starts spinning (i.e., before + * launchNodes() or any rclcpp::spin). Calling from within a ROS callback + * while the node is spinning may cause a deadlock. + */ void resetMpcNode(const TargetTrajectories& initTargetTrajectories) override; /** @@ -91,12 +95,14 @@ class MRT_ROS_Interface : public MRT_BASE { void spinMRT(); /** - * Launches the ROS publishers and subscribers to communicate with the MPC node. - * @param nodeHandle + * Launches the ROS publishers and subscribers to communicate with the MPC + * node. + * @param node */ - void launchNodes(::ros::NodeHandle& nodeHandle); + void launchNodes(const rclcpp::Node::SharedPtr& node); - void setCurrentObservation(const SystemObservation& currentObservation) override; + void setCurrentObservation( + const SystemObservation& currentObservation) override; private: /** @@ -105,7 +111,8 @@ 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::ConstSharedPtr& msg); /** * Helper function to read a MPC policy message. @@ -115,11 +122,14 @@ 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); /** - * A thread function which sends the current state and checks for a new MPC update. + * A thread function which sends the current state and checks for a new MPC + * update. */ void publisherWorkerThread(); @@ -127,16 +137,16 @@ class MRT_ROS_Interface : public MRT_BASE { std::string topicPrefix_; // Publishers and subscribers - ::ros::Publisher mpcObservationPublisher_; - ::ros::Subscriber mpcPolicySubscriber_; - ::ros::ServiceClient mpcResetServiceClient_; + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr + mpcObservationPublisher_; + rclcpp::Subscription::SharedPtr + mpcPolicySubscriber_; + rclcpp::Client::SharedPtr mpcResetServiceClient_; // ROS messages - ocs2_msgs::mpc_observation mpcObservationMsg_; - ocs2_msgs::mpc_observation mpcObservationMsgBuffer_; - - ::ros::CallbackQueue mrtCallbackQueue_; - ::ros::TransportHints mrtTransportHints_; + ocs2_msgs::msg::MpcObservation mpcObservationMsg_; + ocs2_msgs::msg::MpcObservation mpcObservationMsgBuffer_; // Multi-threading for publishers bool terminateThread_; 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 cd3611392..bf73baa00 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 @@ -29,64 +29,80 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include + #include +#include +#include #include #include -#include - -#include +#include "rclcpp/rclcpp.hpp" namespace ocs2 { /** - * Decorates ReferenceManager with ROS subscribers to receive ModeSchedule and TargetTrajectories through ROS messages. + * Decorates ReferenceManager with ROS subscribers to receive ModeSchedule and + * TargetTrajectories through ROS messages. */ class RosReferenceManager final : public ReferenceManagerDecorator { public: /** * Constructor which decorates referenceManagerPtr. * - * @param [in] topicPrefix: The ReferenceManager will subscribe to "topicPrefix_mode_schedule" and "topicPrefix_mpc_target" - * @param [in] referenceManagerPtr: The ReferenceManager which will be decorated with ROS subscribers functionalities. - * topics to receive user-commanded ModeSchedule and TargetTrajectories respectively. + * @param [in] topicPrefix: The ReferenceManager will subscribe to + * "topicPrefix_mode_schedule" and "topicPrefix_mpc_target" + * @param [in] referenceManagerPtr: The ReferenceManager which will be + * decorated with ROS subscribers functionalities. topics to receive + * user-commanded ModeSchedule and TargetTrajectories respectively. */ - explicit RosReferenceManager(std::string topicPrefix, std::shared_ptr referenceManagerPtr); + explicit RosReferenceManager( + std::string topicPrefix, + std::shared_ptr referenceManagerPtr); ~RosReferenceManager() override = default; /** - * Creates a pointer to RosReferenceManager using a the derived class of type ReferenceManagerInterface, i.e. - * DerivedReferenceManager(args...). + * Creates a pointer to RosReferenceManager using a the derived class of type + * ReferenceManagerInterface, i.e. DerivedReferenceManager(args...). * - * @param [in] topicPrefix: The ReferenceManager will subscribe to "topicPrefix_mode_schedule" and "topicPrefix_mpc_target" - * topics to receive user-commanded ModeSchedule and TargetTrajectories respectively. - * @param args: arguments to forward to the constructor of DerivedReferenceManager + * @param [in] topicPrefix: The ReferenceManager will subscribe to + * "topicPrefix_mode_schedule" and "topicPrefix_mpc_target" topics to receive + * user-commanded ModeSchedule and TargetTrajectories respectively. + * @param args: arguments to forward to the constructor of + * DerivedReferenceManager */ template - static std::unique_ptr create(const std::string& topicPrefix, Args&&... args); + static std::unique_ptr create( + const std::string& topicPrefix, Args&&... args); /** - * Subscribers to "topicPrefix_mode_schedule" and "topicPrefix_mpc_target" topics to receive respectively: - * (1) ModeSchedule : The predefined mode schedule for time-triggered hybrid systems. - * (2) TargetTrajectories : The commanded TargetTrajectories. + * Subscribers to "topicPrefix_mode_schedule" and "topicPrefix_mpc_target" + * topics to receive respectively: (1) ModeSchedule : The predefined mode + * schedule for time-triggered hybrid systems. (2) TargetTrajectories : The + * commanded TargetTrajectories. */ - void subscribe(ros::NodeHandle& nodeHandle); + void subscribe(const rclcpp::Node::SharedPtr& node); private: const std::string topicPrefix_; - - ::ros::Subscriber modeScheduleSubscriber_; - ::ros::Subscriber targetTrajectoriesSubscriber_; + rclcpp::Node::SharedPtr node_; + rclcpp::Subscription::SharedPtr + modeScheduleSubscriber_; + rclcpp::Subscription::SharedPtr + targetTrajectoriesSubscriber_; }; /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ template -std::unique_ptr RosReferenceManager::create(const std::string& topicPrefix, Args&&... args) { - auto referenceManagerPtr = std::make_shared(std::forward(args)...); - return std::make_unique(topicPrefix, std::move(referenceManagerPtr)); +std::unique_ptr RosReferenceManager::create( + const std::string& topicPrefix, Args&&... args) { + auto referenceManagerPtr = + std::make_shared(std::forward(args)...); + return std::make_unique(topicPrefix, + std::move(referenceManagerPtr)); } } // namespace ocs2 diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h index b76dd4278..2b2eab259 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h @@ -29,10 +29,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include - #include +#include "rclcpp/rclcpp.hpp" + namespace ocs2 { namespace ros { @@ -42,45 +42,67 @@ enum class CallbackInterpolationStrategy { }; /** - * Creates a ROS-based callback for SolverObserver that publishes a constraint term at the requested lookahead time points. + * Creates a ROS-based callback for SolverObserver that publishes a constraint + * term at the requested lookahead time points. * - * @param [in] nodeHandle: ROS node handle. - * @param [in] observingTimePoints: An array of lookahead times for which we want to publish the values of constraint. - * @param [in] topicNames: An array of topic names. For each observing time points, you should provide a unique topic name. - * @param [in] interpolationStrategy: The interpolation method used for acquiring data at each time point. - * @return A callback which can be set to SolverObserverModule in order to observe a requested term's constraint. + * @param [in] node: ROS node handle. + * @param [in] observingTimePoints: An array of lookahead times for which we + * want to publish the values of constraint. + * @param [in] topicNames: An array of topic names. For each observing time + * points, you should provide a unique topic name. + * @param [in] interpolationStrategy: The interpolation method used for + * acquiring data at each time point. + * @return A callback which can be set to SolverObserverModule in order to + * observe a requested term's constraint. */ SolverObserver::constraint_callback_t createConstraintCallback( - ::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, const std::vector& topicNames, - CallbackInterpolationStrategy interpolationStrategy = CallbackInterpolationStrategy::nearest_time); + const rclcpp::Node::SharedPtr& node, + const scalar_array_t& observingTimePoints, + const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy = + CallbackInterpolationStrategy::nearest_time); /** - * Creates a ROS-based callback for SolverObserver that publishes a term's LagrangianMetrics at the - * requested lookahead time points. + * Creates a ROS-based callback for SolverObserver that publishes a term's + * LagrangianMetrics at the requested lookahead time points. * - * @param [in] nodeHandle: ROS node handle. - * @param [in] observingTimePoints: An array of lookahead times for which we want to publish the values of LagrangianMetrics. - * @param [in] topicNames: An array of topic names. For each observing time points, you should provide a unique topic name. - * @param [in] interpolationStrategy: The interpolation method used for acquiring data at each time point. - * @return A callback which can be set to SolverObserverModule in order to observe a requested term's LagrangianMetrics. + * @param [in] node: ROS node handle. + * @param [in] observingTimePoints: An array of lookahead times for which we + * want to publish the values of LagrangianMetrics. + * @param [in] topicNames: An array of topic names. For each observing time + * points, you should provide a unique topic name. + * @param [in] interpolationStrategy: The interpolation method used for + * acquiring data at each time point. + * @return A callback which can be set to SolverObserverModule in order to + * observe a requested term's LagrangianMetrics. */ SolverObserver::lagrangian_callback_t createLagrangianCallback( - ::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, const std::vector& topicNames, - CallbackInterpolationStrategy interpolationStrategy = CallbackInterpolationStrategy::nearest_time); + const rclcpp::Node::SharedPtr& node, + const scalar_array_t& observingTimePoints, + const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy = + CallbackInterpolationStrategy::nearest_time); /** - * Creates a ROS-based callback for SolverObserver that publishes a term's Lagrange multiplier at the - * requested lookahead time points. + * Creates a ROS-based callback for SolverObserver that publishes a term's + * Lagrange multiplier at the requested lookahead time points. * - * @param [in] nodeHandle: ROS node handle. - * @param [in] observingTimePoints: An array of lookahead times for which we want to publish the values of multiplier. - * @param [in] topicNames: An array of topic names. For each observing time points, you should provide a unique topic name. - * @param [in] interpolationStrategy: The interpolation method used for acquiring data at each time point. - * @return A callback which can be set to SolverObserverModule in order to observe a requested term's multiplier. + * @param [in] node: ROS node handle. + * @param [in] observingTimePoints: An array of lookahead times for which we + * want to publish the values of multiplier. + * @param [in] topicNames: An array of topic names. For each observing time + * points, you should provide a unique topic name. + * @param [in] interpolationStrategy: The interpolation method used for + * acquiring data at each time point. + * @return A callback which can be set to SolverObserverModule in order to + * observe a requested term's multiplier. */ SolverObserver::multiplier_callback_t createMultiplierCallback( - ::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, const std::vector& topicNames, - CallbackInterpolationStrategy interpolationStrategy = CallbackInterpolationStrategy::nearest_time); + const rclcpp::Node::SharedPtr& node, + const scalar_array_t& observingTimePoints, + const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy = + CallbackInterpolationStrategy::nearest_time); } // namespace ros } // namespace ocs2 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..8b26331a3 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/visualization/VisualizationHelpers.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/visualization/VisualizationHelpers.h @@ -29,32 +29,33 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "ocs2_ros_interfaces/visualization/VisualizationColors.h" - -#include - #include #include +#include + +#include "ocs2_ros_interfaces/visualization/VisualizationColors.h" +#include "rclcpp/rclcpp.hpp" // 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,34 +68,49 @@ 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, - double length, double thickness); +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, - double liftedAlpha); +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, - double forceScale); +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, - Color color, double diameter) { +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(); double sum_z = 0.0; @@ -109,7 +125,8 @@ 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,12 +136,16 @@ visualization_msgs::Marker getCenterOfPressureMarker(ForceIt firstForce, ForceIt } template -visualization_msgs::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 getSupportPolygonMarker(PositionIt firstPos, + PositionIt lastPos, + ContactIt contactIt, + Color color, + double lineWidth) { + 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 + lineList.points.reserve(numElements * (numElements - 1) / + 2); // Upper bound on the number of lines // Loop over all positions for (; firstPos != lastPos; ++firstPos, ++contactIt) { @@ -133,7 +154,8 @@ visualization_msgs::Marker getSupportPolygonMarker(PositionIt firstPos, Position auto nextContact = std::next(contactIt); for (; nextPos != lastPos; ++nextPos, ++nextContact) { if (*contactIt && *nextContact) { - // When positions are both marked as in contact, draw a line between the two points + // When positions are both marked as in contact, draw a line between the + // two points lineList.points.push_back(getPointMsg(*firstPos)); lineList.points.push_back(getPointMsg(*nextPos)); } @@ -151,7 +173,9 @@ double timedExecutionInSeconds(F func) { auto start = std::chrono::steady_clock::now(); func(); auto finish = std::chrono::steady_clock::now(); - return std::chrono::duration_cast>(finish - start).count(); + return std::chrono::duration_cast>(finish - + start) + .count(); } } // namespace ocs2 diff --git a/ocs2_ros_interfaces/launch/performance_indices.launch.py b/ocs2_ros_interfaces/launch/performance_indices.launch.py new file mode 100644 index 000000000..f97a780fa --- /dev/null +++ b/ocs2_ros_interfaces/launch/performance_indices.launch.py @@ -0,0 +1,36 @@ +import os +import sys + +import launch +import launch_ros.actions +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + xml_path = get_package_share_directory('ocs2_ros_interfaces') + "/multiplot/performance_indices.xml" + ld = launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='mpc_policy_topic_name', + default_value='mpc_policy_topic_name' + ), + launch_ros.actions.Node( + package='ocs2_ros_interfaces', + executable='multiplot_remap', + name='multiplot_remap', + arguments=[LaunchConfiguration('mpc_policy_topic_name')], + output='screen' + ) + # , + # launch_ros.actions.Node( + # package='rqt_multiplot', + # executable='rqt_multiplot', + # name='multiplot_performance_indices', + # prefix='--multiplot-run-all --multiplot-config' + xml_path, + # output='screen' + # ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() \ No newline at end of file diff --git a/ocs2_ros_interfaces/package.xml b/ocs2_ros_interfaces/package.xml index 3398852f0..a557c1492 100644 --- a/ocs2_ros_interfaces/package.xml +++ b/ocs2_ros_interfaces/package.xml @@ -1,6 +1,6 @@ - + ocs2_ros_interfaces 0.0.0 @@ -11,12 +11,9 @@ Jan Carius Ruben Grandia - catkin + ament_cmake - cmake_modules - cmake_clang_tools - - roscpp + rclcpp ocs2_msgs ocs2_core ocs2_mpc @@ -24,6 +21,12 @@ visualization_msgs geometry_msgs interactive_markers - rqt_multiplot + eigen3_cmake_module + boost + ros2launch + + + ament_cmake + diff --git a/ocs2_ros_interfaces/src/command/TargetTrajectoriesInteractiveMarker.cpp b/ocs2_ros_interfaces/src/command/TargetTrajectoriesInteractiveMarker.cpp index 264a59f0f..c2ef1ad0e 100644 --- a/ocs2_ros_interfaces/src/command/TargetTrajectoriesInteractiveMarker.cpp +++ b/ocs2_ros_interfaces/src/command/TargetTrajectoriesInteractiveMarker.cpp @@ -29,36 +29,50 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ros_interfaces/command/TargetTrajectoriesInteractiveMarker.h" -#include #include +#include + namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -TargetTrajectoriesInteractiveMarker::TargetTrajectoriesInteractiveMarker(::ros::NodeHandle& nodeHandle, const std::string& topicPrefix, - GaolPoseToTargetTrajectories gaolPoseToTargetTrajectories) - : server_("simple_marker"), gaolPoseToTargetTrajectories_(std::move(gaolPoseToTargetTrajectories)) { +TargetTrajectoriesInteractiveMarker::TargetTrajectoriesInteractiveMarker( + const rclcpp::Node::SharedPtr& node, const std::string& topicPrefix, + GaolPoseToTargetTrajectories gaolPoseToTargetTrajectories) + : node_(node), + server_("simple_marker", node_), + gaolPoseToTargetTrajectories_(std::move(gaolPoseToTargetTrajectories)) { // observation subscriber - auto observationCallback = [this](const ocs2_msgs::mpc_observation::ConstPtr& msg) { - std::lock_guard lock(latestObservationMutex_); - latestObservation_ = ros_msg_conversions::readObservationMsg(*msg); - }; - observationSubscriber_ = nodeHandle.subscribe(topicPrefix + "_mpc_observation", 1, observationCallback); + auto observationCallback = + [this](const ocs2_msgs::msg::MpcObservation::ConstSharedPtr& msg) { + std::lock_guard lock(latestObservationMutex_); + latestObservation_ = ros_msg_conversions::readObservationMsg(*msg); + }; + 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)); + auto feedback_cb = + [&](const visualization_msgs::msg::InteractiveMarkerFeedback:: + ConstSharedPtr& feedback) { processFeedback(feedback); }; + menuHandler_.insert("Send target pose", feedback_cb); // create an interactive marker for our server auto interactiveMarker = createInteractiveMarker(); // add the interactive marker to our collection & // tell the server to call processFeedback() when feedback arrives for it - server_.insert(interactiveMarker); //, boost::bind(&TargetTrajectoriesInteractiveMarker::processFeedback, this, _1)); + server_.insert( + interactiveMarker); //, + // boost::bind(&TargetTrajectoriesInteractiveMarker::processFeedback, + // this, _1)); menuHandler_.apply(server_, interactiveMarker.name); // 'commit' changes and send to all clients @@ -68,10 +82,11 @@ 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_->now(); interactiveMarker.name = "Goal"; interactiveMarker.scale = 0.2; interactiveMarker.description = "Right click to send command"; @@ -79,8 +94,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 +107,11 @@ 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 +119,19 @@ 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 +139,12 @@ 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 +152,12 @@ 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,11 +166,16 @@ 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, - feedback->pose.orientation.z); + 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, feedback->pose.orientation.z); // get the latest observation SystemObservation observation; @@ -158,10 +185,12 @@ void TargetTrajectoriesInteractiveMarker::processFeedback(const visualization_ms } // get TargetTrajectories - const auto targetTrajectories = gaolPoseToTargetTrajectories_(position, orientation, observation); + const auto targetTrajectories = + gaolPoseToTargetTrajectories_(position, orientation, observation); // publish TargetTrajectories - targetTrajectoriesPublisherPtr_->publishTargetTrajectories(targetTrajectories); + targetTrajectoriesPublisherPtr_->publishTargetTrajectories( + targetTrajectories); } } // namespace ocs2 diff --git a/ocs2_ros_interfaces/src/command/TargetTrajectoriesKeyboardPublisher.cpp b/ocs2_ros_interfaces/src/command/TargetTrajectoriesKeyboardPublisher.cpp index df30f83d9..7528d32da 100644 --- a/ocs2_ros_interfaces/src/command/TargetTrajectoriesKeyboardPublisher.cpp +++ b/ocs2_ros_interfaces/src/command/TargetTrajectoriesKeyboardPublisher.cpp @@ -31,44 +31,57 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include #include +#include + namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -TargetTrajectoriesKeyboardPublisher::TargetTrajectoriesKeyboardPublisher(::ros::NodeHandle& nodeHandle, const std::string& topicPrefix, - const scalar_array_t& targetCommandLimits, - CommandLineToTargetTrajectories commandLineToTargetTrajectoriesFun) - : targetCommandLimits_(Eigen::Map(targetCommandLimits.data(), targetCommandLimits.size())), - commandLineToTargetTrajectoriesFun_(std::move(commandLineToTargetTrajectoriesFun)) { +TargetTrajectoriesKeyboardPublisher::TargetTrajectoriesKeyboardPublisher( + const rclcpp::Node::SharedPtr& node, const std::string& topicPrefix, + const scalar_array_t& targetCommandLimits, + CommandLineToTargetTrajectories commandLineToTargetTrajectoriesFun) + : node_(node), + targetCommandLimits_(Eigen::Map( + targetCommandLimits.data(), targetCommandLimits.size())), + commandLineToTargetTrajectoriesFun_( + std::move(commandLineToTargetTrajectoriesFun)) { // observation subscriber - auto observationCallback = [this](const ocs2_msgs::mpc_observation::ConstPtr& msg) { - std::lock_guard lock(latestObservationMutex_); - latestObservation_ = ros_msg_conversions::readObservationMsg(*msg); - }; - observationSubscriber_ = nodeHandle.subscribe(topicPrefix + "_mpc_observation", 1, observationCallback); + auto observationCallback = + [this](const ocs2_msgs::msg::MpcObservation::ConstSharedPtr& msg) { + std::lock_guard lock(latestObservationMutex_); + latestObservation_ = ros_msg_conversions::readObservationMsg(*msg); + }; + observationSubscriber_ = + node->create_subscription( + topicPrefix + "_mpc_observation", 1, observationCallback); // Trajectories publisher - targetTrajectoriesPublisherPtr_.reset(new TargetTrajectoriesRosPublisher(nodeHandle, topicPrefix)); + targetTrajectoriesPublisherPtr_.reset( + new TargetTrajectoriesRosPublisher(node, topicPrefix)); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void TargetTrajectoriesKeyboardPublisher::publishKeyboardCommand(const std::string& commadMsg) { - while (ros::ok() && ros::master::check()) { +void TargetTrajectoriesKeyboardPublisher::publishKeyboardCommand( + const std::string& commadMsg) { + while (rclcpp::ok()) { // get command line std::cout << commadMsg << ": "; - const vector_t commandLineInput = getCommandLine().cwiseMin(targetCommandLimits_).cwiseMax(-targetCommandLimits_); + const vector_t commandLineInput = getCommandLine() + .cwiseMin(targetCommandLimits_) + .cwiseMax(-targetCommandLimits_); // display - std::cout << "The following command is published: [" << toDelimitedString(commandLineInput) << "]\n\n"; + std::cout << "The following command is published: [" + << toDelimitedString(commandLineInput) << "]\n\n"; // get the latest observation - ::ros::spinOnce(); + rclcpp::spin_some(node_->get_node_base_interface()); SystemObservation observation; { std::lock_guard lock(latestObservationMutex_); @@ -76,10 +89,12 @@ void TargetTrajectoriesKeyboardPublisher::publishKeyboardCommand(const std::stri } // get TargetTrajectories - const auto targetTrajectories = commandLineToTargetTrajectoriesFun_(commandLineInput, observation); + const auto targetTrajectories = + commandLineToTargetTrajectoriesFun_(commandLineInput, observation); // publish TargetTrajectories - targetTrajectoriesPublisherPtr_->publishTargetTrajectories(targetTrajectories); + targetTrajectoriesPublisherPtr_->publishTargetTrajectories( + targetTrajectories); } // end of while loop } @@ -88,7 +103,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..b7757ed51 100644 --- a/ocs2_ros_interfaces/src/command/TargetTrajectoriesRosPublisher.cpp +++ b/ocs2_ros_interfaces/src/command/TargetTrajectoriesRosPublisher.cpp @@ -30,32 +30,30 @@ 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 node, const std::string& topicPrefix) : node_(std::move(node)) { + + publisher_ = node_->create_publisher(topicPrefix + "_mpc_target", 1); + RCLCPP_INFO(node_->get_logger(), "The TargetTrajectories is publishing on %s_mpc_target topic.", topicPrefix.c_str()); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -TargetTrajectoriesRosPublisher::~TargetTrajectoriesRosPublisher() { - targetTrajectoriesPublisher_.shutdown(); -} +TargetTrajectoriesRosPublisher::~TargetTrajectoriesRosPublisher() {} /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ void TargetTrajectoriesRosPublisher::publishTargetTrajectories(const TargetTrajectories& targetTrajectories) { const auto mpcTargetTrajectoriesMsg = ros_msg_conversions::createTargetTrajectoriesMsg(targetTrajectories); - targetTrajectoriesPublisher_.publish(mpcTargetTrajectoriesMsg); + publisher_->publish(mpcTargetTrajectoriesMsg); } } // namespace ocs2 diff --git a/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp b/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp index 7c5e82bcd..9ade81948 100644 --- a/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp +++ b/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp @@ -35,8 +35,9 @@ 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,16 +59,21 @@ 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; const auto& state = observationMsg.state.value; - observation.state = Eigen::Map(state.data(), state.size()).cast(); + observation.state = + Eigen::Map(state.data(), state.size()) + .cast(); const auto& input = observationMsg.input.value; - observation.input = Eigen::Map(input.data(), input.size()).cast(); + observation.input = + Eigen::Map(input.data(), input.size()) + .cast(); observation.mode = observationMsg.mode; @@ -77,20 +83,21 @@ 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,37 +106,44 @@ 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.push_back(si); + size_array_t mode_sequence; + mode_sequence.reserve(modeScheduleMsg.mode_sequence.size()); + for (const auto& si : modeScheduleMsg.mode_sequence) { + mode_sequence.push_back(si); } - return {eventTimes, modeSequence}; + return {eventTimes, mode_sequence}; } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -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 +151,20 @@ 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,29 +172,32 @@ 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 = - std::vector(stateTrajectory[i].data(), stateTrajectory[i].data() + stateTrajectory[i].size()); + 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 = - std::vector(inputTrajectory[i].data(), inputTrajectory[i].data() + inputTrajectory[i].size()); + targetTrajectoriesMsg.input_trajectory[i].value = std::vector( + inputTrajectory[i].data(), + inputTrajectory[i].data() + inputTrajectory[i].size()); } // end of i loop return targetTrajectoriesMsg; @@ -184,40 +206,48 @@ 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."); + throw std::runtime_error( + "An empty target trajectories message is received."); } // state and time 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()) - .cast(); + 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()) - .cast(); + desiredInputTrajectory[i] = + Eigen::Map( + targetTrajectoriesMsg.input_trajectory[i].value.data(), + targetTrajectoriesMsg.input_trajectory[i].value.size()) + .cast(); } // end of i loop - return {desiredTimeTrajectory, desiredStateTrajectory, desiredInputTrajectory}; + return {desiredTimeTrajectory, desiredStateTrajectory, + desiredInputTrajectory}; } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::constraint createConstraintMsg(scalar_t time, const vector_t& constraint) { - ocs2_msgs::constraint constraintMsg; +ocs2_msgs::msg::Constraint createConstraintMsg(scalar_t time, + const vector_t& constraint) { + ocs2_msgs::msg::Constraint constraintMsg; constraintMsg.time = time; constraintMsg.value.resize(constraint.size()); @@ -231,8 +261,10 @@ ocs2_msgs::constraint createConstraintMsg(scalar_t time, const vector_t& constra /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::lagrangian_metrics createLagrangianMetricsMsg(scalar_t time, LagrangianMetricsConstRef metrics) { - ocs2_msgs::lagrangian_metrics metricsMsg; +ocs2_msgs::msg::LagrangianMetrics createLagrangianMetricsMsg( + scalar_t time, + LagrangianMetricsConstRef metrics) { + ocs2_msgs::msg::LagrangianMetrics metricsMsg; metricsMsg.time = time; metricsMsg.penalty = metrics.penalty; @@ -248,8 +280,9 @@ ocs2_msgs::lagrangian_metrics createLagrangianMetricsMsg(scalar_t time, Lagrangi /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::multiplier createMultiplierMsg(scalar_t time, MultiplierConstRef multiplier) { - ocs2_msgs::multiplier multiplierMsg; +ocs2_msgs::msg::Multiplier createMultiplierMsg(scalar_t time, + MultiplierConstRef multiplier) { + ocs2_msgs::msg::Multiplier multiplierMsg; multiplierMsg.time = time; multiplierMsg.penalty = multiplier.penalty; 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..2dd541f8d 100644 --- a/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp +++ b/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp @@ -31,6 +31,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ros_interfaces/common/RosMsgConversions.h" +#include + +const rclcpp::Logger LOGGER = rclcpp::get_logger("MPC_ROS_Interface"); + namespace ocs2 { /******************************************************************************************************/ @@ -54,17 +58,17 @@ MPC_ROS_Interface::MPC_ROS_Interface(MPC_BASE& mpc, std::string topicPrefix) /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MPC_ROS_Interface::~MPC_ROS_Interface() { - shutdownNode(); -} +MPC_ROS_Interface::~MPC_ROS_Interface() { shutdownNode(); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MPC_ROS_Interface::resetMpcNode(TargetTrajectories&& initTargetTrajectories) { +void MPC_ROS_Interface::resetMpcNode( + TargetTrajectories&& initTargetTrajectories) { std::lock_guard resetLock(resetMutex_); mpc_.reset(); - mpc_.getSolverPtr()->getReferenceManager().setTargetTrajectories(std::move(initTargetTrajectories)); + mpc_.getSolverPtr()->getReferenceManager().setTargetTrajectories( + std::move(initTargetTrajectories)); mpcTimer_.reset(); resetRequestedEver_ = true; terminateThread_ = false; @@ -74,90 +78,100 @@ 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); +void MPC_ROS_Interface::resetMpcCallback( + const std::shared_ptr req, + std::shared_ptr res) { + if (req->reset) { + auto targetTrajectories = ros_msg_conversions::readTargetTrajectoriesMsg( + req->target_trajectories); resetMpcNode(std::move(targetTrajectories)); - res.done = static_cast(true); + res->done = true; std::cerr << "\n#####################################################" << "\n#####################################################" << "\n################# MPC is reset. ###################" << "\n#####################################################" << "\n#####################################################\n"; - return true; - } else { - ROS_WARN_STREAM("[MPC_ROS_Interface] Reset request failed!"); - return false; + res->done = false; + RCLCPP_WARN_STREAM(LOGGER, "[MPC_ROS_Interface] Reset request failed!"); } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::mpc_flattened_controller MPC_ROS_Interface::createMpcPolicyMsg(const PrimalSolution& primalSolution, - const CommandData& commandData, - const PerformanceIndex& performanceIndices) { - ocs2_msgs::mpc_flattened_controller mpcPolicyMsg; +ocs2_msgs::msg::MpcFlattenedController MPC_ROS_Interface::createMpcPolicyMsg( + const PrimalSolution& primalSolution, const CommandData& commandData, + const PerformanceIndex& performanceIndices) { + 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 = - ros_msg_conversions::createPerformanceIndicesMsg(commandData.mpcInitObservation_.time, 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"); + throw std::runtime_error( + "MPC_ROS_Interface::createMpcPolicyMsg: Unknown ControllerType"); } // 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,14 +179,15 @@ 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); } // end of k loop // serialize controller into data buffer - primalSolution.controllerPtr_->flatten(timeTrajectoryTruncated, policyMsgDataPointers); + primalSolution.controllerPtr_->flatten(timeTrajectoryTruncated, + policyMsgDataPointers); return mpcPolicyMsg; } @@ -197,11 +212,12 @@ void MPC_ROS_Interface::publisherWorker() { publisherPerformanceIndicesPtr_.swap(bufferPerformanceIndicesPtr_); } - ocs2_msgs::mpc_flattened_controller mpcPolicyMsg = - createMpcPolicyMsg(*publisherPrimalSolutionPtr_, *publisherCommandPtr_, *publisherPerformanceIndicesPtr_); + ocs2_msgs::msg::MpcFlattenedController mpcPolicyMsg = + createMpcPolicyMsg(*publisherPrimalSolutionPtr_, *publisherCommandPtr_, + *publisherPerformanceIndicesPtr_); // publish the message - mpcPolicyPublisher_.publish(mpcPolicyMsg); + mpcPolicyPublisher_->publish(mpcPolicyMsg); readyToPublish_ = false; lk.unlock(); @@ -212,20 +228,24 @@ void MPC_ROS_Interface::publisherWorker() { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MPC_ROS_Interface::copyToBuffer(const SystemObservation& mpcInitObservation) { +void MPC_ROS_Interface::copyToBuffer( + const SystemObservation& mpcInitObservation) { // buffer policy mutex std::lock_guard policyBufferLock(bufferMutex_); // get solution - scalar_t finalTime = mpcInitObservation.time + mpc_.settings().solutionTimeWindow_; + scalar_t finalTime = + mpcInitObservation.time + mpc_.settings().solutionTimeWindow_; if (mpc_.settings().solutionTimeWindow_ < 0) { finalTime = mpc_.getSolverPtr()->getFinalTime(); } - mpc_.getSolverPtr()->getPrimalSolution(finalTime, bufferPrimalSolutionPtr_.get()); + mpc_.getSolverPtr()->getPrimalSolution(finalTime, + bufferPrimalSolutionPtr_.get()); // command bufferCommandPtr_->mpcInitObservation_ = mpcInitObservation; - bufferCommandPtr_->mpcTargetTrajectories_ = mpc_.getSolverPtr()->getReferenceManager().getTargetTrajectories(); + bufferCommandPtr_->mpcTargetTrajectories_ = + mpc_.getSolverPtr()->getReferenceManager().getTargetTrajectories(); // performance indices *bufferPerformanceIndicesPtr_ = mpc_.getSolverPtr()->getPerformanceIndeces(); @@ -234,11 +254,14 @@ 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::ConstSharedPtr& 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; } @@ -249,7 +272,8 @@ void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation: mpcTimer_.startTimer(); // run MPC - bool controllerIsUpdated = mpc_.run(currentObservation.time, currentObservation.state); + bool controllerIsUpdated = + mpc_.run(currentObservation.time, currentObservation.state); if (!controllerIsUpdated) { return; } @@ -264,16 +288,21 @@ void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation: timeWindow = mpc_.getSolverPtr()->getFinalTime() - currentObservation.time; } if (timeWindow < 2.0 * mpcTimer_.getAverageInMilliseconds() * 1e-3) { - std::cerr << "WARNING: The solution time window might be shorter than the MPC delay!\n"; + std::cerr << "WARNING: The solution time window might be shorter than the " + "MPC delay!\n"; } // display if (mpc_.settings().debugPrint_) { std::cerr << '\n'; std::cerr << "\n### MPC_ROS Benchmarking"; - std::cerr << "\n### Maximum : " << mpcTimer_.getMaxIntervalInMilliseconds() << "[ms]."; - std::cerr << "\n### Average : " << mpcTimer_.getAverageInMilliseconds() << "[ms]."; - std::cerr << "\n### Latest : " << mpcTimer_.getLastIntervalInMilliseconds() << "[ms]." << std::endl; + std::cerr << "\n### Maximum : " + << mpcTimer_.getMaxIntervalInMilliseconds() << "[ms]."; + std::cerr << "\n### Average : " << mpcTimer_.getAverageInMilliseconds() + << "[ms]."; + std::cerr << "\n### Latest : " + << mpcTimer_.getLastIntervalInMilliseconds() << "[ms]." + << std::endl; } #ifdef PUBLISH_THREAD @@ -283,9 +312,10 @@ void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation: msgReady_.notify_one(); #else - ocs2_msgs::mpc_flattened_controller mpcPolicyMsg = - createMpcPolicyMsg(*bufferPrimalSolutionPtr_, *bufferCommandPtr_, *bufferPerformanceIndicesPtr_); - mpcPolicyPublisher_.publish(mpcPolicyMsg); + ocs2_msgs::msg::MpcFlattenedController mpcPolicyMsg = + createMpcPolicyMsg(*bufferPrimalSolutionPtr_, *bufferCommandPtr_, + *bufferPerformanceIndicesPtr_); + mpcPolicyPublisher_->publish(mpcPolicyMsg); #endif } @@ -294,7 +324,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(LOGGER, "Shutting down workers ..."); std::unique_lock lk(publisherMutex_); terminateThread_ = true; @@ -306,46 +336,61 @@ void MPC_ROS_Interface::shutdownNode() { publisherWorker_.join(); } - ROS_INFO_STREAM("All workers are shut down."); + RCLCPP_INFO(LOGGER, "All workers are shut down."); #endif - - // shutdown publishers - mpcPolicyPublisher_.shutdown(); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ 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)); + RCLCPP_INFO(LOGGER, "Start spinning now ..."); + if (!node_) { + throw std::runtime_error( + "[MPC_ROS_Interface::spin] launchNodes() must be called before spin()."); } + rclcpp::spin(node_); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MPC_ROS_Interface::launchNodes(ros::NodeHandle& nodeHandle) { - ROS_INFO_STREAM("MPC node is setting up ..."); +void MPC_ROS_Interface::launchNodes(const rclcpp::Node::SharedPtr& node) { + RCLCPP_INFO(LOGGER, "MPC node is setting up ..."); + if (!node) { + throw std::runtime_error( + "[MPC_ROS_Interface::launchNodes] node is null."); + } + node_ = node; // Observation subscriber - mpcObservationSubscriber_ = nodeHandle.subscribe(topicPrefix_ + "_mpc_observation", 1, &MPC_ROS_Interface::mpcObservationCallback, this, - ::ros::TransportHints().tcpNoDelay()); + mpcObservationSubscriber_ = + node_->create_subscription( + topicPrefix_ + "_mpc_observation", 1, + std::bind(&MPC_ROS_Interface::mpcObservationCallback, this, + std::placeholders::_1)); // MPC publisher - mpcPolicyPublisher_ = nodeHandle.advertise(topicPrefix_ + "_mpc_policy", 1, true); + const auto latchedQos = + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(); + mpcPolicyPublisher_ = + node_->create_publisher( + topicPrefix_ + "_mpc_policy", latchedQos); // MPC reset service server - mpcResetServiceServer_ = nodeHandle.advertiseService(topicPrefix_ + "_mpc_reset", &MPC_ROS_Interface::resetMpcCallback, this); + mpcResetServiceServer_ = node_->create_service( + topicPrefix_ + "_mpc_reset", + [this](const std::shared_ptr& request, + const std::shared_ptr& response) { + return resetMpcCallback(request, response); + }); // display #ifdef PUBLISH_THREAD - ROS_INFO_STREAM("Publishing SLQ-MPC messages on a separate thread."); + RCLCPP_INFO(LOGGER, "Publishing SLQ-MPC messages on a separate thread."); #endif - ROS_INFO_STREAM("MPC node is ready."); + RCLCPP_INFO(LOGGER, "MPC node is ready."); // spin spin(); diff --git a/ocs2_ros_interfaces/src/mrt/LoopshapingDummyObserver.cpp b/ocs2_ros_interfaces/src/mrt/LoopshapingDummyObserver.cpp index 79e63676b..0f44e66eb 100644 --- a/ocs2_ros_interfaces/src/mrt/LoopshapingDummyObserver.cpp +++ b/ocs2_ros_interfaces/src/mrt/LoopshapingDummyObserver.cpp @@ -34,15 +34,20 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { -LoopshapingDummyObserver::LoopshapingDummyObserver(std::shared_ptr loopshapingDefinitionPtr, - std::vector> observersPtrArray) - : loopshapingDefinitionPtr_(std::move(loopshapingDefinitionPtr)), observersPtrArray_(std::move(observersPtrArray)) {} - -void LoopshapingDummyObserver::update(const SystemObservation& observation, const PrimalSolution& primalSolution, +LoopshapingDummyObserver::LoopshapingDummyObserver( + std::shared_ptr loopshapingDefinitionPtr, + std::vector> observersPtrArray) + : loopshapingDefinitionPtr_(std::move(loopshapingDefinitionPtr)), + observersPtrArray_(std::move(observersPtrArray)) {} + +void LoopshapingDummyObserver::update(const SystemObservation& observation, + const PrimalSolution& primalSolution, const CommandData& command) { if (!observersPtrArray_.empty()) { - const auto systemObservation = loopshapingToSystemObservation(observation, *loopshapingDefinitionPtr_); - const auto systemPrimalSolution = loopshapingToSystemPrimalSolution(primalSolution, *loopshapingDefinitionPtr_); + const auto systemObservation = + loopshapingToSystemObservation(observation, *loopshapingDefinitionPtr_); + const auto systemPrimalSolution = loopshapingToSystemPrimalSolution( + primalSolution, *loopshapingDefinitionPtr_); for (auto& observer : observersPtrArray_) { observer->update(systemObservation, systemPrimalSolution, command); 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..7a64c5bb9 100644 --- a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp +++ b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp @@ -31,36 +31,45 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { +const rclcpp::Logger LOGGER = rclcpp::get_logger("MRT_ROS_Dummy_Loop"); + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MRT_ROS_Dummy_Loop::MRT_ROS_Dummy_Loop(MRT_ROS_Interface& mrt, scalar_t mrtDesiredFrequency, scalar_t mpcDesiredFrequency) - : mrt_(mrt), mrtDesiredFrequency_(mrtDesiredFrequency), mpcDesiredFrequency_(mpcDesiredFrequency) { +MRT_ROS_Dummy_Loop::MRT_ROS_Dummy_Loop(MRT_ROS_Interface& mrt, + scalar_t mrtDesiredFrequency, + scalar_t mpcDesiredFrequency) + : mrt_(mrt), + mrtDesiredFrequency_(mrtDesiredFrequency), + mpcDesiredFrequency_(mpcDesiredFrequency) { if (mrtDesiredFrequency_ < 0) { throw std::runtime_error("MRT loop frequency should be a positive number."); } 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."); } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Dummy_Loop::run(const SystemObservation& initObservation, const TargetTrajectories& initTargetTrajectories) { - ROS_INFO_STREAM("Waiting for the initial policy ..."); +void MRT_ROS_Dummy_Loop::run(const SystemObservation& initObservation, + const TargetTrajectories& initTargetTrajectories) { + RCLCPP_INFO_STREAM(LOGGER, "Waiting for the initial policy ..."); // 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(); + rclcpp::Rate(mrtDesiredFrequency_).sleep(); } - 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) { @@ -73,23 +82,31 @@ void MRT_ROS_Dummy_Loop::run(const SystemObservation& initObservation, const Tar /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Dummy_Loop::synchronizedDummyLoop(const SystemObservation& initObservation, const TargetTrajectories& initTargetTrajectories) { +void MRT_ROS_Dummy_Loop::synchronizedDummyLoop( + const SystemObservation& initObservation, + const TargetTrajectories& initTargetTrajectories) { // Determine the ratio between MPC updates and simulation steps. - const auto mpcUpdateRatio = std::max(static_cast(mrtDesiredFrequency_ / mpcDesiredFrequency_), size_t(1)); + const auto mpcUpdateRatio = + std::max(static_cast(mrtDesiredFrequency_ / mpcDesiredFrequency_), + size_t(1)); // Loop variables size_t loopCounter = 0; SystemObservation currentObservation = initObservation; // Helper function to check if policy is updated and starts at the given time. - // Due to ROS message conversion delay and very fast MPC loop, we might get an old policy instead of the latest one. + // Due to ROS message conversion delay and very fast MPC loop, we might get an + // old policy instead of the latest one. const auto policyUpdatedForTime = [this](scalar_t time) { - constexpr scalar_t tol = 0.1; // policy must start within this fraction of dt - return mrt_.updatePolicy() && std::abs(mrt_.getPolicy().timeTrajectory_.front() - time) < (tol / mpcDesiredFrequency_); + constexpr scalar_t tol = + 0.1; // policy must start within this fraction of dt + 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,10 +115,11 @@ 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"; + std::cout << "<<< New MPC policy starting at " + << mrt_.getPolicy().timeTrajectory_.front() << "\n"; } // Forward simulation @@ -113,7 +131,8 @@ void MRT_ROS_Dummy_Loop::synchronizedDummyLoop(const SystemObservation& initObse // Publish observation if at the next step we want a new policy if ((loopCounter + 1) % mpcUpdateRatio == 0) { mrt_.setCurrentObservation(currentObservation); - std::cout << ">>> Observation is published at " << currentObservation.time << "\n"; + std::cout << ">>> Observation is published at " << currentObservation.time + << "\n"; } // Update observers @@ -122,7 +141,7 @@ void MRT_ROS_Dummy_Loop::synchronizedDummyLoop(const SystemObservation& initObse } ++loopCounter; - ros::spinOnce(); + mrt_.spinMRT(); simRate.sleep(); } } @@ -130,12 +149,14 @@ void MRT_ROS_Dummy_Loop::synchronizedDummyLoop(const SystemObservation& initObse /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Dummy_Loop::realtimeDummyLoop(const SystemObservation& initObservation, const TargetTrajectories& initTargetTrajectories) { +void MRT_ROS_Dummy_Loop::realtimeDummyLoop( + const SystemObservation& initObservation, + const TargetTrajectories& initTargetTrajectories) { // 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 @@ -143,7 +164,8 @@ void MRT_ROS_Dummy_Loop::realtimeDummyLoop(const SystemObservation& initObservat // Update the policy if a new on was received if (mrt_.updatePolicy()) { - std::cout << "<<< New MPC policy starting at " << mrt_.getPolicy().timeTrajectory_.front() << "\n"; + std::cout << "<<< New MPC policy starting at " + << mrt_.getPolicy().timeTrajectory_.front() << "\n"; } // Forward simulation @@ -160,7 +182,7 @@ void MRT_ROS_Dummy_Loop::realtimeDummyLoop(const SystemObservation& initObservat observer->update(currentObservation, mrt_.getPolicy(), mrt_.getCommand()); } - ros::spinOnce(); + // TODO zx rclcpp::spin_some(); simRate.sleep(); } } @@ -168,16 +190,21 @@ void MRT_ROS_Dummy_Loop::realtimeDummyLoop(const SystemObservation& initObservat /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -SystemObservation MRT_ROS_Dummy_Loop::forwardSimulation(const SystemObservation& currentObservation) { +SystemObservation MRT_ROS_Dummy_Loop::forwardSimulation( + const SystemObservation& currentObservation) { const scalar_t dt = 1.0 / mrtDesiredFrequency_; SystemObservation nextObservation; nextObservation.time = currentObservation.time + dt; - if (mrt_.isRolloutSet()) { // If available, use the provided rollout as to integrate the dynamics. - mrt_.rolloutPolicy(currentObservation.time, currentObservation.state, dt, nextObservation.state, nextObservation.input, + if (mrt_.isRolloutSet()) { // If available, use the provided rollout as to + // integrate the dynamics. + mrt_.rolloutPolicy(currentObservation.time, currentObservation.state, dt, + nextObservation.state, nextObservation.input, nextObservation.mode); - } else { // Otherwise, we fake integration by interpolating the current MPC policy at t+dt - mrt_.evaluatePolicy(currentObservation.time + dt, currentObservation.state, nextObservation.state, nextObservation.input, + } else { // Otherwise, we fake integration by interpolating the current MPC + // policy at t+dt + mrt_.evaluatePolicy(currentObservation.time + dt, currentObservation.state, + nextObservation.state, nextObservation.input, nextObservation.mode); } diff --git a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp index 9d013071d..4f38bc24a 100644 --- a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp +++ b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp @@ -32,58 +32,97 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + namespace ocs2 { +const rclcpp::Logger LOGGER = rclcpp::get_logger("MRT_ROS_Interface"); + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MRT_ROS_Interface::MRT_ROS_Interface(std::string topicPrefix, ros::TransportHints mrtTransportHints) - : topicPrefix_(std::move(topicPrefix)), mrtTransportHints_(mrtTransportHints) { +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); + publisherWorker_ = + std::thread(&MRT_ROS_Interface::publisherWorkerThread, this); #endif } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MRT_ROS_Interface::~MRT_ROS_Interface() { - shutdownNodes(); -} +MRT_ROS_Interface::~MRT_ROS_Interface() { shutdownNodes(); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Interface::resetMpcNode(const TargetTrajectories& initTargetTrajectories) { +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); + if (!node_) { + throw std::runtime_error( + "[MRT_ROS_Interface::resetMpcNode] launchNodes() must be called before " + "resetMpcNode()."); + } + if (!mpcResetServiceClient_) { + throw std::runtime_error( + "[MRT_ROS_Interface::resetMpcNode] Reset service client is not " + "initialized."); + } + + auto resetSrvRequest = std::make_shared(); + resetSrvRequest->reset = true; + resetSrvRequest->target_trajectories = + ros_msg_conversions::createTargetTrajectoriesMsg(initTargetTrajectories); + + while (!mpcResetServiceClient_->wait_for_service(std::chrono::seconds(5)) && + rclcpp::ok()) { + RCLCPP_ERROR_STREAM(LOGGER, + "Failed to call service to reset MPC, retrying..."); + } + if (!rclcpp::ok()) { + throw std::runtime_error( + "[MRT_ROS_Interface::resetMpcNode] ROS shutdown while waiting for MPC " + "reset service."); + } - while (!mpcResetServiceClient_.waitForExistence(ros::Duration(5.0)) && ::ros::ok() && ::ros::master::check()) { - ROS_ERROR_STREAM("Failed to call service to reset MPC, retrying..."); + auto future = + mpcResetServiceClient_->async_send_request(resetSrvRequest); + const auto status = + rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + if (status != rclcpp::FutureReturnCode::SUCCESS) { + throw std::runtime_error( + "[MRT_ROS_Interface::resetMpcNode] MPC reset service call failed or " + "timed out."); + } + const auto response = future.get(); + if (!response->done) { + throw std::runtime_error( + "[MRT_ROS_Interface::resetMpcNode] MPC reset service returned done=false."); } - mpcResetServiceClient_.call(resetSrv); - ROS_INFO_STREAM("MPC node has been reset."); + RCLCPP_INFO_STREAM(LOGGER, "MPC node has been reset."); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Interface::setCurrentObservation(const SystemObservation& currentObservation) { +void MRT_ROS_Interface::setCurrentObservation( + const SystemObservation& currentObservation) { #ifdef PUBLISH_THREAD std::unique_lock lk(publisherMutex_); #endif // create the message - mpcObservationMsg_ = ros_msg_conversions::createObservationMsg(currentObservation); + mpcObservationMsg_ = + ros_msg_conversions::createObservationMsg(currentObservation); // publish the current observation #ifdef PUBLISH_THREAD @@ -91,7 +130,7 @@ void MRT_ROS_Interface::setCurrentObservation(const SystemObservation& currentOb lk.unlock(); msgReady_.notify_one(); #else - mpcObservationPublisher_.publish(mpcObservationMsg_); + mpcObservationPublisher_->publish(mpcObservationMsg_); #endif } @@ -115,33 +154,43 @@ void MRT_ROS_Interface::publisherWorkerThread() { lk.unlock(); msgReady_.notify_one(); - mpcObservationPublisher_.publish(mpcObservationMsgBuffer_); + 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); +void MRT_ROS_Interface::readPolicyMsg( + const ocs2_msgs::msg::MpcFlattenedController& msg, CommandData& commandData, + PrimalSolution& primalSolution, PerformanceIndex& performanceIndices) { + 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); - const size_t N = msg.timeTrajectory.size(); + const size_t N = msg.time_trajectory.size(); if (N == 0) { - throw std::runtime_error("[MRT_ROS_Interface::readPolicyMsg] controller message is empty!"); + 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.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!"); + throw std::runtime_error( + "[MRT_ROS_Interface::readPolicyMsg] Data has the wrong length!"); } primalSolution.clear(); - primalSolution.modeSchedule_ = ros_msg_conversions::readModeScheduleMsg(msg.modeSchedule); + primalSolution.modeSchedule_ = + ros_msg_conversions::readModeScheduleMsg(msg.mode_schedule); size_array_t stateDim(N); size_array_t inputDim(N); @@ -149,17 +198,21 @@ void MRT_ROS_Interface::readPolicyMsg(const ocs2_msgs::mpc_flattened_controller& 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]); + 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.stateTrajectory[i].value.data(), stateDim[i]).cast()); + Eigen::Map(msg.state_trajectory[i].value.data(), + stateDim[i]) + .cast()); primalSolution.inputTrajectory_.emplace_back( - Eigen::Map(msg.inputTrajectory[i].value.data(), inputDim[i]).cast()); + Eigen::Map(msg.input_trajectory[i].value.data(), + inputDim[i]) + .cast()); } - primalSolution.postEventIndices_.reserve(msg.postEventIndices.size()); - for (auto ind : msg.postEventIndices) { + primalSolution.postEventIndices_.reserve(msg.post_event_indices.size()); + for (auto ind : msg.post_event_indices) { primalSolution.postEventIndices_.emplace_back(static_cast(ind)); } @@ -169,33 +222,41 @@ void MRT_ROS_Interface::readPolicyMsg(const ocs2_msgs::mpc_flattened_controller& } // 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))); + 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::mpc_flattened_controller::CONTROLLER_LINEAR: { - auto controller = LinearController::unFlatten(stateDim, inputDim, primalSolution.timeTrajectory_, controllerDataPtrArray); - primalSolution.controllerPtr_.reset(new LinearController(std::move(controller))); + 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!"); + throw std::runtime_error( + "[MRT_ROS_Interface::readPolicyMsg] Unknown controllerType!"); } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Interface::mpcPolicyCallback(const ocs2_msgs::mpc_flattened_controller::ConstPtr& msg) { +void MRT_ROS_Interface::mpcPolicyCallback( + const ocs2_msgs::msg::MpcFlattenedController::ConstSharedPtr& msg) { // read new policy and command from msg auto commandPtr = std::make_unique(); auto primalSolutionPtr = std::make_unique(); auto performanceIndicesPtr = std::make_unique(); readPolicyMsg(*msg, *commandPtr, *primalSolutionPtr, *performanceIndicesPtr); - this->moveToBuffer(std::move(commandPtr), std::move(primalSolutionPtr), std::move(performanceIndicesPtr)); + this->moveToBuffer(std::move(commandPtr), std::move(primalSolutionPtr), + std::move(performanceIndicesPtr)); } /******************************************************************************************************/ @@ -203,19 +264,15 @@ void MRT_ROS_Interface::mpcPolicyCallback(const ocs2_msgs::mpc_flattened_control /******************************************************************************************************/ void MRT_ROS_Interface::shutdownNodes() { #ifdef PUBLISH_THREAD - ROS_INFO_STREAM("Shutting down workers ..."); + RCLCPP_INFO_STREAM(LOGGER, "Shutting down workers ..."); 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(); - - // shutdown publishers - mpcObservationPublisher_.shutdown(); + // callback_executor_.cancel(); } /******************************************************************************************************/ @@ -237,41 +294,48 @@ void MRT_ROS_Interface::shutdownPublisher() { /******************************************************************************************************/ /******************************************************************************************************/ void MRT_ROS_Interface::spinMRT() { - mrtCallbackQueue_.callOne(); -}; + // callback_executor_.spin_once(); + if (!node_) { + throw std::runtime_error( + "[MRT_ROS_Interface::spinMRT] launchNodes() must be called before spinMRT()."); + } + rclcpp::spin_some(node_); +} /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Interface::launchNodes(ros::NodeHandle& nodeHandle) { +void MRT_ROS_Interface::launchNodes(const rclcpp::Node::SharedPtr& node) { this->reset(); - + node_ = node; // display - ROS_INFO_STREAM("MRT node is setting up ..."); + RCLCPP_INFO_STREAM(LOGGER, "MRT node is setting up ..."); // observation publisher - mpcObservationPublisher_ = nodeHandle.advertise(topicPrefix_ + "_mpc_observation", 1); + mpcObservationPublisher_ = + node_->create_publisher( + 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); + const auto latchedQos = + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(); + mpcPolicySubscriber_ = + node_->create_subscription( + topicPrefix_ + "_mpc_policy", // topic name + latchedQos, + std::bind(&MRT_ROS_Interface::mpcPolicyCallback, this, + std::placeholders::_1)); // MPC reset service client - mpcResetServiceClient_ = nodeHandle.serviceClient(topicPrefix_ + "_mpc_reset"); + mpcResetServiceClient_ = + node_->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(); } diff --git a/ocs2_ros_interfaces/src/multiplot/MultiplotRemap.cpp b/ocs2_ros_interfaces/src/multiplot/MultiplotRemap.cpp index eca6fda8d..7dd718a80 100644 --- a/ocs2_ros_interfaces/src/multiplot/MultiplotRemap.cpp +++ b/ocs2_ros_interfaces/src/multiplot/MultiplotRemap.cpp @@ -27,56 +27,60 @@ 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 -#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" namespace { -class MultiplotRemap { +class MultiplotRemap : public rclcpp::Node { public: /** Constructor */ - explicit MultiplotRemap(const std::string& mpcPolicyTopicName, ::ros::NodeHandle& nodeHandle) { + explicit MultiplotRemap(const std::string& mpcPolicyTopicName) + : Node("MultiplotRemap") { mpcPolicySubscriber_ = - nodeHandle.subscribe(mpcPolicyTopicName, 1, &MultiplotRemap::mpcPoicyCallback, this, ::ros::TransportHints().tcpNoDelay()); - mpcPerformanceIndicesPublisher_ = nodeHandle.advertise("mpc_performance_indices", 1); + this->create_subscription( + mpcPolicyTopicName, 1, + std::bind(&MultiplotRemap::mpcPoicyCallback, this, + std::placeholders::_1)); + mpcPerformanceIndicesPublisher_ = + this->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); + void mpcPoicyCallback( + const ocs2_msgs::msg::MpcFlattenedController::ConstSharedPtr& 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 int main(int argc, char** argv) { // mpc policy topic name - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector 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; - MultiplotRemap multiplotRemap(mpcPolicyTopicName, nodeHandle); - - ::ros::spin(); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared(mpcPolicyTopicName)); return 0; } diff --git a/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp b/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp index c7e880031..cbbb0845a 100644 --- a/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp +++ b/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp @@ -30,39 +30,48 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h" #include "ocs2_ros_interfaces/common/RosMsgConversions.h" - -#include +#include "rclcpp/rclcpp.hpp" // MPC messages -#include -#include +#include +#include namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -RosReferenceManager::RosReferenceManager(std::string topicPrefix, std::shared_ptr referenceManagerPtr) - : ReferenceManagerDecorator(std::move(referenceManagerPtr)), topicPrefix_(std::move(topicPrefix)) {} +RosReferenceManager::RosReferenceManager( + std::string topicPrefix, + std::shared_ptr referenceManagerPtr) + : ReferenceManagerDecorator(std::move(referenceManagerPtr)), + topicPrefix_(std::move(topicPrefix)) {} /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void RosReferenceManager::subscribe(ros::NodeHandle& nodeHandle) { +void RosReferenceManager::subscribe(const rclcpp::Node::SharedPtr& node) { + node_ = node; // ModeSchedule - auto modeScheduleCallback = [this](const ocs2_msgs::mode_schedule::ConstPtr& msg) { - auto modeSchedule = ros_msg_conversions::readModeScheduleMsg(*msg); + auto modeScheduleCallback = [this](const ocs2_msgs::msg::ModeSchedule& msg) { + auto modeSchedule = ros_msg_conversions::readModeScheduleMsg(msg); referenceManagerPtr_->setModeSchedule(std::move(modeSchedule)); }; - modeScheduleSubscriber_ = nodeHandle.subscribe(topicPrefix_ + "_mode_schedule", 1, modeScheduleCallback); + modeScheduleSubscriber_ = + node_->create_subscription( + topicPrefix_ + "_mode_schedule", 1, modeScheduleCallback); // TargetTrajectories - auto targetTrajectoriesCallback = [this](const ocs2_msgs::mpc_target_trajectories::ConstPtr& msg) { - auto targetTrajectories = ros_msg_conversions::readTargetTrajectoriesMsg(*msg); - referenceManagerPtr_->setTargetTrajectories(std::move(targetTrajectories)); - }; + auto targetTrajectoriesCallback = + [this](const ocs2_msgs::msg::MpcTargetTrajectories& msg) { + auto targetTrajectories = + ros_msg_conversions::readTargetTrajectoriesMsg(msg); + referenceManagerPtr_->setTargetTrajectories( + std::move(targetTrajectories)); + }; targetTrajectoriesSubscriber_ = - nodeHandle.subscribe(topicPrefix_ + "_mpc_target", 1, targetTrajectoriesCallback); + node_->create_subscription( + topicPrefix_ + "_mpc_target", 1, targetTrajectoriesCallback); } } // namespace ocs2 diff --git a/ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp b/ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp index be5de2ad2..81bf512cb 100644 --- a/ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp +++ b/ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp @@ -38,42 +38,57 @@ namespace ros { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -SolverObserver::constraint_callback_t createConstraintCallback(::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, - const std::vector& topicNames, - CallbackInterpolationStrategy interpolationStrategy) { - using vector_ref_array_t = std::vector>; +SolverObserver::constraint_callback_t createConstraintCallback( + const rclcpp::Node::SharedPtr& node, + const scalar_array_t& observingTimePoints, + const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy) { + using vector_ref_array_t = + std::vector>; if (observingTimePoints.size() != topicNames.size()) { - throw std::runtime_error("[createConstraintCallback] For each observing time points, you should provide a unique topic name!"); + throw std::runtime_error( + "[createConstraintCallback] For each observing time points, you should " + "provide a unique topic name!"); } - std::vector<::ros::Publisher> constraintPublishers; + std::vector::SharedPtr> + constraintPublishers; for (const auto& name : topicNames) { - constraintPublishers.push_back(nodeHandle.advertise(name, 1, true)); + constraintPublishers.push_back( + node->create_publisher(name, 1)); } - // note that we need to copy the publishers as the local ones will go out of scope. Good news is that ROS publisher - // behaves like std::sharted_ptr ("Once all copies of a specific Publisher go out of scope, any subscriber status callbacks - // associated with that handle will stop being called.") - return [=](const scalar_array_t& timeTrajectory, const vector_ref_array_t& termConstraintArray) { + // note that we need to copy the publishers as the local ones will go out of + // scope. Good news is that ROS publisher behaves like std::sharted_ptr ("Once + // all copies of a specific Publisher go out of scope, any subscriber status + // callbacks associated with that handle will stop being called.") + return [=](const scalar_array_t& timeTrajectory, + const vector_ref_array_t& termConstraintArray) { if (!timeTrajectory.empty()) { for (size_t i = 0; i < observingTimePoints.size(); i++) { const auto t = timeTrajectory.front() + observingTimePoints[i]; - const auto indexAlpha = LinearInterpolation::timeSegment(t, timeTrajectory); + const auto indexAlpha = + LinearInterpolation::timeSegment(t, timeTrajectory); const auto constraint = [&]() -> vector_t { switch (interpolationStrategy) { case CallbackInterpolationStrategy::nearest_time: - return (indexAlpha.second > 0.5) ? termConstraintArray[indexAlpha.first].get() - : termConstraintArray[indexAlpha.first + 1].get(); + return (indexAlpha.second > 0.5) + ? termConstraintArray[indexAlpha.first].get() + : termConstraintArray[indexAlpha.first + 1].get(); case CallbackInterpolationStrategy::linear_interpolation: return LinearInterpolation::interpolate( indexAlpha, termConstraintArray, - [](const vector_ref_array_t& array, size_t t) -> const vector_t& { return array[t].get(); }); + [](const vector_ref_array_t& array, + size_t t) -> const vector_t& { return array[t].get(); }); default: - throw std::runtime_error("[createConstraintCallback] This CallbackInterpolationStrategy is not implemented!"); + throw std::runtime_error( + "[createConstraintCallback] This " + "CallbackInterpolationStrategy is not implemented!"); } }(); - constraintPublishers[i].publish(ros_msg_conversions::createConstraintMsg(t, constraint)); + constraintPublishers[i]->publish( + ros_msg_conversions::createConstraintMsg(t, constraint)); } } }; @@ -82,38 +97,54 @@ SolverObserver::constraint_callback_t createConstraintCallback(::ros::NodeHandle /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -SolverObserver::lagrangian_callback_t createLagrangianCallback(::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, - const std::vector& topicNames, - CallbackInterpolationStrategy interpolationStrategy) { +SolverObserver::lagrangian_callback_t createLagrangianCallback( + const rclcpp::Node::SharedPtr& node, + const scalar_array_t& observingTimePoints, + const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy) { if (observingTimePoints.size() != topicNames.size()) { - throw std::runtime_error("[createLagrangianCallback] For each observing time points, you should provide a unique topic name!"); + throw std::runtime_error( + "[createLagrangianCallback] For each observing time points, you should " + "provide a unique topic name!"); } - std::vector<::ros::Publisher> metricsPublishers; + std::vector::SharedPtr> + metricsPublishers; for (const auto& name : topicNames) { - metricsPublishers.push_back(nodeHandle.advertise(name, 1, true)); + metricsPublishers.push_back( + node->create_publisher(name, 1)); } - // note that we need to copy the publishers as the local ones will go out of scope. Good news is that ROS publisher - // behaves like std::sharted_ptr ("Once all copies of a specific Publisher go out of scope, any subscriber status callbacks - // associated with that handle will stop being called.") - return [=](const scalar_array_t& timeTrajectory, const std::vector& termMetricsArray) { + // note that we need to copy the publishers as the local ones will go out of + // scope. Good news is that ROS publisher behaves like std::sharted_ptr ("Once + // all copies of a specific Publisher go out of scope, any subscriber status + // callbacks associated with that handle will stop being called.") + return [=](const scalar_array_t& timeTrajectory, + const std::vector& termMetricsArray) { if (!timeTrajectory.empty()) { for (size_t i = 0; i < observingTimePoints.size(); i++) { const auto t = timeTrajectory.front() + observingTimePoints[i]; - const auto indexAlpha = LinearInterpolation::timeSegment(t, timeTrajectory); + const auto indexAlpha = + LinearInterpolation::timeSegment(t, timeTrajectory); const auto metrics = [&]() -> LagrangianMetrics { switch (interpolationStrategy) { case CallbackInterpolationStrategy::nearest_time: - return (indexAlpha.second > 0.5) ? static_cast(termMetricsArray[indexAlpha.first]) - : static_cast(termMetricsArray[indexAlpha.first + 1]); + return (indexAlpha.second > 0.5) + ? static_cast( + termMetricsArray[indexAlpha.first]) + : static_cast( + termMetricsArray[indexAlpha.first + 1]); case CallbackInterpolationStrategy::linear_interpolation: - return LinearInterpolation::interpolate(indexAlpha, termMetricsArray); + return LinearInterpolation::interpolate(indexAlpha, + termMetricsArray); default: - throw std::runtime_error("[createLagrangianCallback] This CallbackInterpolationStrategy is not implemented!"); + throw std::runtime_error( + "[createLagrangianCallback] This " + "CallbackInterpolationStrategy is not implemented!"); } }(); - metricsPublishers[i].publish(ros_msg_conversions::createLagrangianMetricsMsg(t, metrics)); + metricsPublishers[i]->publish( + ros_msg_conversions::createLagrangianMetricsMsg(t, metrics)); } } }; @@ -122,38 +153,54 @@ SolverObserver::lagrangian_callback_t createLagrangianCallback(::ros::NodeHandle /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -SolverObserver::multiplier_callback_t createMultiplierCallback(::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, - const std::vector& topicNames, - CallbackInterpolationStrategy interpolationStrategy) { +SolverObserver::multiplier_callback_t createMultiplierCallback( + const rclcpp::Node::SharedPtr& node, + const scalar_array_t& observingTimePoints, + const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy) { if (observingTimePoints.size() != topicNames.size()) { - throw std::runtime_error("[createMultiplierCallback] For each observing time points, you should provide a unique topic name!"); + throw std::runtime_error( + "[createMultiplierCallback] For each observing time points, you should " + "provide a unique topic name!"); } - std::vector<::ros::Publisher> multiplierPublishers; + std::vector::SharedPtr> + multiplierPublishers; for (const auto& name : topicNames) { - multiplierPublishers.push_back(nodeHandle.advertise(name, 1, true)); + multiplierPublishers.push_back( + node->create_publisher(name, 1)); } - // note that we need to copy the publishers as the local ones will go out of scope. Good news is that ROS publisher - // behaves like std::sharted_ptr ("Once all copies of a specific Publisher go out of scope, any subscriber status callbacks - // associated with that handle will stop being called.") - return [=](const scalar_array_t& timeTrajectory, const std::vector& termMultiplierArray) { + // note that we need to copy the publishers as the local ones will go out of + // scope. Good news is that ROS publisher behaves like std::sharted_ptr ("Once + // all copies of a specific Publisher go out of scope, any subscriber status + // callbacks associated with that handle will stop being called.") + return [=](const scalar_array_t& timeTrajectory, + const std::vector& termMultiplierArray) { if (!timeTrajectory.empty()) { for (size_t i = 0; i < observingTimePoints.size(); i++) { const auto t = timeTrajectory.front() + observingTimePoints[i]; - const auto indexAlpha = LinearInterpolation::timeSegment(t, timeTrajectory); + const auto indexAlpha = + LinearInterpolation::timeSegment(t, timeTrajectory); const auto multiplier = [&]() -> Multiplier { switch (interpolationStrategy) { case CallbackInterpolationStrategy::nearest_time: - return (indexAlpha.second > 0.5) ? static_cast(termMultiplierArray[indexAlpha.first]) - : static_cast(termMultiplierArray[indexAlpha.first + 1]); + return (indexAlpha.second > 0.5) + ? static_cast( + termMultiplierArray[indexAlpha.first]) + : static_cast( + termMultiplierArray[indexAlpha.first + 1]); case CallbackInterpolationStrategy::linear_interpolation: - return LinearInterpolation::interpolate(indexAlpha, termMultiplierArray); + return LinearInterpolation::interpolate(indexAlpha, + termMultiplierArray); default: - throw std::runtime_error("[createMultiplierCallback] This CallbackInterpolationStrategy is not implemented!"); + throw std::runtime_error( + "[createMultiplierCallback] This " + "CallbackInterpolationStrategy is not implemented!"); } }(); - multiplierPublishers[i].publish(ros_msg_conversions::createMultiplierMsg(t, multiplier)); + multiplierPublishers[i]->publish( + ros_msg_conversions::createMultiplierMsg(t, multiplier)); } } }; diff --git a/ocs2_ros_interfaces/src/visualization/VisualizationHelpers.cpp b/ocs2_ros_interfaces/src/visualization/VisualizationHelpers.cpp index d95726ac5..15d450958 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..58cb6532e 100644 --- a/ocs2_ros_interfaces/test/test_custom_callback_queue.cpp +++ b/ocs2_ros_interfaces/test/test_custom_callback_queue.cpp @@ -29,62 +29,49 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include -#include -#include "std_msgs/String.h" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" // Global queue: will be available as a member variable -ros::CallbackQueue my_queue; +rclcpp::CallbackGroup::SharedPtr callback_group_subscriber; -void chatterCallback(const std_msgs::String::ConstPtr& msg) { - printf("I heard: [%s]\n", msg->data.c_str()); -} - -void subscriberWorker() { - ros::Rate loop_rate(30); - while (ros::ok()) { - std::cout << "Check for new messages" << std::endl; - my_queue.callOne(); - loop_rate.sleep(); - } +void chatterCallback(const std_msgs::msg::String& msg) { + printf("I heard: [%s]\n", msg.data.c_str()); } int main(int argc, char* argv[]) { - ros::init(argc, argv, "my_node"); + rclcpp::init(argc, argv); + rclcpp::executors::MultiThreadedExecutor executor; // Publisher - ros::NodeHandle nh_pub; + std::shared_ptr node_pub = rclcpp::Node::make_shared("test_custom_callback_publisher"); size_t publish_queue_size = 1000; - ros::Publisher chatter_pub = nh_pub.advertise("chatter", publish_queue_size); + rclcpp::Publisher::SharedPtr chatter_pub = node_pub->create_publisher("chatter", publish_queue_size); // Subscriber - ros::NodeHandle nh_sub; - nh_sub.setCallbackQueue(&my_queue); - + std::shared_ptr node_sub = rclcpp::Node::make_shared("test_custom_callback_subscriber"); size_t subscribe_queue_size = 1000; - ros::Subscriber sub = nh_sub.subscribe("chatter", subscribe_queue_size, chatterCallback); - - auto subscriberThread = std::thread(&subscriberWorker); + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = node_sub->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::Subscription::SharedPtr sub = node_sub->create_subscription("chatter", subscribe_queue_size, &chatterCallback, sub_opt); - ros::Rate loop_rate(10); + executor.add_node(node_pub); + executor.add_node(node_sub); + rclcpp::Rate loop_rate(10); int count = 0; - while (ros::ok()) { - std_msgs::String msg; + while (rclcpp::ok()) { + std_msgs::msg::String msg; std::stringstream ss; ss << "hello world " << count; msg.data = ss.str(); - chatter_pub.publish(msg); - ros::spinOnce(); + chatter_pub->publish(msg); + executor.spin_once(); loop_rate.sleep(); ++count; } - if (subscriberThread.joinable()) { - subscriberThread.join(); - }; - return 0; } diff --git a/ocs2_slp/CMakeLists.txt b/ocs2_slp/CMakeLists.txt index 3ac73e7de..b30a98569 100644 --- a/ocs2_slp/CMakeLists.txt +++ b/ocs2_slp/CMakeLists.txt @@ -1,48 +1,22 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_slp) -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_mpc - ocs2_oc - ocs2_qp_solver -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_qp_solver REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) +find_package(OpenMP REQUIRED) +find_package(Threads REQUIRED) -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - LIBRARIES - ${PROJECT_NAME} - DEPENDS - Boost -) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) ########### ## Build ## ########### -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} -) add_library(${PROJECT_NAME} src/pipg/PipgSettings.cpp @@ -52,60 +26,72 @@ add_library(${PROJECT_NAME} src/SlpSettings.cpp src/SlpSolver.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_mpc + ocs2_oc + ocs2_qp_solver ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -add_executable(${PROJECT_NAME}_lintTarget - src/lintTarget.cpp -) - -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}_lintTarget) - add_clang_tooling( - TARGETS ${PROJECT_NAME}_lintTarget ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -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} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + DESTINATION include/${PROJECT_NAME}) + +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_mpc + ocs2_oc + ocs2_qp_solver + Eigen3 + Boost + OpenMP + Threads +) ############# ## Testing ## ############# -catkin_add_gtest(test_${PROJECT_NAME} +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(test_${PROJECT_NAME} test/testHelpers.cpp test/testPipgSolver.cpp test/testSlpSolver.cpp ) -add_dependencies(test_${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) +endif() + +ament_package() diff --git a/ocs2_slp/package.xml b/ocs2_slp/package.xml index 52ba012d7..69ffc031f 100644 --- a/ocs2_slp/package.xml +++ b/ocs2_slp/package.xml @@ -1,5 +1,5 @@ - + ocs2_slp 0.0.0 A numerical implementation of a first order primal-dual MPC basee on PIPG. @@ -9,12 +9,17 @@ BSD3 - catkin + ament_cmake + ocs2_core ocs2_mpc ocs2_oc - - ocs2_qp_solver + eigen3_cmake_module + boost + + + ament_cmake + - \ No newline at end of file + diff --git a/ocs2_sqp/blasfeo_catkin/CMakeLists.txt b/ocs2_sqp/blasfeo_catkin/CMakeLists.txt index d6f371272..57db69beb 100644 --- a/ocs2_sqp/blasfeo_catkin/CMakeLists.txt +++ b/ocs2_sqp/blasfeo_catkin/CMakeLists.txt @@ -1,53 +1,53 @@ cmake_minimum_required(VERSION 3.14) project(blasfeo_catkin) -find_package(catkin REQUIRED) +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}) - -# BLASFEO Settings -set(BUILD_SHARED_LIBS ON CACHE STRING "Build shared libraries" FORCE) +# BLASFEO settings +set(BUILD_SHARED_LIBS ON CACHE BOOL "Build shared libraries" FORCE) set(TARGET X64_AUTOMATIC CACHE STRING "Target architecture" FORCE) -set(BLASFEO_EXAMPLES OFF CACHE BOOL "Examples enabled") - -# Download & build source -FetchContent_Declare(blasfeoDownload - GIT_REPOSITORY https://github.com/giaf/blasfeo - GIT_TAG ae6e2d1dea015862a09990b95905038a756ffc7d - UPDATE_COMMAND "" - SOURCE_DIR ${BLASFEO_DOWNLOAD_DIR} - BINARY_DIR ${BLASFEO_BUILD_DIR} - BUILD_COMMAND $(MAKE) - INSTALL_COMMAND "$(MAKE) install" +set(BLASFEO_EXAMPLES OFF CACHE BOOL "Examples enabled" FORCE) + +FetchContent_Declare(blasfeodownload + GIT_REPOSITORY https://github.com/giaf/blasfeo + GIT_TAG ae6e2d1dea015862a09990b95905038a756ffc7d + UPDATE_COMMAND "" +) +FetchContent_MakeAvailable(blasfeodownload) + +# Install blasfeo target and headers into this package prefix +install( + TARGETS blasfeo + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -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() - -# Install the library where catkin expects them -set_target_properties(blasfeo PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${BLASFEO_LIB_DIR}) - -# Propagate dependencies -catkin_package( - INCLUDE_DIRS ${BLASFEO_INCLUDE_DIR} - LIBRARIES blasfeo - CFG_EXTRAS blasfeo-extras.cmake +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) + +install( + DIRECTORY ${blasfeodownload_SOURCE_DIR}/include/ + DESTINATION include +) + +# Provide BLASFEO_PATH for downstream packages (used by hpipm_catkin) +set(BLASFEO_DEVEL_PREFIX ${CMAKE_INSTALL_PREFIX}) +configure_file( + cmake/blasfeo-extras.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/blasfeo-extras.cmake + @ONLY +) +install( + FILES ${CMAKE_CURRENT_BINARY_DIR}/blasfeo-extras.cmake + DESTINATION share/${PROJECT_NAME}/cmake +) + +ament_export_include_directories(include) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) + +ament_package(CONFIG_EXTRAS "${CMAKE_CURRENT_BINARY_DIR}/blasfeo-extras.cmake") diff --git a/ocs2_sqp/blasfeo_catkin/package.xml b/ocs2_sqp/blasfeo_catkin/package.xml index b7059c424..81a05eeb1 100644 --- a/ocs2_sqp/blasfeo_catkin/package.xml +++ b/ocs2_sqp/blasfeo_catkin/package.xml @@ -1,5 +1,5 @@ - + blasfeo_catkin 0.0.0 Catkin wrapper for Blasfeo. @@ -7,5 +7,9 @@ See package - catkin + ament_cmake + + ament_cmake + + diff --git a/ocs2_sqp/hpipm_catkin/CMakeLists.txt b/ocs2_sqp/hpipm_catkin/CMakeLists.txt index 9cad4c92a..61434162b 100644 --- a/ocs2_sqp/hpipm_catkin/CMakeLists.txt +++ b/ocs2_sqp/hpipm_catkin/CMakeLists.txt @@ -1,134 +1,120 @@ cmake_minimum_required(VERSION 3.14) project(hpipm_catkin) -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - blasfeo_catkin - ocs2_core - ocs2_oc - ocs2_qp_solver -) - +find_package(ament_cmake REQUIRED) +find_package(blasfeo_catkin REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_qp_solver REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) +find_package(OpenMP REQUIRED) +find_package(Threads REQUIRED) 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}) - -message(STATUS "BLASFEO_PATH: " ${BLASFEO_PATH}) +message(STATUS "BLASFEO_PATH: ${BLASFEO_PATH}") -# HPIPM Settings +# HPIPM settings set(BLASFEO_PATH ${BLASFEO_PATH} CACHE STRING "BLASFEO installation path" FORCE) set(BLASFEO_INCLUDE_DIR ${BLASFEO_PATH}/include CACHE STRING "Path to BLASFEO header files." FORCE) -set(BUILD_SHARED_LIBS ON CACHE STRING "Build shared libraries" FORCE) -set(HPIPM_TESTING OFF CACHE BOOL "Examples enabled") +set(BUILD_SHARED_LIBS ON CACHE BOOL "Build shared libraries" FORCE) +set(HPIPM_TESTING OFF CACHE BOOL "Examples enabled" FORCE) -# Download & build source -FetchContent_Declare(hpipmDownload +FetchContent_Declare(hpipmdownload GIT_REPOSITORY https://github.com/giaf/hpipm GIT_TAG 255ffdf38d3a5e2c3285b29568ce65ae286e5faf UPDATE_COMMAND "" - SOURCE_DIR ${HPIPM_DOWNLOAD_DIR} - BINARY_DIR ${HPIPM_BUILD_DIR} - BUILD_COMMAND $(MAKE) - INSTALL_COMMAND "$(MAKE) install" - ) -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 ) +FetchContent_MakeAvailable(hpipmdownload) ########### ## Build ## ########### -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} -) - # Hpipm interface add_library(${PROJECT_NAME} src/HpipmInterface.cpp src/HpipmInterfaceSettings.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} hpipm + blasfeo_catkin::blasfeo +) +target_link_directories(${PROJECT_NAME} + PUBLIC + ${BLASFEO_PATH}/lib +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + blasfeo_catkin + ocs2_core + ocs2_oc + ocs2_qp_solver ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}) - add_clang_tooling( - TARGETS - ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -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} + TARGETS ${PROJECT_NAME} hpipm + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + DESTINATION include/${PROJECT_NAME} +) + +install(DIRECTORY ${hpipmdownload_SOURCE_DIR}/include/ + DESTINATION include +) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + blasfeo_catkin + ocs2_core + ocs2_oc + ocs2_qp_solver + Eigen3 + Boost + OpenMP + Threads +) ############# ## Testing ## ############# -catkin_add_gtest(test_${PROJECT_NAME} +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_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 ) +endif() + +ament_package() diff --git a/ocs2_sqp/hpipm_catkin/package.xml b/ocs2_sqp/hpipm_catkin/package.xml index 33f411ebe..9219ff2ae 100644 --- a/ocs2_sqp/hpipm_catkin/package.xml +++ b/ocs2_sqp/hpipm_catkin/package.xml @@ -1,5 +1,5 @@ - + hpipm_catkin 0.0.0 Catkin wrapper for HPIPM. @@ -7,9 +7,17 @@ See package - catkin + ament_cmake + + blasfeo_catkin ocs2_core + ocs2_oc ocs2_qp_solver - blasfeo_catkin + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_sqp/ocs2_sqp/CMakeLists.txt b/ocs2_sqp/ocs2_sqp/CMakeLists.txt index fae6f1ddf..8d092cbf2 100644 --- a/ocs2_sqp/ocs2_sqp/CMakeLists.txt +++ b/ocs2_sqp/ocs2_sqp/CMakeLists.txt @@ -1,110 +1,111 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_sqp) -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_mpc - ocs2_oc - ocs2_qp_solver - blasfeo_catkin - hpipm_catkin -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_qp_solver REQUIRED) +find_package(blasfeo_catkin REQUIRED) +find_package(hpipm_catkin REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) +find_package(OpenMP REQUIRED) +find_package(Threads REQUIRED) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - LIBRARIES - ${PROJECT_NAME} - DEPENDS - Boost -) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) ########### ## Build ## ########### -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} -) - # Multiple shooting solver library add_library(${PROJECT_NAME} src/SqpLogging.cpp src/SqpSettings.cpp src/SqpSolver.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} + Boost::system + Boost::filesystem + Boost::log_setup + Boost::log + OpenMP::OpenMP_CXX + Threads::Threads + blasfeo_catkin::blasfeo +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_mpc + ocs2_oc + ocs2_qp_solver + blasfeo_catkin + hpipm_catkin ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}) - add_clang_tooling( - TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -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} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + DESTINATION include/${PROJECT_NAME}) + +install( + EXPORT export_${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME}/cmake +) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_mpc + ocs2_oc + ocs2_qp_solver + blasfeo_catkin + hpipm_catkin + Eigen3 + Boost + OpenMP + Threads +) ############# ## Testing ## ############# -catkin_add_gtest(test_${PROJECT_NAME} +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(test_${PROJECT_NAME} test/testCircularKinematics.cpp test/testSwitchedProblem.cpp 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 +) +endif() + +ament_package() diff --git a/ocs2_sqp/ocs2_sqp/package.xml b/ocs2_sqp/ocs2_sqp/package.xml index 9402df863..33c21a3c2 100644 --- a/ocs2_sqp/ocs2_sqp/package.xml +++ b/ocs2_sqp/ocs2_sqp/package.xml @@ -1,5 +1,5 @@ - + ocs2_sqp 0.0.0 The ocs2_hpipm package @@ -8,12 +8,19 @@ TODO - catkin + ament_cmake + ocs2_core ocs2_mpc ocs2_oc ocs2_qp_solver blasfeo_catkin hpipm_catkin + eigen3_cmake_module + boost + + + ament_cmake + diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index 26f3fd1f4..de00697f2 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -30,6 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_sqp/SqpSolver.h" #include +#include #include #include diff --git a/ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt b/ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt index 800dfc775..f53284b88 100644 --- a/ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt +++ b/ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt @@ -1,39 +1,14 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(ocs2_qp_solver) -find_package(catkin REQUIRED COMPONENTS - ocs2_core - ocs2_oc -) - +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log) -# 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 -) - -include_directories( - include - test/include - ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) # Declare a C++ library add_library(${PROJECT_NAME} @@ -43,48 +18,57 @@ add_library(${PROJECT_NAME} src/QpTrajectories.cpp ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_oc ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}) - add_clang_tooling( - TARGETS - ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR - ) -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} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + DESTINATION include/${PROJECT_NAME}) install(DIRECTORY test/include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} +) + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies( + ocs2_core + ocs2_oc + Eigen3 ) ############# ## Testing ## ############# -catkin_add_gtest(test_${PROJECT_NAME} +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(test_${PROJECT_NAME} test/testDiscreteTranscription.cpp test/testQpSolver.cpp test/testOcs2QpSolver.cpp @@ -93,3 +77,6 @@ target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} gtest_main ) +endif() + +ament_package() diff --git a/ocs2_test_tools/ocs2_qp_solver/package.xml b/ocs2_test_tools/ocs2_qp_solver/package.xml index 440c5ce43..b40a13058 100644 --- a/ocs2_test_tools/ocs2_qp_solver/package.xml +++ b/ocs2_test_tools/ocs2_qp_solver/package.xml @@ -1,5 +1,5 @@ - + ocs2_qp_solver 0.0.0 The ocs2_qp_solver package @@ -10,9 +10,14 @@ BSD - catkin - cmake_clang_tools + ament_cmake + ocs2_core ocs2_oc + eigen3_cmake_module + + + ament_cmake + diff --git a/ocs2_thirdparty/CMakeLists.txt b/ocs2_thirdparty/CMakeLists.txt index 9217b8724..e8bf7b493 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.5) project(ocs2_thirdparty) -find_package(catkin) +find_package(ament_cmake REQUIRED) -catkin_package( - INCLUDE_DIRS - include -) +ament_export_include_directories(include) install(DIRECTORY include/ - DESTINATION "${CATKIN_PACKAGE_INCLUDE_DESTINATION}/../") + DESTINATION include +) + +ament_package() diff --git a/ocs2_thirdparty/package.xml b/ocs2_thirdparty/package.xml index 01135f0ec..926d3eca0 100644 --- a/ocs2_thirdparty/package.xml +++ b/ocs2_thirdparty/package.xml @@ -1,5 +1,5 @@ - + ocs2_thirdparty 0.0.1 Package containing third party libraries that OCS2 uses. @@ -10,8 +10,10 @@ Check individual license files for each library - catkin - cmake_modules + ament_cmake - + + ament_cmake + +