From 16c37cbef1a0ce643c95c4a64e1e9b26a364a027 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Sat, 4 May 2024 20:29:12 +0000 Subject: [PATCH 01/39] Fix devcontainer --- .devcontainer/devcontainer.json | 17 +++--- Dockerfile | 88 +++++++++++++++-------------- nav2_map_server/CMakeLists.txt | 6 +- nav2_mppi_controller/CMakeLists.txt | 2 +- tools/underlay.repos | 9 +-- 5 files changed, 66 insertions(+), 56 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 09aaa5d31d4..fe39b99429e 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -7,6 +7,7 @@ "cacheFrom": "ghcr.io/ros-planning/navigation2:main" }, "runArgs": [ + "--name=nav2" // "--cap-add=SYS_PTRACE", // enable debugging, e.g. gdb // "--ipc=host", // shared memory transport with host, e.g. rviz GUIs // "--network=host", // network access to host interfaces, e.g. eth0 @@ -16,9 +17,9 @@ ], "workspaceFolder": "/opt/overlay_ws/src/navigation2", "workspaceMount": "source=${localWorkspaceFolder},target=${containerWorkspaceFolder},type=bind", - "onCreateCommand": ".devcontainer/on-create-command.sh", - "updateContentCommand": ".devcontainer/update-content-command.sh", - "postCreateCommand": ".devcontainer/post-create-command.sh", + // "onCreateCommand": ".devcontainer/on-create-command.sh", + // "updateContentCommand": ".devcontainer/update-content-command.sh", + // "postCreateCommand": ".devcontainer/post-create-command.sh", "remoteEnv": { "OVERLAY_MIXINS": "release ccache lld", "CCACHE_DIR": "/tmp/.ccache" @@ -29,11 +30,11 @@ "target": "/tmp/.ccache", "type": "volume" }, - { - "source": "overlay-${devcontainerId}", - "target": "/opt/overlay_ws", - "type": "volume" - } + // { + // "source": "overlay-${devcontainerId}", + // "target": "/opt/overlay_ws", + // "type": "volume" + // } ], "features": { // "ghcr.io/devcontainers/features/desktop-lite:1": {}, diff --git a/Dockerfile b/Dockerfile index 69c89230193..abe33030291 100644 --- a/Dockerfile +++ b/Dockerfile @@ -56,8 +56,9 @@ RUN apt-get update && \ python3-pip \ ros-$ROS_DISTRO-rmw-fastrtps-cpp \ ros-$ROS_DISTRO-rmw-connextdds \ - ros-$ROS_DISTRO-rmw-cyclonedds-cpp \ - && pip3 install \ + ros-$ROS_DISTRO-rmw-cyclonedds-cpp + +RUN pip3 install --break-system-packages \ fastcov \ git+https://github.com/ruffsl/colcon-cache.git@a937541bfc496c7a267db7ee9d6cceca61e470ca \ git+https://github.com/ruffsl/colcon-clean.git@a7f1074d1ebc1a54a6508625b117974f2672f2a9 \ @@ -72,10 +73,12 @@ ENV UNDERLAY_WS $UNDERLAY_WS WORKDIR $UNDERLAY_WS COPY --from=cacher /tmp/$UNDERLAY_WS ./ RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ - apt-get update && rosdep install -q -y \ + apt-get update && rosdep install -r -q -y \ --from-paths src \ --skip-keys " \ slam_toolbox \ + turtlebot3_gazebo \ + gazebo_ros_pkgs \ " \ --ignore-src \ && rm -rf /var/lib/apt/lists/* @@ -97,10 +100,13 @@ ENV OVERLAY_WS $OVERLAY_WS WORKDIR $OVERLAY_WS COPY --from=cacher /tmp/$OVERLAY_WS ./ RUN . $UNDERLAY_WS/install/setup.sh && \ - apt-get update && rosdep install -q -y \ + apt-get update && rosdep install -r -q -y \ --from-paths src \ --skip-keys " \ slam_toolbox \ + turtlebot3_gazebo \ + gazebo_ros_pkgs \ + behaviortree_cpp \ "\ --ignore-src \ && rm -rf /var/lib/apt/lists/* @@ -145,7 +151,7 @@ RUN apt-get update && \ bash-completion \ gdb \ wget && \ - pip3 install \ + pip3 install --break-system-packages \ bottle \ glances @@ -167,42 +173,42 @@ RUN mkdir -p $ROOT_SRV # install demo dependencies RUN apt-get update && apt-get install -y \ - ros-$ROS_DISTRO-aws-robomaker-small-warehouse-world \ - ros-$ROS_DISTRO-rviz2 \ - ros-$ROS_DISTRO-turtlebot3-simulations - -# install gzweb dependacies -RUN apt-get install -y --no-install-recommends \ - imagemagick \ - libboost-all-dev \ - libgazebo-dev \ - libgts-dev \ - libjansson-dev \ - libtinyxml-dev \ - nodejs \ - npm \ - psmisc \ - xvfb - -# clone gzweb -ENV GZWEB_WS /opt/gzweb -RUN git clone https://github.com/osrf/gzweb.git $GZWEB_WS - -# setup gzweb -RUN cd $GZWEB_WS && . /usr/share/gazebo/setup.sh && \ - GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:$(find /opt/ros/$ROS_DISTRO/share \ - -mindepth 1 -maxdepth 2 -type d -name "models" | paste -s -d: -) && \ - sed -i "s|var modelList =|var modelList = []; var oldModelList =|g" gz3d/src/gzgui.js && \ - xvfb-run -s "-screen 0 1280x1024x24" ./deploy.sh -m local && \ - ln -s $GZWEB_WS/http/client/assets http/client/assets/models && \ - ln -s $GZWEB_WS/http/client $ROOT_SRV/gzweb - -# patch gzsever -RUN GZSERVER=$(which gzserver) && \ - mv $GZSERVER $GZSERVER.orig && \ - echo '#!/bin/bash' > $GZSERVER && \ - echo 'exec xvfb-run -s "-screen 0 1280x1024x24" gzserver.orig "$@"' >> $GZSERVER && \ - chmod +x $GZSERVER + # ros-$ROS_DISTRO-aws-robomaker-small-warehouse-world \ + ros-$ROS_DISTRO-rviz2 + # ros-$ROS_DISTRO-turtlebot3-simulations + +# # install gzweb dependacies +# RUN apt-get install -y --no-install-recommends \ +# imagemagick \ +# libboost-all-dev \ +# libgazebo-dev \ +# libgts-dev \ +# libjansson-dev \ +# libtinyxml-dev \ +# nodejs \ +# npm \ +# psmisc \ +# xvfb + +# # clone gzweb +# ENV GZWEB_WS /opt/gzweb +# RUN git clone https://github.com/osrf/gzweb.git $GZWEB_WS + +# # setup gzweb +# RUN cd $GZWEB_WS && . /usr/share/gazebo/setup.sh && \ +# GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:$(find /opt/ros/$ROS_DISTRO/share \ +# -mindepth 1 -maxdepth 2 -type d -name "models" | paste -s -d: -) && \ +# sed -i "s|var modelList =|var modelList = []; var oldModelList =|g" gz3d/src/gzgui.js && \ +# xvfb-run -s "-screen 0 1280x1024x24" ./deploy.sh -m local && \ +# ln -s $GZWEB_WS/http/client/assets http/client/assets/models && \ +# ln -s $GZWEB_WS/http/client $ROOT_SRV/gzweb + +# # patch gzsever +# RUN GZSERVER=$(which gzserver) && \ +# mv $GZSERVER $GZSERVER.orig && \ +# echo '#!/bin/bash' > $GZSERVER && \ +# echo 'exec xvfb-run -s "-screen 0 1280x1024x24" gzserver.orig "$@"' >> $GZSERVER && \ +# chmod +x $GZSERVER # install foxglove dependacies RUN apt-get install -y --no-install-recommends \ diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt index 0a43c136966..fd4208454bb 100644 --- a/nav2_map_server/CMakeLists.txt +++ b/nav2_map_server/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(nav2_util REQUIRED) find_package(GRAPHICSMAGICKCPP REQUIRED) +find_package(yaml-cpp REQUIRED) nav2_package() @@ -118,7 +119,8 @@ target_include_directories(${map_io_library_name} SYSTEM PRIVATE ${GRAPHICSMAGICKCPP_INCLUDE_DIRS}) target_link_libraries(${map_io_library_name} - ${GRAPHICSMAGICKCPP_LIBRARIES}) + ${GRAPHICSMAGICKCPP_LIBRARIES} + yaml-cpp::yaml-cpp) if(WIN32) target_compile_definitions(${map_io_library_name} PRIVATE @@ -160,5 +162,5 @@ ament_export_libraries( ${library_name} ${map_io_library_name} ) -ament_export_dependencies(${map_io_dependencies} ${map_server_dependencies}) +ament_export_dependencies(${map_io_dependencies} ${map_server_dependencies} yaml-cpp) ament_package() diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 95eabf6c411..1cd42cf4320 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -65,7 +65,7 @@ if(COMPILER_SUPPORTS_FMA) endif() # If building one the same hardware to be deployed on, try `-march=native`! -add_compile_options(-O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic) +add_compile_options(-O3 -ffp-contract=fast -ffast-math -mtune=generic) add_library(mppi_controller SHARED src/controller.cpp diff --git a/tools/underlay.repos b/tools/underlay.repos index dd12e220027..31df9e5c836 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -1,8 +1,9 @@ repositories: - # BehaviorTree/BehaviorTree.CPP: - # type: git - # url: https://github.com/BehaviorTree/BehaviorTree.CPP.git - # version: master + BehaviorTree/BehaviorTree.CPP: + # Remove when 4.6.0 is released + type: git + url: https://github.com/BehaviorTree/BehaviorTree.CPP.git + version: 4.6.0 # ros/angles: # type: git # url: https://github.com/ros/angles.git From 6ce218711da126536365601259caf87e0e64b4a8 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Sat, 4 May 2024 21:18:27 +0000 Subject: [PATCH 02/39] fix --- .devcontainer/devcontainer.json | 2 +- Dockerfile | 71 +++++++++++++++----------------- nav2_system_tests/CMakeLists.txt | 4 +- nav2_system_tests/package.xml | 4 +- tools/underlay.repos | 4 -- 5 files changed, 39 insertions(+), 46 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index fe39b99429e..57be80106a5 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -3,7 +3,7 @@ "build": { "dockerfile": "../Dockerfile", "context": "..", - "target": "visualizer", + "target": "dever", "cacheFrom": "ghcr.io/ros-planning/navigation2:main" }, "runArgs": [ diff --git a/Dockerfile b/Dockerfile index abe33030291..354a1acd8be 100644 --- a/Dockerfile +++ b/Dockerfile @@ -56,9 +56,8 @@ RUN apt-get update && \ python3-pip \ ros-$ROS_DISTRO-rmw-fastrtps-cpp \ ros-$ROS_DISTRO-rmw-connextdds \ - ros-$ROS_DISTRO-rmw-cyclonedds-cpp - -RUN pip3 install --break-system-packages \ + ros-$ROS_DISTRO-rmw-cyclonedds-cpp \ + && pip3 install --break-system-packages \ fastcov \ git+https://github.com/ruffsl/colcon-cache.git@a937541bfc496c7a267db7ee9d6cceca61e470ca \ git+https://github.com/ruffsl/colcon-clean.git@a7f1074d1ebc1a54a6508625b117974f2672f2a9 \ @@ -78,7 +77,6 @@ RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ --skip-keys " \ slam_toolbox \ turtlebot3_gazebo \ - gazebo_ros_pkgs \ " \ --ignore-src \ && rm -rf /var/lib/apt/lists/* @@ -105,7 +103,6 @@ RUN . $UNDERLAY_WS/install/setup.sh && \ --skip-keys " \ slam_toolbox \ turtlebot3_gazebo \ - gazebo_ros_pkgs \ behaviortree_cpp \ "\ --ignore-src \ @@ -177,38 +174,38 @@ RUN apt-get update && apt-get install -y \ ros-$ROS_DISTRO-rviz2 # ros-$ROS_DISTRO-turtlebot3-simulations -# # install gzweb dependacies -# RUN apt-get install -y --no-install-recommends \ -# imagemagick \ -# libboost-all-dev \ -# libgazebo-dev \ -# libgts-dev \ -# libjansson-dev \ -# libtinyxml-dev \ -# nodejs \ -# npm \ -# psmisc \ -# xvfb - -# # clone gzweb -# ENV GZWEB_WS /opt/gzweb -# RUN git clone https://github.com/osrf/gzweb.git $GZWEB_WS - -# # setup gzweb -# RUN cd $GZWEB_WS && . /usr/share/gazebo/setup.sh && \ -# GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:$(find /opt/ros/$ROS_DISTRO/share \ -# -mindepth 1 -maxdepth 2 -type d -name "models" | paste -s -d: -) && \ -# sed -i "s|var modelList =|var modelList = []; var oldModelList =|g" gz3d/src/gzgui.js && \ -# xvfb-run -s "-screen 0 1280x1024x24" ./deploy.sh -m local && \ -# ln -s $GZWEB_WS/http/client/assets http/client/assets/models && \ -# ln -s $GZWEB_WS/http/client $ROOT_SRV/gzweb - -# # patch gzsever -# RUN GZSERVER=$(which gzserver) && \ -# mv $GZSERVER $GZSERVER.orig && \ -# echo '#!/bin/bash' > $GZSERVER && \ -# echo 'exec xvfb-run -s "-screen 0 1280x1024x24" gzserver.orig "$@"' >> $GZSERVER && \ -# chmod +x $GZSERVER +# install gzweb dependacies +RUN apt-get install -y --no-install-recommends \ + imagemagick \ + libboost-all-dev \ + libgazebo-dev \ + libgts-dev \ + libjansson-dev \ + libtinyxml-dev \ + nodejs \ + npm \ + psmisc \ + xvfb + +# clone gzweb +ENV GZWEB_WS /opt/gzweb +RUN git clone https://github.com/osrf/gzweb.git $GZWEB_WS + +# setup gzweb +RUN cd $GZWEB_WS && . /usr/share/gazebo/setup.sh && \ + GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:$(find /opt/ros/$ROS_DISTRO/share \ + -mindepth 1 -maxdepth 2 -type d -name "models" | paste -s -d: -) && \ + sed -i "s|var modelList =|var modelList = []; var oldModelList =|g" gz3d/src/gzgui.js && \ + xvfb-run -s "-screen 0 1280x1024x24" ./deploy.sh -m local && \ + ln -s $GZWEB_WS/http/client/assets http/client/assets/models && \ + ln -s $GZWEB_WS/http/client $ROOT_SRV/gzweb + +# patch gzsever +RUN GZSERVER=$(which gzserver) && \ + mv $GZSERVER $GZSERVER.orig && \ + echo '#!/bin/bash' > $GZSERVER && \ + echo 'exec xvfb-run -s "-screen 0 1280x1024x24" gzserver.orig "$@"' >> $GZSERVER && \ + chmod +x $GZSERVER # install foxglove dependacies RUN apt-get install -y --no-install-recommends \ diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index bfab0a79fed..c0c16c17f35 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -14,7 +14,7 @@ find_package(visualization_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) -find_package(gazebo_ros_pkgs REQUIRED) +# find_package(gazebo_ros_pkgs REQUIRED) find_package(nav2_amcl REQUIRED) find_package(nav2_lifecycle_manager REQUIRED) find_package(rclpy REQUIRED) @@ -37,7 +37,7 @@ set(dependencies nav2_amcl nav2_lifecycle_manager nav2_behavior_tree - gazebo_ros_pkgs +# gazebo_ros_pkgs geometry_msgs std_msgs tf2_geometry_msgs diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 049b2b2181d..83d179b35ac 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -26,7 +26,7 @@ geometry_msgs std_msgs tf2_geometry_msgs - gazebo_ros_pkgs + launch_ros launch_testing nav2_planner @@ -48,7 +48,7 @@ nav2_amcl std_msgs tf2_geometry_msgs - gazebo_ros_pkgs + navigation2 lcov robot_state_publisher diff --git a/tools/underlay.repos b/tools/underlay.repos index 31df9e5c836..7e7ae572194 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -36,10 +36,6 @@ repositories: # type: git # url: https://github.com/cra-ros-pkg/robot_localization.git # version: ros2 - # ros-simulation/gazebo_ros_pkgs: - # type: git - # url: https://github.com/ros-simulation/gazebo_ros_pkgs.git - # version: ros2 # ros2/geometry2: # type: git # url: https://github.com/ros2/geometry2.git From fc3c5fee296cce44e6f2089899c7a5d59157687e Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Sat, 4 May 2024 21:33:00 +0000 Subject: [PATCH 03/39] . --- .devcontainer/devcontainer.json | 16 ++++++++-------- Dockerfile | 10 +++++----- nav2_system_tests/package.xml | 4 ++-- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 57be80106a5..e3d34c3b125 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -17,9 +17,9 @@ ], "workspaceFolder": "/opt/overlay_ws/src/navigation2", "workspaceMount": "source=${localWorkspaceFolder},target=${containerWorkspaceFolder},type=bind", - // "onCreateCommand": ".devcontainer/on-create-command.sh", - // "updateContentCommand": ".devcontainer/update-content-command.sh", - // "postCreateCommand": ".devcontainer/post-create-command.sh", + "onCreateCommand": ".devcontainer/on-create-command.sh", + "updateContentCommand": ".devcontainer/update-content-command.sh", + "postCreateCommand": ".devcontainer/post-create-command.sh", "remoteEnv": { "OVERLAY_MIXINS": "release ccache lld", "CCACHE_DIR": "/tmp/.ccache" @@ -30,11 +30,11 @@ "target": "/tmp/.ccache", "type": "volume" }, - // { - // "source": "overlay-${devcontainerId}", - // "target": "/opt/overlay_ws", - // "type": "volume" - // } + { + "source": "overlay-${devcontainerId}", + "target": "/opt/overlay_ws", + "type": "volume" + } ], "features": { // "ghcr.io/devcontainers/features/desktop-lite:1": {}, diff --git a/Dockerfile b/Dockerfile index 354a1acd8be..290dff603d3 100644 --- a/Dockerfile +++ b/Dockerfile @@ -72,11 +72,10 @@ ENV UNDERLAY_WS $UNDERLAY_WS WORKDIR $UNDERLAY_WS COPY --from=cacher /tmp/$UNDERLAY_WS ./ RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ - apt-get update && rosdep install -r -q -y \ + apt-get update && rosdep install -q -y \ --from-paths src \ --skip-keys " \ slam_toolbox \ - turtlebot3_gazebo \ " \ --ignore-src \ && rm -rf /var/lib/apt/lists/* @@ -98,11 +97,12 @@ ENV OVERLAY_WS $OVERLAY_WS WORKDIR $OVERLAY_WS COPY --from=cacher /tmp/$OVERLAY_WS ./ RUN . $UNDERLAY_WS/install/setup.sh && \ - apt-get update && rosdep install -r -q -y \ + apt-get update && rosdep install -q -y \ --from-paths src \ --skip-keys " \ slam_toolbox \ turtlebot3_gazebo \ + gazebo_ros_pkgs \ behaviortree_cpp \ "\ --ignore-src \ @@ -170,9 +170,9 @@ RUN mkdir -p $ROOT_SRV # install demo dependencies RUN apt-get update && apt-get install -y \ - # ros-$ROS_DISTRO-aws-robomaker-small-warehouse-world \ + ros-$ROS_DISTRO-aws-robomaker-small-warehouse-world \ ros-$ROS_DISTRO-rviz2 - # ros-$ROS_DISTRO-turtlebot3-simulations + ros-$ROS_DISTRO-turtlebot3-simulations # install gzweb dependacies RUN apt-get install -y --no-install-recommends \ diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 83d179b35ac..049b2b2181d 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -26,7 +26,7 @@ geometry_msgs std_msgs tf2_geometry_msgs - + gazebo_ros_pkgs launch_ros launch_testing nav2_planner @@ -48,7 +48,7 @@ nav2_amcl std_msgs tf2_geometry_msgs - + gazebo_ros_pkgs navigation2 lcov robot_state_publisher From 5045bd7b81991417f99e2044a2e3fceeb9ec8c4c Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Sat, 4 May 2024 21:33:41 +0000 Subject: [PATCH 04/39] . --- Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Dockerfile b/Dockerfile index 290dff603d3..9f27fff01e8 100644 --- a/Dockerfile +++ b/Dockerfile @@ -171,7 +171,7 @@ RUN mkdir -p $ROOT_SRV # install demo dependencies RUN apt-get update && apt-get install -y \ ros-$ROS_DISTRO-aws-robomaker-small-warehouse-world \ - ros-$ROS_DISTRO-rviz2 + ros-$ROS_DISTRO-rviz2 \ ros-$ROS_DISTRO-turtlebot3-simulations # install gzweb dependacies From 40976ea85272f03449d150006fcb9c357c156555 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Sat, 4 May 2024 21:37:26 +0000 Subject: [PATCH 05/39] . --- Dockerfile | 1 - nav2_common/cmake/nav2_package.cmake | 2 +- tools/underlay.repos | 9 ++++----- 3 files changed, 5 insertions(+), 7 deletions(-) diff --git a/Dockerfile b/Dockerfile index 9f27fff01e8..265d688d1f9 100644 --- a/Dockerfile +++ b/Dockerfile @@ -103,7 +103,6 @@ RUN . $UNDERLAY_WS/install/setup.sh && \ slam_toolbox \ turtlebot3_gazebo \ gazebo_ros_pkgs \ - behaviortree_cpp \ "\ --ignore-src \ && rm -rf /var/lib/apt/lists/* diff --git a/nav2_common/cmake/nav2_package.cmake b/nav2_common/cmake/nav2_package.cmake index 3f4977193c8..42c1e948526 100644 --- a/nav2_common/cmake/nav2_package.cmake +++ b/nav2_common/cmake/nav2_package.cmake @@ -37,7 +37,7 @@ macro(nav2_package) endif() if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC -Wshadow -Wnull-dereference) + add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC -Wnull-dereference) add_compile_options("$<$:-Wnon-virtual-dtor>") endif() diff --git a/tools/underlay.repos b/tools/underlay.repos index 7e7ae572194..a0f6a83ae27 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -1,9 +1,8 @@ repositories: - BehaviorTree/BehaviorTree.CPP: - # Remove when 4.6.0 is released - type: git - url: https://github.com/BehaviorTree/BehaviorTree.CPP.git - version: 4.6.0 + # BehaviorTree/BehaviorTree.CPP: + # type: git + # url: https://github.com/BehaviorTree/BehaviorTree.CPP.git + # version: master # ros/angles: # type: git # url: https://github.com/ros/angles.git From a58249455d5fce61d0b02588e89e88f059ecf4ed Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 6 May 2024 18:40:30 +0000 Subject: [PATCH 06/39] fix --- nav2_common/cmake/nav2_package.cmake | 2 +- nav2_system_tests/CMakeLists.txt | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/nav2_common/cmake/nav2_package.cmake b/nav2_common/cmake/nav2_package.cmake index 42c1e948526..c1d7367b4be 100644 --- a/nav2_common/cmake/nav2_package.cmake +++ b/nav2_common/cmake/nav2_package.cmake @@ -37,7 +37,7 @@ macro(nav2_package) endif() if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC -Wnull-dereference) + add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC) add_compile_options("$<$:-Wnon-virtual-dtor>") endif() diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index c0c16c17f35..1d4b1246538 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -1,6 +1,10 @@ cmake_minimum_required(VERSION 3.5) project(nav2_system_tests) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 23) +endif() + find_package(ament_cmake REQUIRED) find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) From 56372f0c630f8c81a269e6f187f46e5edc1a225b Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 6 May 2024 20:41:03 +0000 Subject: [PATCH 07/39] pr comments --- .circleci/config.yml | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index e26e29f6866..11bff0aed00 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -96,11 +96,6 @@ _commands: (echo vcs_export && cat) >> lockfile.txt sha256sum $PWD/lockfile.txt >> lockfile.txt - # Temp: Rolling transition to 24.04 leaves rosdep in 22.04 in the lurch - # This replaces main rosdep index with the last prior to release: Feb 28, 2024 - sed -i "s|ros\/rosdistro\/master|ros\/rosdistro\/rolling\/2024-02-28|" /etc/ros/rosdep/sources.list.d/20-default.list - export ROSDISTRO_INDEX_URL=https://raw.githubusercontent.com/ros/rosdistro/rolling/2024-02-28/index-v4.yaml - apt-get update rosdep update --rosdistro $ROS_DISTRO dependencies=$( @@ -109,6 +104,8 @@ _commands: --ignore-src \ --skip-keys " \ slam_toolbox \ + turtlebot3_gazebo \ + gazebo_ros_pkgs \ " \ --verbose | \ awk '$1 ~ /^resolution\:/' | \ From 481b7775f3675262e12c156d6cded8273d3b1312 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 6 May 2024 21:14:01 +0000 Subject: [PATCH 08/39] bust cache --- .circleci/config.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 11bff0aed00..22f1347c570 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v21\ + - "<< parameters.key >>-v22\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v21\ + - "<< parameters.key >>-v22\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v21\ + key: "<< parameters.key >>-v22\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ From 17b434ebd0aa49aac9cb7b903f8830a3a98a21fe Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 6 May 2024 21:19:15 +0000 Subject: [PATCH 09/39] test with docker image --- .circleci/config.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 22f1347c570..4329da68bd6 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -484,7 +484,7 @@ _environments: executors: release_exec: docker: - - image: ghcr.io/ros-planning/navigation2:main + - image: ros:rolling resource_class: large working_directory: /opt/overlay_ws environment: From 6699cb5522c08b7f72a155e4ae88863a94136ec7 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 6 May 2024 21:20:32 +0000 Subject: [PATCH 10/39] Revert "test with docker image" This reverts commit 17b434ebd0aa49aac9cb7b903f8830a3a98a21fe. --- .circleci/config.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 4329da68bd6..22f1347c570 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -484,7 +484,7 @@ _environments: executors: release_exec: docker: - - image: ros:rolling + - image: ghcr.io/ros-planning/navigation2:main resource_class: large working_directory: /opt/overlay_ws environment: From ffdd5154498e2e6d3505005601350377194797a6 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 15 May 2024 19:32:56 +0000 Subject: [PATCH 11/39] fixes --- nav2_behavior_tree/CMakeLists.txt | 3 +++ nav2_common/cmake/nav2_package.cmake | 2 +- nav2_system_tests/CMakeLists.txt | 3 +++ nav2_system_tests/package.xml | 4 ++-- 4 files changed, 9 insertions(+), 3 deletions(-) diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 91fc49b71d0..0836f66c081 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -20,6 +20,9 @@ find_package(nav2_util REQUIRED) nav2_package() +# TODO: Delete after https://github.com/BehaviorTree/BehaviorTree.CPP/issues/811 is released +add_compile_options(-Wno-shadow) + include_directories( include ) diff --git a/nav2_common/cmake/nav2_package.cmake b/nav2_common/cmake/nav2_package.cmake index c1d7367b4be..3f4977193c8 100644 --- a/nav2_common/cmake/nav2_package.cmake +++ b/nav2_common/cmake/nav2_package.cmake @@ -37,7 +37,7 @@ macro(nav2_package) endif() if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC) + add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC -Wshadow -Wnull-dereference) add_compile_options("$<$:-Wnon-virtual-dtor>") endif() diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 1d4b1246538..364fbcbc803 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -31,6 +31,9 @@ find_package(pluginlib REQUIRED) nav2_package() +# TODO: Remove after workaround for https://gcc.gnu.org/bugzilla/show_bug.cgi?id=105580 is found +add_compile_options(-Wno-null-dereference) + set(dependencies rclcpp nav2_util diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 049b2b2181d..83d179b35ac 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -26,7 +26,7 @@ geometry_msgs std_msgs tf2_geometry_msgs - gazebo_ros_pkgs + launch_ros launch_testing nav2_planner @@ -48,7 +48,7 @@ nav2_amcl std_msgs tf2_geometry_msgs - gazebo_ros_pkgs + navigation2 lcov robot_state_publisher From e451b1c6da4a1534294dbd7816dea0ab41b83333 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 15 May 2024 21:04:14 +0000 Subject: [PATCH 12/39] fixes --- nav2_mppi_controller/CMakeLists.txt | 9 ++++++--- nav2_system_tests/CMakeLists.txt | 1 - .../src/behavior_tree/test_behavior_tree_node.cpp | 8 ++++---- 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 1cd42cf4320..6274320f9a9 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -64,9 +64,6 @@ if(COMPILER_SUPPORTS_FMA) add_compile_options(-mfma) endif() -# If building one the same hardware to be deployed on, try `-march=native`! -add_compile_options(-O3 -ffp-contract=fast -ffast-math -mtune=generic) - add_library(mppi_controller SHARED src/controller.cpp src/optimizer.cpp @@ -77,6 +74,9 @@ add_library(mppi_controller SHARED src/noise_generator.cpp ) +# If building one the same hardware to be deployed on, try `-march=native`! +target_compile_options(mppi_controller PRIVATE -O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic) + add_library(mppi_critics SHARED src/critics/obstacles_critic.cpp src/critics/cost_critic.cpp @@ -91,6 +91,9 @@ add_library(mppi_critics SHARED src/critics/velocity_deadband_critic.cpp ) +# If building one the same hardware to be deployed on, try `-march=native`! +target_compile_options(mppi_critics PRIVATE -O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic) + set(libraries mppi_controller mppi_critics) foreach(lib IN LISTS libraries) diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 364fbcbc803..37211b390c8 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -32,7 +32,6 @@ find_package(pluginlib REQUIRED) nav2_package() # TODO: Remove after workaround for https://gcc.gnu.org/bugzilla/show_bug.cgi?id=105580 is found -add_compile_options(-Wno-null-dereference) set(dependencies rclcpp diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 0a4b320d8f4..100e5dad936 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -79,10 +79,10 @@ class BehaviorTreeHandler return false; } - auto xml_string = std::string( - std::istreambuf_iterator(xml_file), - std::istreambuf_iterator()); - + std::stringstream buffer; + buffer << xml_file.rdbuf(); + xml_file.close(); + std::string xml_string = buffer.str(); // Create the blackboard that will be shared by all of the nodes in the tree blackboard = BT::Blackboard::create(); From f17374edf87462e257a717d5fb6246ddb56a0b2b Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 15 May 2024 21:04:38 +0000 Subject: [PATCH 13/39] remove comment --- nav2_system_tests/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 37211b390c8..1d4b1246538 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -31,8 +31,6 @@ find_package(pluginlib REQUIRED) nav2_package() -# TODO: Remove after workaround for https://gcc.gnu.org/bugzilla/show_bug.cgi?id=105580 is found - set(dependencies rclcpp nav2_util From abda16785206181426562e7c10334d49ead87945 Mon Sep 17 00:00:00 2001 From: Ruffin Date: Fri, 17 May 2024 20:16:53 +0000 Subject: [PATCH 14/39] Update URI for new GitHub org and GHCR repo --- .circleci/config.yml | 2 +- .devcontainer/devcontainer.json | 2 +- README.md | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 22f1347c570..330566a4c27 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -484,7 +484,7 @@ _environments: executors: release_exec: docker: - - image: ghcr.io/ros-planning/navigation2:main + - image: ghcr.io/ros-navigation/navigation2:main resource_class: large working_directory: /opt/overlay_ws environment: diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index e3d34c3b125..77a326648db 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -4,7 +4,7 @@ "dockerfile": "../Dockerfile", "context": "..", "target": "dever", - "cacheFrom": "ghcr.io/ros-planning/navigation2:main" + "cacheFrom": "ghcr.io/ros-navigation/navigation2:main" }, "runArgs": [ "--name=nav2" diff --git a/README.md b/README.md index ab34c01ce3d..49ce011c96f 100644 --- a/README.md +++ b/README.md @@ -15,7 +15,7 @@ For detailed instructions on how to: - [Configure](https://docs.nav2.org/configuration/index.html) - [Navigation Plugins](https://docs.nav2.org/plugins/index.html) - [Migration Guides](https://docs.nav2.org/migration/index.html) -- [Container Images for Building Nav2](https://github.com/orgs/ros-planning/packages/container/package/navigation2) +- [Container Images for Building Nav2](https://github.com/orgs/ros-navigation/packages/container/package/navigation2) - [Contribute](https://docs.nav2.org/development_guides/involvement_docs/index.html) Please visit our [documentation site](https://docs.nav2.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-hu52lnnq-cKYjuhTY~sEMbZXL8p9tOw) (if this link does not work, please contact maintainers to reactivate). From 1c5a90cb7d18d0f6f1980f3708bef4a2a900311c Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Sat, 18 May 2024 21:17:53 +0200 Subject: [PATCH 15/39] replace fix --- nav2_mppi_controller/CMakeLists.txt | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 6274320f9a9..1cef5aaeaff 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -64,6 +64,9 @@ if(COMPILER_SUPPORTS_FMA) add_compile_options(-mfma) endif() +# If building one the same hardware to be deployed on, try `-march=native`! +add_compile_options(-O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic -Wno-error=array-bounds) + add_library(mppi_controller SHARED src/controller.cpp src/optimizer.cpp @@ -74,9 +77,6 @@ add_library(mppi_controller SHARED src/noise_generator.cpp ) -# If building one the same hardware to be deployed on, try `-march=native`! -target_compile_options(mppi_controller PRIVATE -O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic) - add_library(mppi_critics SHARED src/critics/obstacles_critic.cpp src/critics/cost_critic.cpp @@ -91,9 +91,6 @@ add_library(mppi_critics SHARED src/critics/velocity_deadband_critic.cpp ) -# If building one the same hardware to be deployed on, try `-march=native`! -target_compile_options(mppi_critics PRIVATE -O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic) - set(libraries mppi_controller mppi_critics) foreach(lib IN LISTS libraries) From 10bade2b20993c387e0fa3dfa4945498a70de015 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Sat, 18 May 2024 21:21:13 +0200 Subject: [PATCH 16/39] replace fix --- nav2_mppi_controller/CMakeLists.txt | 2 +- nav2_mppi_controller/test/utils_test.cpp | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 1cef5aaeaff..95eabf6c411 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -65,7 +65,7 @@ if(COMPILER_SUPPORTS_FMA) endif() # If building one the same hardware to be deployed on, try `-march=native`! -add_compile_options(-O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic -Wno-error=array-bounds) +add_compile_options(-O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic) add_library(mppi_controller SHARED src/controller.cpp diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 054cbbe4c0e..3603d4f3cf3 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -15,7 +15,11 @@ #include #include +#pragma GCC diagnostic ignored "-Warray-bounds" +#pragma GCC diagnostic ignored "-Wstringop-overflow" #include +#pragma GCC diagnostic pop + #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "nav2_mppi_controller/tools/utils.hpp" From fd719ff1779f7017be46bf6b0a2b6c63eb6ae3bc Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Sat, 18 May 2024 21:32:03 +0200 Subject: [PATCH 17/39] ignore maybe uninitialized error --- .../include/nav2_mppi_controller/tools/utils.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index fae3328f066..ed303157a85 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -27,6 +27,7 @@ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Warray-bounds" #pragma GCC diagnostic ignored "-Wstringop-overflow" +#pragma GCC diagnostic ignored "-Wmaybe-uninitialized" #include #include #include From 75a628231498edf584faff4c58cb7de064e5b123 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 21 May 2024 17:38:37 +0200 Subject: [PATCH 18/39] inline comment --- nav2_behavior_tree/CMakeLists.txt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 0836f66c081..6d43f4e760e 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -20,8 +20,7 @@ find_package(nav2_util REQUIRED) nav2_package() -# TODO: Delete after https://github.com/BehaviorTree/BehaviorTree.CPP/issues/811 is released -add_compile_options(-Wno-shadow) +add_compile_options(-Wno-shadow) # Delete after https://github.com/BehaviorTree/BehaviorTree.CPP/issues/811 is released include_directories( include From b676fc5979eeba58c23b12a4c9ba377e7152a83a Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 21 May 2024 17:43:20 +0200 Subject: [PATCH 19/39] fix linting --- .../include/nav2_behavior_tree/bt_action_node.hpp | 2 +- .../test/test_constrained_smoother.cpp | 4 ++-- nav2_costmap_2d/src/footprint.cpp | 6 ++++-- nav2_costmap_2d/test/integration/costmap_tester.cpp | 3 ++- .../include/nav2_mppi_controller/tools/utils.hpp | 2 +- .../src/critics/obstacles_critic.cpp | 2 +- nav2_navfn_planner/src/navfn.cpp | 12 ++++++------ nav2_smac_planner/test/test_node2d.cpp | 2 +- nav2_smac_planner/test/test_nodehybrid.cpp | 2 +- nav2_smac_planner/test/test_nodelattice.cpp | 2 +- 10 files changed, 20 insertions(+), 17 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 8c9fcb54d25..c6c5b890ae6 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -387,7 +387,7 @@ class BtActionNode : public BT::ActionNodeBase }; send_goal_options.feedback_callback = [this](typename rclcpp_action::ClientGoalHandle::SharedPtr, - const std::shared_ptr feedback) { + const std::shared_ptr feedback) { feedback_ = feedback; emitWakeUpSignal(); }; diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp index 6687b3452da..1ca6461e9a9 100644 --- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp +++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp @@ -404,7 +404,7 @@ class SmootherTest : public ::testing::Test int cusp_i_ = -1; QualityCriterion3 mvmt_smoothness_criterion_ = [this](int i, const Eigen::Vector3d & prev_p, const Eigen::Vector3d & p, - const Eigen::Vector3d & next_p) { + const Eigen::Vector3d & next_p) { Eigen::Vector2d prev_mvmt = p.block<2, 1>(0, 0) - prev_p.block<2, 1>(0, 0); Eigen::Vector2d next_mvmt = next_p.block<2, 1>(0, 0) - p.block<2, 1>(0, 0); if (i == cusp_i_) { @@ -986,7 +986,7 @@ TEST_F(SmootherTest, testingDownsamplingUpsampling) int cusp_i_out = 6; // for upsampled path QualityCriterion3 mvmt_smoothness_criterion_out = [&cusp_i_out](int i, const Eigen::Vector3d & prev_p, const Eigen::Vector3d & p, - const Eigen::Vector3d & next_p) { + const Eigen::Vector3d & next_p) { Eigen::Vector2d prev_mvmt = p.block<2, 1>(0, 0) - prev_p.block<2, 1>(0, 0); Eigen::Vector2d next_mvmt = next_p.block<2, 1>(0, 0) - p.block<2, 1>(0, 0); if (i == cusp_i_out) { diff --git a/nav2_costmap_2d/src/footprint.cpp b/nav2_costmap_2d/src/footprint.cpp index 75de1feb917..319702c5bde 100644 --- a/nav2_costmap_2d/src/footprint.cpp +++ b/nav2_costmap_2d/src/footprint.cpp @@ -194,7 +194,8 @@ bool makeFootprintFromString( RCLCPP_ERROR( rclcpp::get_logger( "nav2_costmap_2d"), - "You must specify at least three points for the robot footprint, reverting to previous footprint."); //NOLINT + "You must specify at least three points for the robot footprint," + " reverting to previous footprint."); return false; } footprint.reserve(vvf.size()); @@ -209,7 +210,8 @@ bool makeFootprintFromString( RCLCPP_ERROR( rclcpp::get_logger( "nav2_costmap_2d"), - "Points in the footprint specification must be pairs of numbers. Found a point with %d numbers.", //NOLINT + "Points in the footprint specification must be pairs of numbers." + " Found a point with %d numbers.", static_cast(vvf[i].size())); return false; } diff --git a/nav2_costmap_2d/test/integration/costmap_tester.cpp b/nav2_costmap_2d/test/integration/costmap_tester.cpp index b64d049ab7d..59676e23596 100644 --- a/nav2_costmap_2d/test/integration/costmap_tester.cpp +++ b/nav2_costmap_2d/test/integration/costmap_tester.cpp @@ -129,7 +129,8 @@ void CostmapTester::compareCells( RCLCPP_ERROR( rclcpp::get_logger( "costmap_tester"), - "Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f", // NOLINT + "Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost:" + " %d, cell distance: %.2f, furthest valid distance: %.2f", x, y, cell_cost, nx, ny, neighbor_cost, expected_lowest_cost, cell_distance, furthest_valid_distance); RCLCPP_ERROR( diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index ed303157a85..ddde6077659 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -474,7 +474,7 @@ inline void savitskyGolayFilter( auto applyFilterOverAxis = [&](xt::xtensor & sequence, - const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void + const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void { unsigned int idx = 0; sequence(idx) = applyFilter( diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 4386ec1dc12..e3e49918d72 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -192,8 +192,8 @@ bool ObstaclesCritic::inCollision(float cost) const bool is_tracking_unknown = costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); + using namespace nav2_costmap_2d; // NOLINT switch (static_cast(cost)) { - using namespace nav2_costmap_2d; // NOLINT case (LETHAL_OBSTACLE): return true; case (INSCRIBED_INFLATED_OBSTACLE): diff --git a/nav2_navfn_planner/src/navfn.cpp b/nav2_navfn_planner/src/navfn.cpp index 2cce713b0f2..d166dbc7b1b 100644 --- a/nav2_navfn_planner/src/navfn.cpp +++ b/nav2_navfn_planner/src/navfn.cpp @@ -326,14 +326,14 @@ int NavFn::getPathLen() {return npath;} // inserting onto the priority blocks #define push_cur(n) {if (n >= 0 && n < ns && !pending[n] && \ - costarr[n] < COST_OBS && curPe < PRIORITYBUFSIZE) \ - {curP[curPe++] = n; pending[n] = true;}} + costarr[n] < COST_OBS && curPe < PRIORITYBUFSIZE) \ + {curP[curPe++] = n; pending[n] = true;}} #define push_next(n) {if (n >= 0 && n < ns && !pending[n] && \ - costarr[n] < COST_OBS && nextPe < PRIORITYBUFSIZE) \ - {nextP[nextPe++] = n; pending[n] = true;}} + costarr[n] < COST_OBS && nextPe < PRIORITYBUFSIZE) \ + {nextP[nextPe++] = n; pending[n] = true;}} #define push_over(n) {if (n >= 0 && n < ns && !pending[n] && \ - costarr[n] < COST_OBS && overPe < PRIORITYBUFSIZE) \ - {overP[overPe++] = n; pending[n] = true;}} + costarr[n] < COST_OBS && overPe < PRIORITYBUFSIZE) \ + {overP[overPe++] = n; pending[n] = true;}} // Set up navigation potential arrays for new propagation diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp index 1326fb02136..7b99858500f 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -145,7 +145,7 @@ TEST(Node2DTest, test_node_2d_neighbors) std::function neighborGetter = [&, this](const uint64_t & index, - nav2_smac_planner::Node2D * & neighbor_rtn) -> bool + nav2_smac_planner::Node2D * & neighbor_rtn) -> bool { return false; }; diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index 856f37c0a11..1db62df2be8 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -364,7 +364,7 @@ TEST(NodeHybridTest, test_node_reeds_neighbors) std::function neighborGetter = [&, this](const uint64_t & index, - nav2_smac_planner::NodeHybrid * & neighbor_rtn) -> bool + nav2_smac_planner::NodeHybrid * & neighbor_rtn) -> bool { // because we don't return a real object return false; diff --git a/nav2_smac_planner/test/test_nodelattice.cpp b/nav2_smac_planner/test/test_nodelattice.cpp index 2355ac9a842..e118388a073 100644 --- a/nav2_smac_planner/test/test_nodelattice.cpp +++ b/nav2_smac_planner/test/test_nodelattice.cpp @@ -306,7 +306,7 @@ TEST(NodeLatticeTest, test_get_neighbors) std::function neighborGetter = [&, this](const uint64_t & index, - nav2_smac_planner::NodeLattice * & neighbor_rtn) -> bool + nav2_smac_planner::NodeLattice * & neighbor_rtn) -> bool { // because we don't return a real object return false; From 7c0a9ac90936dedb9f7e481c3a21332a487cf439 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 21 May 2024 20:20:27 +0200 Subject: [PATCH 20/39] Add comments --- .circleci/config.yml | 1 + Dockerfile | 2 ++ nav2_system_tests/CMakeLists.txt | 4 ++-- nav2_system_tests/package.xml | 4 ++-- 4 files changed, 7 insertions(+), 4 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 330566a4c27..ceae7cf323e 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -82,6 +82,7 @@ _commands: - run: name: Install Dependencies | << parameters.workspace >> working_directory: << parameters.workspace >> + # Remove/Replace turtlebot3_gazebo and gazebo_ros_pkgs from --skip-keys after https://github.com/ros-navigation/navigation2/pull/3634 command: | . << parameters.underlay >>/install/setup.sh AMENT_PREFIX_PATH=$(echo "$AMENT_PREFIX_PATH" | \ diff --git a/Dockerfile b/Dockerfile index 265d688d1f9..97f587aa1d9 100644 --- a/Dockerfile +++ b/Dockerfile @@ -96,6 +96,8 @@ ARG OVERLAY_WS ENV OVERLAY_WS $OVERLAY_WS WORKDIR $OVERLAY_WS COPY --from=cacher /tmp/$OVERLAY_WS ./ + +# Remove/Replace turtlebot3_gazebo and gazebo_ros_pkgs from --skip-keys after https://github.com/ros-navigation/navigation2/pull/3634 RUN . $UNDERLAY_WS/install/setup.sh && \ apt-get update && rosdep install -q -y \ --from-paths src \ diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 1d4b1246538..70f2449cb40 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -18,7 +18,7 @@ find_package(visualization_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) -# find_package(gazebo_ros_pkgs REQUIRED) +# find_package(gazebo_ros_pkgs REQUIRED) Remove/Replace after https://github.com/ros-navigation/navigation2/pull/3634 find_package(nav2_amcl REQUIRED) find_package(nav2_lifecycle_manager REQUIRED) find_package(rclpy REQUIRED) @@ -41,7 +41,7 @@ set(dependencies nav2_amcl nav2_lifecycle_manager nav2_behavior_tree -# gazebo_ros_pkgs +# gazebo_ros_pkgs Remove/Replace after https://github.com/ros-navigation/navigation2/pull/3634 geometry_msgs std_msgs tf2_geometry_msgs diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 83d179b35ac..5fdbc230b73 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -26,7 +26,7 @@ geometry_msgs std_msgs tf2_geometry_msgs - + launch_ros launch_testing nav2_planner @@ -48,7 +48,7 @@ nav2_amcl std_msgs tf2_geometry_msgs - + navigation2 lcov robot_state_publisher From 6fc3344d0353e3488ee4a7739c3ac19fea9857fe Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 21 May 2024 19:34:33 +0000 Subject: [PATCH 21/39] fix bt test --- nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index fa5badf750d..c20708788c1 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -57,7 +57,7 @@ inline BT::NodeStatus GoalUpdater::tick() getInput("input_goal", goal); - callback_group_executor_.spin_some(); + callback_group_executor_.spin_all(std::chrono::milliseconds(50)); if (last_goal_received_.header.stamp != rclcpp::Time(0)) { auto last_goal_received_time = rclcpp::Time(last_goal_received_.header.stamp); From 7034a1dc845dea32406d458a39724525197e06ab Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 21 May 2024 20:25:33 +0000 Subject: [PATCH 22/39] fix nav2 test --- nav2_collision_monitor/test/collision_monitor_node_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 69f8ac21e06..ac6bb712768 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -1502,9 +1502,9 @@ TEST_F(Tester, testCollisionPointsMarkers) ASSERT_TRUE(waitCollisionPointsMarker(500ms)); ASSERT_EQ(collision_points_marker_msg_->markers[0].points.size(), 0u); - publishCmdVel(0.5, 0.2, 0.1); publishScan(0.5, curr_time); ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCollisionPointsMarker(500ms)); ASSERT_NE(collision_points_marker_msg_->markers[0].points.size(), 0u); // Stop Collision Monitor node From 32dfd1cc6aebc43a31df3373dcc04b3bfba4bc42 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 21 May 2024 21:02:58 +0000 Subject: [PATCH 23/39] fix waypoint follower test --- .../test/test_dynamic_parameters.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/nav2_waypoint_follower/test/test_dynamic_parameters.cpp b/nav2_waypoint_follower/test/test_dynamic_parameters.cpp index 345db77d857..82ebf74ec09 100644 --- a/nav2_waypoint_follower/test/test_dynamic_parameters.cpp +++ b/nav2_waypoint_follower/test/test_dynamic_parameters.cpp @@ -42,6 +42,18 @@ class WPShim : public nav2_waypoint_follower::WaypointFollower rclcpp_lifecycle::State state; this->on_activate(state); } + + void deactivate() + { + rclcpp_lifecycle::State state; + this->on_deactivate(state); + } + + void cleanup() + { + rclcpp_lifecycle::State state; + this->on_cleanup(state); + } }; class RclCppFixture @@ -73,4 +85,7 @@ TEST(WPTest, test_dynamic_parameters) EXPECT_EQ(follower->get_parameter("loop_rate").as_int(), 100); EXPECT_EQ(follower->get_parameter("stop_on_failure").as_bool(), false); + follower->deactivate(); + follower->cleanup(); + follower.reset(); } From ebd9fcb45d222c7f07865582a4fd877585f60f51 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 21 May 2024 16:05:11 -0700 Subject: [PATCH 24/39] More improvements towards Jazzy migration --- nav2_behavior_tree/test/test_bt_utils.cpp | 55 +++-------------------- nav2_util/test/test_actions.cpp | 24 +++++++++- 2 files changed, 28 insertions(+), 51 deletions(-) diff --git a/nav2_behavior_tree/test/test_bt_utils.cpp b/nav2_behavior_tree/test/test_bt_utils.cpp index 1c008d6478a..932c2385d0f 100644 --- a/nav2_behavior_tree/test/test_bt_utils.cpp +++ b/nav2_behavior_tree/test/test_bt_utils.cpp @@ -59,13 +59,7 @@ TEST(PointPortTest, test_wrong_syntax) BT::BehaviorTreeFactory factory; factory.registerNodeType>("PointPort"); - auto tree = factory.createTreeFromText(xml_txt); - - geometry_msgs::msg::Point value; - tree.rootNode()->getInput("test", value); - EXPECT_EQ(value.x, 0.0); - EXPECT_EQ(value.y, 0.0); - EXPECT_EQ(value.z, 0.0); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); xml_txt = R"( @@ -75,11 +69,7 @@ TEST(PointPortTest, test_wrong_syntax) )"; - tree = factory.createTreeFromText(xml_txt); - tree.rootNode()->getInput("test", value); - EXPECT_EQ(value.x, 0.0); - EXPECT_EQ(value.y, 0.0); - EXPECT_EQ(value.z, 0.0); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); } TEST(PointPortTest, test_correct_syntax) @@ -115,14 +105,8 @@ TEST(QuaternionPortTest, test_wrong_syntax) BT::BehaviorTreeFactory factory; factory.registerNodeType>("QuaternionPort"); - auto tree = factory.createTreeFromText(xml_txt); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); - geometry_msgs::msg::Quaternion value; - tree.rootNode()->getInput("test", value); - EXPECT_EQ(value.x, 0.0); - EXPECT_EQ(value.y, 0.0); - EXPECT_EQ(value.z, 0.0); - EXPECT_EQ(value.w, 1.0); xml_txt = R"( @@ -132,12 +116,7 @@ TEST(QuaternionPortTest, test_wrong_syntax) )"; - tree = factory.createTreeFromText(xml_txt); - tree.rootNode()->getInput("test", value); - EXPECT_EQ(value.x, 0.0); - EXPECT_EQ(value.y, 0.0); - EXPECT_EQ(value.z, 0.0); - EXPECT_EQ(value.w, 1.0); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); } TEST(QuaternionPortTest, test_correct_syntax) @@ -174,19 +153,7 @@ TEST(PoseStampedPortTest, test_wrong_syntax) BT::BehaviorTreeFactory factory; factory.registerNodeType>("PoseStampedPort"); - auto tree = factory.createTreeFromText(xml_txt); - - geometry_msgs::msg::PoseStamped value; - tree.rootNode()->getInput("test", value); - EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0); - EXPECT_EQ(value.header.frame_id, ""); - EXPECT_EQ(value.pose.position.x, 0.0); - EXPECT_EQ(value.pose.position.y, 0.0); - EXPECT_EQ(value.pose.position.z, 0.0); - EXPECT_EQ(value.pose.orientation.x, 0.0); - EXPECT_EQ(value.pose.orientation.y, 0.0); - EXPECT_EQ(value.pose.orientation.z, 0.0); - EXPECT_EQ(value.pose.orientation.w, 1.0); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); xml_txt = R"( @@ -196,17 +163,7 @@ TEST(PoseStampedPortTest, test_wrong_syntax) )"; - tree = factory.createTreeFromText(xml_txt); - tree.rootNode()->getInput("test", value); - EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0); - EXPECT_EQ(value.header.frame_id, ""); - EXPECT_EQ(value.pose.position.x, 0.0); - EXPECT_EQ(value.pose.position.y, 0.0); - EXPECT_EQ(value.pose.position.z, 0.0); - EXPECT_EQ(value.pose.orientation.x, 0.0); - EXPECT_EQ(value.pose.orientation.y, 0.0); - EXPECT_EQ(value.pose.orientation.z, 0.0); - EXPECT_EQ(value.pose.orientation.w, 1.0); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); } TEST(PoseStampedPortTest, test_correct_syntax) diff --git a/nav2_util/test/test_actions.cpp b/nav2_util/test/test_actions.cpp index 3af965d4c68..2d551489e74 100644 --- a/nav2_util/test/test_actions.cpp +++ b/nav2_util/test/test_actions.cpp @@ -81,6 +81,10 @@ class FibonacciServerNode : public rclcpp::Node // assert(action_server_->is_cancel_requested() == false); // auto feedback = std::make_shared(); // action_server_->publish_feedback(feedback); + omit_preempt_subs_.reset(); + activate_subs_.reset(); + deactivate_subs_.reset(); + action_server_->deactivate(); action_server_.reset(); } @@ -150,21 +154,33 @@ class RclCppFixture std::make_shared(std::bind(&RclCppFixture::server_thread_func, this)); } + void TearDown() + { + stop_.store(true); + if (server_thread_ && server_thread_->joinable()) { + server_thread_->join(); + server_thread_.reset(); + } + } + ~RclCppFixture() { - server_thread_->join(); } void server_thread_func() { auto node = std::make_shared(); node->on_init(); - rclcpp::spin(node->get_node_base_interface()); + while (rclcpp::ok() && !stop_.load()) { + rclcpp::spin_some(node->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } node->on_term(); node.reset(); } std::shared_ptr server_thread_; + std::atomic stop_{false}; }; RclCppFixture g_rclcppfixture; @@ -189,6 +205,9 @@ class ActionTestNode : public rclcpp::Node void on_term() { + omit_prempt_pub_.reset(); + activate_pub_.reset(); + deactivate_pub_.reset(); action_client_.reset(); } @@ -547,6 +566,7 @@ int main(int argc, char ** argv) g_rclcppfixture.Setup(); ::testing::InitGoogleTest(&argc, argv); auto result = RUN_ALL_TESTS(); + g_rclcppfixture.TearDown(); rclcpp::shutdown(); return result; } From 2b48c372fe31d8f6c36f6f66d4df5c1da6ce5509 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 21 May 2024 17:13:20 -0700 Subject: [PATCH 25/39] fix more tests --- .../test/plugins/condition/test_distance_traveled.cpp | 2 +- .../test/plugins/condition/test_time_expired.cpp | 1 + nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp | 1 + .../test/plugins/decorator/test_rate_controller.cpp | 1 + nav2_util/test/test_actions.cpp | 3 +++ 5 files changed, 7 insertions(+), 1 deletion(-) diff --git a/nav2_behavior_tree/test/plugins/condition/test_distance_traveled.cpp b/nav2_behavior_tree/test/plugins/condition/test_distance_traveled.cpp index 180e2d879e4..6fa4dfc111c 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_distance_traveled.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_distance_traveled.cpp @@ -29,6 +29,7 @@ class DistanceTraveledConditionTestFixture : public nav2_behavior_tree::Behavior { config_->input_ports["global_frame"] = "map"; config_->input_ports["robot_base_frame"] = "base_link"; + config_->input_ports["distance"] = 1.0; bt_node_ = std::make_shared( "distance_traveled", *config_); } @@ -89,6 +90,5 @@ int main(int argc, char ** argv) // shutdown ROS rclcpp::shutdown(); - return all_successful; } diff --git a/nav2_behavior_tree/test/plugins/condition/test_time_expired.cpp b/nav2_behavior_tree/test/plugins/condition/test_time_expired.cpp index 0ec8c224e0f..4f5b8929cff 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_time_expired.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_time_expired.cpp @@ -32,6 +32,7 @@ class TimeExpiredConditionTestFixture : public nav2_behavior_tree::BehaviorTreeT public: void SetUp() { + config_->input_ports["seconds"] = 1.0; bt_node_ = std::make_shared( "time_expired", *config_); } diff --git a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp index 1ca41e798e8..82927031429 100644 --- a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp @@ -58,6 +58,7 @@ class RecoveryNodeTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixtu public: void SetUp() override { + config_->input_ports["number_of_retries"] = 1; bt_node_ = std::make_shared( "recovery_node", *config_); first_child_ = std::make_shared(); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_rate_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_rate_controller.cpp index f7095357c30..df9847e90fc 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_rate_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_rate_controller.cpp @@ -29,6 +29,7 @@ class RateControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFix public: void SetUp() { + config_->input_ports["hz"] = 10.0; bt_node_ = std::make_shared( "rate_controller", *config_); dummy_node_ = std::make_shared(); diff --git a/nav2_util/test/test_actions.cpp b/nav2_util/test/test_actions.cpp index 2d551489e74..5ecd95d60b9 100644 --- a/nav2_util/test/test_actions.cpp +++ b/nav2_util/test/test_actions.cpp @@ -84,6 +84,9 @@ class FibonacciServerNode : public rclcpp::Node omit_preempt_subs_.reset(); activate_subs_.reset(); deactivate_subs_.reset(); + while (action_server_->is_running()) { + std::this_thread::sleep_for(10ms); + } action_server_->deactivate(); action_server_.reset(); } From d7b3c83d390a136a77b27058a0f2d419813d5908 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 21 May 2024 17:30:27 -0700 Subject: [PATCH 26/39] do a few more --- .../test/plugins/condition/test_initial_pose_received.cpp | 1 + .../test/plugins/decorator/test_distance_controller.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp b/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp index 7b8a67529ed..ad25521f5b8 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp @@ -26,6 +26,7 @@ class InitialPoseReceivedConditionTestFixture : public nav2_behavior_tree::Behav public: void SetUp() { + config_->input_ports["initial_pose_received"] = false; bt_node_ = std::make_shared("TestNode", *config_); } diff --git a/nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp index dadbec3cd4a..0fdf84b9086 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp @@ -30,6 +30,7 @@ class DistanceControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTes public: void SetUp() { + config_->input_ports["distance"] = 1.0; config_->input_ports["global_frame"] = "map"; config_->input_ports["robot_base_frame"] = "base_link"; bt_node_ = std::make_shared( From 974e051888a4abf198c57ea1ec9d74e00dbbe6ba Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 21 May 2024 17:39:49 -0700 Subject: [PATCH 27/39] last for the day --- .../test/plugins/condition/test_path_expiring_timer.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp b/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp index 2f47ab826ad..4c22702c331 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp @@ -33,6 +33,9 @@ class PathExpiringTimerConditionTestFixture : public nav2_behavior_tree::Behavio { node_ = std::make_shared("test_path_expiring_condition"); config_ = new BT::NodeConfiguration(); + config_->input_ports["seconds"] = 1.0; + config_->input_ports["path"] = ""; + config_->blackboard = BT::Blackboard::create(); config_->blackboard->set("node", node_); bt_node_ = std::make_shared( From f77277f16a210f6b2f7c184b3a46502314804125 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 22 May 2024 11:05:59 -0700 Subject: [PATCH 28/39] fix remaining BT tests --- .../test/plugins/condition/test_globally_updated_goal.cpp | 2 ++ .../test/plugins/condition/test_goal_updated.cpp | 2 ++ .../test/plugins/condition/test_path_expiring_timer.cpp | 2 ++ .../plugins/decorator/test_goal_updated_controller.cpp | 3 ++- .../test/plugins/decorator/test_speed_controller.cpp | 7 +++++++ 5 files changed, 15 insertions(+), 1 deletion(-) diff --git a/nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp b/nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp index ead9dd21480..97301ed1004 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp @@ -27,6 +27,8 @@ class GloballyUpdatedGoalConditionTestFixture : public nav2_behavior_tree::Behav public: void SetUp() { + config_->input_ports["goals"] = ""; + config_->input_ports["goal"] = ""; bt_node_ = std::make_shared( "globally_updated_goal", *config_); } diff --git a/nav2_behavior_tree/test/plugins/condition/test_goal_updated.cpp b/nav2_behavior_tree/test/plugins/condition/test_goal_updated.cpp index 592ff5042a9..f8a856b2370 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_goal_updated.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_goal_updated.cpp @@ -28,6 +28,8 @@ class GoalUpdatedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeT public: void SetUp() { + config_->input_ports["goals"] = ""; + config_->input_ports["goal"] = ""; bt_node_ = std::make_shared( "goal_updated", *config_); } diff --git a/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp b/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp index 4c22702c331..92834a15f28 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp @@ -18,10 +18,12 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/path.hpp" #include "nav2_util/robot_utils.hpp" #include "utils/test_behavior_tree_fixture.hpp" #include "nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" using namespace std::chrono; // NOLINT using namespace std::chrono_literals; // NOLINT diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp index b33e9e3a8b2..2e5d34ed4f6 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp @@ -36,7 +36,8 @@ class GoalUpdatedControllerTestFixture : public nav2_behavior_tree::BehaviorTree poses1.push_back(goal1); config_->blackboard->set("goal", goal1); config_->blackboard->set("goals", poses1); - + config_->input_ports["goals"] = ""; + config_->input_ports["goal"] = ""; bt_node_ = std::make_shared( "goal_updated_controller", *config_); dummy_node_ = std::make_shared(); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp index 26113b26993..903b6385ed3 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp @@ -45,6 +45,13 @@ class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFi std::vector fake_poses; config_->blackboard->set("goals", fake_poses); // NOLINT + config_->input_ports["min_rate"] = 0.1; + config_->input_ports["max_rate"] = 1.0; + config_->input_ports["min_speed"] = 0.0; + config_->input_ports["max_speed"] = 0.5; + config_->input_ports["goals"] = ""; + config_->input_ports["goal"] = ""; + bt_node_ = std::make_shared("speed_controller", *config_); dummy_node_ = std::make_shared(); bt_node_->setChild(dummy_node_.get()); From 19940fb752d9fbb2833f33849ebd8304fec8730c Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 22 May 2024 14:43:13 -0700 Subject: [PATCH 29/39] fixing last test issues --- .../include/nav2_smac_planner/node_hybrid.hpp | 10 ++++++++++ nav2_smac_planner/src/smac_planner_hybrid.cpp | 1 + nav2_smac_planner/src/smac_planner_lattice.cpp | 1 + nav2_smac_planner/test/test_a_star.cpp | 3 +++ nav2_smac_planner/test/test_nodehybrid.cpp | 1 + nav2_smac_planner/test/test_smoother.cpp | 1 + 6 files changed, 17 insertions(+) diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index b1e265e139b..9dec3ffa7ef 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -463,6 +463,16 @@ class NodeHybrid */ bool backtracePath(CoordinateVector & path); + /** + * @brief Destroy shared pointer assets at the end of the process that don't + * require normal destruction handling + */ + static void destroyStaticAssets() + { + inflation_layer.reset(); + costmap_ros.reset(); + } + NodeHybrid * parent; Coordinates pose; diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 0e0c8829222..36201b26de4 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -319,6 +319,7 @@ void SmacPlannerHybrid::cleanup() RCLCPP_INFO( _logger, "Cleaning up plugin %s of type SmacPlannerHybrid", _name.c_str()); + nav2_smac_planner::NodeHybrid::destroyStaticAssets(); _a_star.reset(); _smoother.reset(); if (_costmap_downsampler) { diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 4f2dba45e43..10605094e45 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -258,6 +258,7 @@ void SmacPlannerLattice::cleanup() RCLCPP_INFO( _logger, "Cleaning up plugin %s of type SmacPlannerLattice", _name.c_str()); + nav2_smac_planner::NodeHybrid::destroyStaticAssets(); _a_star.reset(); _smoother.reset(); _raw_plan_publisher.reset(); diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index bdcf37c7215..3ec5156f23a 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -215,6 +215,7 @@ TEST(AStarTest, test_a_star_se2) EXPECT_GT(expansions->size(), 5u); delete costmapA; + nav2_smac_planner::NodeHybrid::destroyStaticAssets(); } TEST(AStarTest, test_a_star_lattice) @@ -290,6 +291,7 @@ TEST(AStarTest, test_a_star_lattice) } delete costmapA; + nav2_smac_planner::NodeHybrid::destroyStaticAssets(); } TEST(AStarTest, test_se2_single_pose_path) @@ -348,6 +350,7 @@ TEST(AStarTest, test_se2_single_pose_path) EXPECT_GE(path.size(), 1u); delete costmapA; + nav2_smac_planner::NodeHybrid::destroyStaticAssets(); } TEST(AStarTest, test_constants) diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index 1db62df2be8..fb17dad5201 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -236,6 +236,7 @@ TEST(NodeHybridTest, test_obstacle_heuristic) EXPECT_EQ(wide_passage_cost, two_passages_cost); delete costmapA; + nav2_smac_planner::NodeHybrid::destroyStaticAssets(); } TEST(NodeHybridTest, test_node_debin_neighbors) diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp index 616c674595a..3ef261f8a51 100644 --- a/nav2_smac_planner/test/test_smoother.cpp +++ b/nav2_smac_planner/test/test_smoother.cpp @@ -189,4 +189,5 @@ TEST(SmootherTest, test_full_smoother) EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.w, 0.0, 1e-3); delete costmap; + nav2_smac_planner::NodeHybrid::destroyStaticAssets(); } From 4bc062c471cd1d0be875fe7704a3f51a5f5dc1c6 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 23 May 2024 19:25:41 +0000 Subject: [PATCH 30/39] change linting image --- .github/workflows/lint.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index 8732b239d9b..c181fe25f4c 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -7,7 +7,7 @@ jobs: name: ament_${{ matrix.linter }} runs-on: ubuntu-latest container: - image: rostooling/setup-ros-docker:ubuntu-jammy-ros-rolling-ros-base-latest + image: rostooling/setup-ros-docker:ubuntu-noble-ros-rolling-ros-base-latest strategy: fail-fast: false matrix: From f671d38f73f543cb5c5adc6b0ff24b32c0ccc227 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 23 May 2024 19:25:50 +0000 Subject: [PATCH 31/39] comment out tests needing gazebo --- nav2_system_tests/CMakeLists.txt | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 70f2449cb40..45a8097b355 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -113,18 +113,19 @@ if(BUILD_TESTING) add_subdirectory(src/behavior_tree) add_subdirectory(src/planning) - add_subdirectory(src/localization) - add_subdirectory(src/system) - add_subdirectory(src/system_failure) - add_subdirectory(src/updown) - add_subdirectory(src/waypoint_follower) - add_subdirectory(src/gps_navigation) - add_subdirectory(src/behaviors/spin) - add_subdirectory(src/behaviors/wait) - add_subdirectory(src/behaviors/backup) - add_subdirectory(src/behaviors/drive_on_heading) - add_subdirectory(src/behaviors/assisted_teleop) - add_subdirectory(src/costmap_filters) + # Uncomment after https://github.com/ros-navigation/navigation2/pull/3634 + # add_subdirectory(src/localization) + # add_subdirectory(src/system) + # add_subdirectory(src/system_failure) + # add_subdirectory(src/updown) + # add_subdirectory(src/waypoint_follower) + # add_subdirectory(src/gps_navigation) + # add_subdirectory(src/behaviors/spin) + # add_subdirectory(src/behaviors/wait) + # add_subdirectory(src/behaviors/backup) + # add_subdirectory(src/behaviors/drive_on_heading) + # add_subdirectory(src/behaviors/assisted_teleop) + # add_subdirectory(src/costmap_filters) add_subdirectory(src/error_codes) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) From c3ecef906706d5a8df496925905bd4b31b4997a3 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 23 May 2024 20:06:11 +0000 Subject: [PATCH 32/39] break the cache cuz why not --- .circleci/config.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index ceae7cf323e..a212f73dad3 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v22\ + - "<< parameters.key >>-v23\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v22\ + - "<< parameters.key >>-v23\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v22\ + key: "<< parameters.key >>-v23\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ From 93bd849e18c3bf5f029dce54c3a514aa394b6fba Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 23 May 2024 20:55:15 +0000 Subject: [PATCH 33/39] fix dynamic param test --- .../test/integration/dyn_params_tests.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/nav2_costmap_2d/test/integration/dyn_params_tests.cpp b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp index 4d0a3bef54a..8ee6d9c210b 100644 --- a/nav2_costmap_2d/test/integration/dyn_params_tests.cpp +++ b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp @@ -38,15 +38,14 @@ class DynParamTestNode TEST(DynParamTestNode, testDynParamsSet) { - auto node = std::make_shared("dyn_param_tester"); auto costmap = std::make_shared("test_costmap"); costmap->on_configure(rclcpp_lifecycle::State()); // Set tf between default global_frame and robot_base_frame in order not to block in on_activate std::unique_ptr tf_broadcaster_ = - std::make_unique(node); + std::make_unique(costmap); geometry_msgs::msg::TransformStamped t; - t.header.stamp = node->get_clock()->now(); + t.header.stamp = costmap->get_clock()->now(); t.header.frame_id = "map"; t.child_frame_id = "base_link"; tf_broadcaster_->sendTransform(t); @@ -57,9 +56,10 @@ TEST(DynParamTestNode, testDynParamsSet) costmap->on_activate(rclcpp_lifecycle::State()); auto parameter_client = std::make_shared( - node->shared_from_this(), - "/test_costmap/test_costmap", - rmw_qos_profile_parameters); + costmap->get_node_base_interface(), costmap->get_node_topics_interface(), + costmap->get_node_graph_interface(), + costmap->get_node_services_interface()); + auto results1 = parameter_client->set_parameters_atomically( { rclcpp::Parameter("robot_radius", 1.234), From ffb9ea003bc7185fbba48f398c574d32859f127a Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 23 May 2024 20:56:51 +0000 Subject: [PATCH 34/39] revert changes to test_bt_utils --- nav2_behavior_tree/test/test_bt_utils.cpp | 55 ++++++++++++++++++++--- 1 file changed, 49 insertions(+), 6 deletions(-) diff --git a/nav2_behavior_tree/test/test_bt_utils.cpp b/nav2_behavior_tree/test/test_bt_utils.cpp index 932c2385d0f..1c008d6478a 100644 --- a/nav2_behavior_tree/test/test_bt_utils.cpp +++ b/nav2_behavior_tree/test/test_bt_utils.cpp @@ -59,7 +59,13 @@ TEST(PointPortTest, test_wrong_syntax) BT::BehaviorTreeFactory factory; factory.registerNodeType>("PointPort"); - EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); + auto tree = factory.createTreeFromText(xml_txt); + + geometry_msgs::msg::Point value; + tree.rootNode()->getInput("test", value); + EXPECT_EQ(value.x, 0.0); + EXPECT_EQ(value.y, 0.0); + EXPECT_EQ(value.z, 0.0); xml_txt = R"( @@ -69,7 +75,11 @@ TEST(PointPortTest, test_wrong_syntax) )"; - EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); + tree = factory.createTreeFromText(xml_txt); + tree.rootNode()->getInput("test", value); + EXPECT_EQ(value.x, 0.0); + EXPECT_EQ(value.y, 0.0); + EXPECT_EQ(value.z, 0.0); } TEST(PointPortTest, test_correct_syntax) @@ -105,8 +115,14 @@ TEST(QuaternionPortTest, test_wrong_syntax) BT::BehaviorTreeFactory factory; factory.registerNodeType>("QuaternionPort"); - EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); + auto tree = factory.createTreeFromText(xml_txt); + geometry_msgs::msg::Quaternion value; + tree.rootNode()->getInput("test", value); + EXPECT_EQ(value.x, 0.0); + EXPECT_EQ(value.y, 0.0); + EXPECT_EQ(value.z, 0.0); + EXPECT_EQ(value.w, 1.0); xml_txt = R"( @@ -116,7 +132,12 @@ TEST(QuaternionPortTest, test_wrong_syntax) )"; - EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); + tree = factory.createTreeFromText(xml_txt); + tree.rootNode()->getInput("test", value); + EXPECT_EQ(value.x, 0.0); + EXPECT_EQ(value.y, 0.0); + EXPECT_EQ(value.z, 0.0); + EXPECT_EQ(value.w, 1.0); } TEST(QuaternionPortTest, test_correct_syntax) @@ -153,7 +174,19 @@ TEST(PoseStampedPortTest, test_wrong_syntax) BT::BehaviorTreeFactory factory; factory.registerNodeType>("PoseStampedPort"); - EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); + auto tree = factory.createTreeFromText(xml_txt); + + geometry_msgs::msg::PoseStamped value; + tree.rootNode()->getInput("test", value); + EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0); + EXPECT_EQ(value.header.frame_id, ""); + EXPECT_EQ(value.pose.position.x, 0.0); + EXPECT_EQ(value.pose.position.y, 0.0); + EXPECT_EQ(value.pose.position.z, 0.0); + EXPECT_EQ(value.pose.orientation.x, 0.0); + EXPECT_EQ(value.pose.orientation.y, 0.0); + EXPECT_EQ(value.pose.orientation.z, 0.0); + EXPECT_EQ(value.pose.orientation.w, 1.0); xml_txt = R"( @@ -163,7 +196,17 @@ TEST(PoseStampedPortTest, test_wrong_syntax) )"; - EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); + tree = factory.createTreeFromText(xml_txt); + tree.rootNode()->getInput("test", value); + EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0); + EXPECT_EQ(value.header.frame_id, ""); + EXPECT_EQ(value.pose.position.x, 0.0); + EXPECT_EQ(value.pose.position.y, 0.0); + EXPECT_EQ(value.pose.position.z, 0.0); + EXPECT_EQ(value.pose.orientation.x, 0.0); + EXPECT_EQ(value.pose.orientation.y, 0.0); + EXPECT_EQ(value.pose.orientation.z, 0.0); + EXPECT_EQ(value.pose.orientation.w, 1.0); } TEST(PoseStampedPortTest, test_correct_syntax) From 12588f55bddd1c699ae7da7a41689282e076b983 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 23 May 2024 22:04:08 +0000 Subject: [PATCH 35/39] fix dyn param --- nav2_costmap_2d/test/integration/dyn_params_tests.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/nav2_costmap_2d/test/integration/dyn_params_tests.cpp b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp index 8ee6d9c210b..2c954a0940a 100644 --- a/nav2_costmap_2d/test/integration/dyn_params_tests.cpp +++ b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp @@ -38,14 +38,15 @@ class DynParamTestNode TEST(DynParamTestNode, testDynParamsSet) { + auto node = std::make_shared("dyn_param_tester"); auto costmap = std::make_shared("test_costmap"); costmap->on_configure(rclcpp_lifecycle::State()); // Set tf between default global_frame and robot_base_frame in order not to block in on_activate std::unique_ptr tf_broadcaster_ = - std::make_unique(costmap); + std::make_unique(node); geometry_msgs::msg::TransformStamped t; - t.header.stamp = costmap->get_clock()->now(); + t.header.stamp = node->get_clock()->now(); t.header.frame_id = "map"; t.child_frame_id = "base_link"; tf_broadcaster_->sendTransform(t); @@ -83,6 +84,7 @@ TEST(DynParamTestNode, testDynParamsSet) rclcpp::Parameter("robot_base_frame", "wrong_test_frame"), }); + rclcpp::spin_some(node->get_node_base_interface()); rclcpp::spin_some(costmap->get_node_base_interface()); EXPECT_EQ(costmap->get_parameter("robot_radius").as_double(), 1.234); From 80d523847a16efee63653717e663aa4f2e22f7d9 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 28 May 2024 12:25:38 +0200 Subject: [PATCH 36/39] comment out turtlebot3_gazebo --- nav2_bringup/package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 7080f580edb..56c8602eebf 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -19,7 +19,8 @@ navigation2 nav2_common slam_toolbox - turtlebot3_gazebo + + ament_lint_common ament_lint_auto From fdd066078599b11aa941aa70080d82d9554e676c Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 28 May 2024 19:00:08 +0000 Subject: [PATCH 37/39] test with spin_all --- nav2_costmap_2d/test/integration/dyn_params_tests.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_costmap_2d/test/integration/dyn_params_tests.cpp b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp index 2c954a0940a..3bb257c2c6b 100644 --- a/nav2_costmap_2d/test/integration/dyn_params_tests.cpp +++ b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp @@ -84,8 +84,8 @@ TEST(DynParamTestNode, testDynParamsSet) rclcpp::Parameter("robot_base_frame", "wrong_test_frame"), }); - rclcpp::spin_some(node->get_node_base_interface()); - rclcpp::spin_some(costmap->get_node_base_interface()); + rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(50)); + rclcpp::spin_all(costmap->get_node_base_interface(), std::chrono::milliseconds(50)); EXPECT_EQ(costmap->get_parameter("robot_radius").as_double(), 1.234); EXPECT_EQ(costmap->get_parameter("footprint_padding").as_double(), 2.345); From ffb85841efd5ee691434f89ae9afd05c251b7e0d Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 28 May 2024 14:53:21 -0700 Subject: [PATCH 38/39] Update Dockerfile Signed-off-by: Steve Macenski --- Dockerfile | 1 - 1 file changed, 1 deletion(-) diff --git a/Dockerfile b/Dockerfile index 97f587aa1d9..1f1060ccd70 100644 --- a/Dockerfile +++ b/Dockerfile @@ -104,7 +104,6 @@ RUN . $UNDERLAY_WS/install/setup.sh && \ --skip-keys " \ slam_toolbox \ turtlebot3_gazebo \ - gazebo_ros_pkgs \ "\ --ignore-src \ && rm -rf /var/lib/apt/lists/* From 8246c783458ed658f28065d50f34ef2acc735ea6 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 28 May 2024 14:53:26 -0700 Subject: [PATCH 39/39] Update .circleci/config.yml Signed-off-by: Steve Macenski --- .circleci/config.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index a212f73dad3..1d0c08e87df 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -106,7 +106,6 @@ _commands: --skip-keys " \ slam_toolbox \ turtlebot3_gazebo \ - gazebo_ros_pkgs \ " \ --verbose | \ awk '$1 ~ /^resolution\:/' | \