diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 923c99ec..d36e19a2 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,16 +1,30 @@ name: CI on: push: + branches: + - develop + - master + - "release/*" pull_request: + types: [opened, synchronize, reopened] jobs: + determine_docker_org_and_tag: + runs-on: ubuntu-latest + outputs: + docker_organization: ${{ steps.docker-org-and-tag.outputs.docker_organization }} + docker_image_tag: ${{ steps.docker-org-and-tag.outputs.docker_image_tag }} + steps: + - id: docker-org-and-tag + uses: usdot-fhwa-stol/actions/docker-org-and-tag@main build: + needs: determine_docker_org_and_tag defaults: run: shell: bash working-directory: "/opt/carma/" runs-on: ubuntu-latest container: - image: usdotfhwastol/carma-base:carma-system-4.5.0 + image: ${{ needs.determine_docker_org_and_tag.outputs.docker_organization }}/carma-base:${{ needs.determine_docker_org_and_tag.outputs.docker_image_tag }} env: INIT_ENV: "/home/carma/.base-image/init-env.sh" ROS_2_ENV: "/opt/ros/foxy/setup.bash" @@ -18,16 +32,24 @@ jobs: options: "--user root" steps: - name: Checkout ${{ github.event.repository.name }} - uses: actions/checkout@v3.3.0 + uses: actions/checkout@v4 with: fetch-depth: 0 path: src/${{ github.event.repository.name }} - name: Move source code run: mv $GITHUB_WORKSPACE/src /opt/carma/ + - name: Determine base branch + id: determine-base-branch + run: | + if [[ "$GITHUB_EVENT_NAME" == "pull_request" ]]; then + echo git_branch="$GITHUB_BASE_REF" >> $GITHUB_OUTPUT + else + echo git_branch="$GITHUB_REF_NAME" >> $GITHUB_OUTPUT + fi - name: Checkout dependencies run: | source "$INIT_ENV" - git clone -b carma-system-4.5.0 --depth 1 https://github.com/usdot-fhwa-stol/carma-msgs.git src/CARMAMsgs + git clone -b ${{ steps.determine-base-branch.outputs.git_branch }} --depth 1 https://github.com/usdot-fhwa-stol/carma-msgs.git src/CARMAMsgs - name: Build ROS1 run: | source "$INIT_ENV" diff --git a/bsm_helper/include/bsm_helper/bsm_helper.h b/bsm_helper/include/bsm_helper/bsm_helper.h index 5e2a87cc..2ecb03dd 100644 --- a/bsm_helper/include/bsm_helper/bsm_helper.h +++ b/bsm_helper/include/bsm_helper/bsm_helper.h @@ -12,7 +12,7 @@ distributed under the License is distributed on an "AS IS" BASIS, WITHOUT #pragma once #include #include -#include +#include #include #include #include diff --git a/carma_ros2_utils/CMakeLists.txt b/carma_ros2_utils/CMakeLists.txt index 69f6ecda..a004c155 100644 --- a/carma_ros2_utils/CMakeLists.txt +++ b/carma_ros2_utils/CMakeLists.txt @@ -1,25 +1,60 @@ +# Copyright 2024 Leidos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + cmake_minimum_required(VERSION 3.5) project(carma_ros2_utils) find_package(carma_cmake_common REQUIRED) carma_check_ros_version(2) -carma_package() -find_package(ament_cmake_auto REQUIRED) -find_package(ament_cmake_python REQUIRED) -ament_auto_find_build_dependencies() +include(dependencies.cmake) + +carma_package() +# TODO(CAR-6017): Remove when we drop support for Foxy +# rclcpp_lifecycle has breaking API changes starting with Humble +if(("$ENV{ROS_DISTRO}" STREQUAL "foxy") OR ("$ENV{ROS_DISTRO}" STREQUAL "galactic")) + set(carma_ros2_utils_LIFECYCLE_PUBLISHER_INTERFACE_TYPE + rclcpp_lifecycle::LifecyclePublisherInterface + ) +else() + set(carma_ros2_utils_LIFECYCLE_PUBLISHER_INTERFACE_TYPE + rclcpp_lifecycle::ManagedEntityInterface + ) +endif() -# Includes -include_directories( - include +configure_file( + src/carma_lifecycle_node.hpp.in + carma_ros2_utils/carma_lifecycle_node.hpp ) -# Build -ament_auto_add_library(${PROJECT_NAME} SHARED +ament_auto_add_library(carma_ros2_utils SHARED src/carma_lifecycle_node.cpp ) +target_include_directories(carma_ros2_utils + PUBLIC + $ +) + +include(GNUInstallDirs) + +INSTALL(FILES ${PROJECT_BINARY_DIR}/carma_ros2_utils/carma_lifecycle_node.hpp + DESTINATION + ${CMAKE_INSTALL_INCLUDEDIR}/carma_ros2_utils +) + ament_auto_add_library(carma_ros2_utils_timers SHARED src/timers/testing/TestTimer.cpp src/timers/testing/TestTimerFactory.cpp @@ -52,42 +87,72 @@ ament_auto_add_executable(carma_component_container_mt src/component_container_mt.cpp ) -# Link local targets - -target_link_libraries(carma_ros2_utils_timers - ${PROJECT_NAME} -) - -target_link_libraries(lifecycle_component_wrapper - ${PROJECT_NAME} -) - -target_link_libraries(lifecycle_component_wrapper_st - lifecycle_component_wrapper - ${PROJECT_NAME} -) - -target_link_libraries(lifecycle_component_wrapper_mt - lifecycle_component_wrapper - ${PROJECT_NAME} -) - -target_link_libraries(carma_component_container - carma_component_manager -) - -target_link_libraries(carma_component_container_mt - carma_component_manager -) - if(BUILD_TESTING) - - add_subdirectory(test) + # These CMake commands were added to ament_cmake_auto in ROS 2 Humble. Until + # CARMA supports ROS 2 Humble, we will use package-local copies. + include(cmake/ament_auto_find_test_dependencies.cmake) + include(cmake/ament_auto_add_gtest.cmake) + + ament_auto_add_executable(test_carma_lifecycle_node + test/test_node.cpp + ) + + ament_auto_add_library(test_minimal_node SHARED + test/test_minimal_node.cpp + ) + + add_launch_test( + test/launch_test_lifecycle_wrapper.py + ) + + ament_add_gtest_executable(test_lifecycle_gtest + test/test_lifecycle_manager.cpp + ) + + ament_target_dependencies(test_lifecycle_gtest + ros2_lifecycle_manager + ) + + target_link_libraries(test_lifecycle_gtest + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ) + + ament_add_test(test_lifecycle + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test/launch_lifecycle_test.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 30 + ENV + TEST_EXECUTABLE=$ + ) + + ament_auto_add_gtest(test_carma_lifecycle_gtest + test/test_carma_lifecycle_exceptions.cpp + test/test_carma_lifecycle_helpers.cpp + ) + + ament_auto_add_gtest(test_timers + test/timers/TestingTimers.cpp + ) + + ament_auto_add_gtest(test_containers + test/containers/containers_test.cpp + ) + + install( + TARGETS test_carma_lifecycle_node test_minimal_node + # EXPORT should not be needed for unit tests + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib/carma_ros2_utils + RUNTIME DESTINATION lib/carma_ros2_utils # In Ament, Executables are installed to lib/${PROJECT_NAME} not bin + INCLUDES DESTINATION include + ) rclcpp_components_register_nodes(test_minimal_node "carma_ros2_utils_testing::MinimalNode") endif() -ament_python_install_package(${PROJECT_NAME}) +ament_python_install_package(carma_ros2_utils) ament_auto_package() diff --git a/carma_ros2_utils/cmake/ament_auto_add_gtest.cmake b/carma_ros2_utils/cmake/ament_auto_add_gtest.cmake new file mode 100644 index 00000000..f723f9c9 --- /dev/null +++ b/carma_ros2_utils/cmake/ament_auto_add_gtest.cmake @@ -0,0 +1,112 @@ +# Copyright 2021 Whitley Software Services, LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# +# Add a gtest with all found test dependencies. +# +# Call add_executable(target ARGN), link it against the gtest libraries +# and all found test dependencies, and then register the executable as a test. +# +# If gtest is not available the specified target is not being created and +# therefore the target existence should be checked before being used. +# +# :param target: the target name which will also be used as the test name +# :type target: string +# :param ARGN: the list of source files +# :type ARGN: list of strings +# :param RUNNER: the path to the test runner script (default: +# see ament_add_test). +# :type RUNNER: string +# :param TIMEOUT: the test timeout in seconds, +# default defined by ``ament_add_test()`` +# :type TIMEOUT: integer +# :param WORKING_DIRECTORY: the working directory for invoking the +# executable in, default defined by ``ament_add_test()`` +# :type WORKING_DIRECTORY: string +# :param SKIP_LINKING_MAIN_LIBRARIES: if set skip linking against the gtest +# main libraries +# :type SKIP_LINKING_MAIN_LIBRARIES: option +# :param SKIP_TEST: if set mark the test as being skipped +# :type SKIP_TEST: option +# :param ENV: list of env vars to set; listed as ``VAR=value`` +# :type ENV: list of strings +# :param APPEND_ENV: list of env vars to append if already set, otherwise set; +# listed as ``VAR=value`` +# :type APPEND_ENV: list of strings +# :param APPEND_LIBRARY_DIRS: list of library dirs to append to the appropriate +# OS specific env var, a la LD_LIBRARY_PATH +# :type APPEND_LIBRARY_DIRS: list of strings +# +# @public +# +macro(ament_auto_add_gtest target) + cmake_parse_arguments(_ARG + "SKIP_LINKING_MAIN_LIBRARIES;SKIP_TEST" + "RUNNER;TIMEOUT;WORKING_DIRECTORY" + "APPEND_ENV;APPEND_LIBRARY_DIRS;ENV" + ${ARGN}) + if(NOT _ARG_UNPARSED_ARGUMENTS) + message(FATAL_ERROR + "ament_auto_add_gtest() must be invoked with at least one source file") + endif() + + # add executable + set(_arg_executable ${_ARG_UNPARSED_ARGUMENTS}) + if(_ARG_SKIP_LINKING_MAIN_LIBRARIES) + list(APPEND _arg_executable "SKIP_LINKING_MAIN_LIBRARIES") + endif() + ament_add_gtest_executable("${target}" ${_arg_executable}) + + # add include directory of this package if it exists + if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/include") + target_include_directories("${target}" PUBLIC + "${CMAKE_CURRENT_SOURCE_DIR}/include") + endif() + + # link against other libraries of this package + if(NOT ${PROJECT_NAME}_LIBRARIES STREQUAL "") + target_link_libraries("${target}" ${${PROJECT_NAME}_LIBRARIES}) + endif() + + # add exported information from found dependencies + ament_target_dependencies(${target} + ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS} + ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} + ) + + # add test + set(_arg_test "") + if(_ARG_RUNNER) + list(APPEND _arg_test "RUNNER" "${_ARG_RUNNER}") + endif() + if(_ARG_TIMEOUT) + list(APPEND _arg_test "TIMEOUT" "${_ARG_TIMEOUT}") + endif() + if(_ARG_WORKING_DIRECTORY) + list(APPEND _arg_test "WORKING_DIRECTORY" "${_ARG_WORKING_DIRECTORY}") + endif() + if(_ARG_SKIP_TEST) + list(APPEND _arg_test "SKIP_TEST") + endif() + if(_ARG_ENV) + list(APPEND _arg_test "ENV" ${_ARG_ENV}) + endif() + if(_ARG_APPEND_ENV) + list(APPEND _arg_test "APPEND_ENV" ${_ARG_APPEND_ENV}) + endif() + if(_ARG_APPEND_LIBRARY_DIRS) + list(APPEND _arg_test "APPEND_LIBRARY_DIRS" ${_ARG_APPEND_LIBRARY_DIRS}) + endif() + ament_add_gtest_test("${target}" ${_arg_test}) +endmacro() diff --git a/carma_ros2_utils/cmake/ament_auto_find_test_dependencies.cmake b/carma_ros2_utils/cmake/ament_auto_find_test_dependencies.cmake new file mode 100644 index 00000000..8320b26e --- /dev/null +++ b/carma_ros2_utils/cmake/ament_auto_find_test_dependencies.cmake @@ -0,0 +1,41 @@ +# Copyright 2021 Whitley Software Services, LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# +# Invoke find_package() for all test dependencies. +# +# All found package names are appended to the +# ``${PROJECT_NAME}_FOUND_TEST_DEPENDS`` variables. +# +# @public +# +macro(ament_auto_find_test_dependencies) + set(_ARGN "${ARGN}") + if(_ARGN) + message(FATAL_ERROR "ament_auto_find_test_dependencies() called with " + "unused arguments: ${_ARGN}") + endif() + + if(NOT _AMENT_PACKAGE_NAME) + ament_package_xml() + endif() + + # try to find_package() all test dependencies + foreach(_dep ${${PROJECT_NAME}_TEST_DEPENDS}) + find_package(${_dep} QUIET) + if(${_dep}_FOUND) + list(APPEND ${PROJECT_NAME}_FOUND_TEST_DEPENDS ${_dep}) + endif() + endforeach() +endmacro() diff --git a/carma_ros2_utils/dependencies.cmake b/carma_ros2_utils/dependencies.cmake new file mode 100644 index 00000000..f4e1264b --- /dev/null +++ b/carma_ros2_utils/dependencies.cmake @@ -0,0 +1,34 @@ +# Copyright 2024 Leidos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # TODO(CAR-6016): Integrate addional tests into package + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_uncrustify + ament_cmake_copyright + ament_cmake_cppcheck + ament_cmake_cpplint + ament_cmake_flake8 + ament_cmake_lint_cmake + ament_cmake_pep257 + ament_cmake_xmllint + ) + + ament_lint_auto_find_test_dependencies() +endif() diff --git a/carma_ros2_utils/include/carma_ros2_utils/timers/ROSTimer.hpp b/carma_ros2_utils/include/carma_ros2_utils/timers/ROSTimer.hpp index 6496a8e4..83242982 100644 --- a/carma_ros2_utils/include/carma_ros2_utils/timers/ROSTimer.hpp +++ b/carma_ros2_utils/include/carma_ros2_utils/timers/ROSTimer.hpp @@ -14,8 +14,9 @@ * License for the specific language governing permissions and limitations under * the License. */ +#include #include -#include "../carma_lifecycle_node.hpp" +#include "carma_ros2_utils/carma_lifecycle_node.hpp" #include #include #include "Timer.hpp" @@ -32,7 +33,7 @@ class ROSTimer : public Timer { rclcpp::TimerBase::SharedPtr timer_; std::weak_ptr weak_node_pointer_; - rclcpp::Duration duration_ = rclcpp::Duration(0); + rclcpp::Duration duration_ = rclcpp::Duration(std::chrono::nanoseconds{0}); bool autostart_=true; std::function callback_; @@ -51,4 +52,4 @@ class ROSTimer : public Timer void stop() override; }; } // namespace timers -} // namespace carma_ros2_utils \ No newline at end of file +} // namespace carma_ros2_utils diff --git a/carma_ros2_utils/include/carma_ros2_utils/timers/ROSTimerFactory.hpp b/carma_ros2_utils/include/carma_ros2_utils/timers/ROSTimerFactory.hpp index bbfe4d4a..fadb6d7e 100644 --- a/carma_ros2_utils/include/carma_ros2_utils/timers/ROSTimerFactory.hpp +++ b/carma_ros2_utils/include/carma_ros2_utils/timers/ROSTimerFactory.hpp @@ -17,7 +17,7 @@ #include #include #include -#include "../carma_lifecycle_node.hpp" +#include "carma_ros2_utils/carma_lifecycle_node.hpp" #include "TimerFactory.hpp" #include "Timer.hpp" #include "ROSTimer.hpp" @@ -38,7 +38,7 @@ class ROSTimerFactory : public TimerFactory * @brief Destructor */ ~ROSTimerFactory(); - + void setCarmaLifecycleNode(std::weak_ptr weak_node_pointer); //// Overrides @@ -51,4 +51,4 @@ class ROSTimerFactory : public TimerFactory std::weak_ptr weak_node_pointer_; }; } // namespace timers -} // namespace carma_ros2_utils \ No newline at end of file +} // namespace carma_ros2_utils diff --git a/carma_ros2_utils/include/carma_ros2_utils/timers/testing/TestTimer.hpp b/carma_ros2_utils/include/carma_ros2_utils/timers/testing/TestTimer.hpp index ff575976..734d1c15 100644 --- a/carma_ros2_utils/include/carma_ros2_utils/timers/testing/TestTimer.hpp +++ b/carma_ros2_utils/include/carma_ros2_utils/timers/testing/TestTimer.hpp @@ -47,7 +47,7 @@ class TestTimer : public Timer std::function callback_; rclcpp::Time start_time_ = rclcpp::Time(0); - rclcpp::Duration duration_ = rclcpp::Duration(0); + rclcpp::Duration duration_ = rclcpp::Duration(std::chrono::nanoseconds{0}); carma_ros2_utils::timers::testing::TestClock::SharedPtr clock_; //! Interface used for accessing current time from rclcpp bool oneshot_ = false; @@ -74,4 +74,4 @@ class TestTimer : public Timer }; } // namespace testing } // namespace timers -} // namespace carma_ros2_utils \ No newline at end of file +} // namespace carma_ros2_utils diff --git a/carma_ros2_utils/src/carma_lifecycle_node.cpp b/carma_ros2_utils/src/carma_lifecycle_node.cpp index 1c3617d1..98d352c3 100644 --- a/carma_ros2_utils/src/carma_lifecycle_node.cpp +++ b/carma_ros2_utils/src/carma_lifecycle_node.cpp @@ -55,7 +55,7 @@ namespace carma_ros2_utils // NOTE: When creating this callback group it was not immediately clear if it should go in on_configure or the constructor // Further usage of callback groups may elucidate this in the future - service_callback_group_ = get_node_base_interface()->create_callback_group(rclcpp::callback_group::CallbackGroupType::Reentrant); + service_callback_group_ = get_node_base_interface()->create_callback_group(rclcpp::CallbackGroupType::Reentrant); } CarmaLifecycleNode::~CarmaLifecycleNode() @@ -204,7 +204,7 @@ namespace carma_ros2_utils rclcpp_lifecycle::State state_at_exception = get_current_state(); - if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) // If the exception was caught in the ACTIVE state we can try to gracefully fail to on_error, by transitioning to deactivate and then throwning an exception + if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) // If the exception was caught in the ACTIVE state we can try to gracefully fail to on_error, by transitioning to deactivate and then throwing an exception { std::string error_msg = "Uncaught Exception from node: " + std::string(get_name()) + " exception: " + e.what() + " while in ACTIVE state."; caught_exception_ = error_msg; diff --git a/carma_ros2_utils/include/carma_ros2_utils/carma_lifecycle_node.hpp b/carma_ros2_utils/src/carma_lifecycle_node.hpp.in similarity index 91% rename from carma_ros2_utils/include/carma_ros2_utils/carma_lifecycle_node.hpp rename to carma_ros2_utils/src/carma_lifecycle_node.hpp.in index 1861f769..664ecaf2 100644 --- a/carma_ros2_utils/include/carma_ros2_utils/carma_lifecycle_node.hpp +++ b/carma_ros2_utils/src/carma_lifecycle_node.hpp.in @@ -14,18 +14,25 @@ * the License. */ +/* + * TODO(CAR-6017): When we drop support for Foxy: + * - replace CMake substitution strings with rclcpp::ManagedEntityInterface + * - drop the .in filename suffix + * - relocate file to include/ directory + */ + /** * This file is loosely based on the reference architecture developed by OSRF for Leidos located here * https://github.com/mjeronimo/carma2/blob/master/carma_utils/carma_utils/include/carma_utils/carma_lifecycle_node.hpp - * + * * That file has the following license and some code snippets from it may be present in this file as well and are under the same license. - * + * * Copyright 2021 Open Source Robotics Foundation, Inc. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at - * + * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software @@ -33,7 +40,7 @@ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. - */ + */ #ifndef CARMA_ROS2_UTILS__CARMA_LIFECYCLE_NODE_HPP_ #define CARMA_ROS2_UTILS__CARMA_LIFECYCLE_NODE_HPP_ @@ -70,17 +77,17 @@ namespace carma_ros2_utils /** * \brief The CarmaLifecycleNode provides default exception handling and SystemAlert handling for components in carma-platform. - * All new nodes in carma-platform are expected to be built off CarmaLifecycleNode and implemented as components. - * + * All new nodes in carma-platform are expected to be built off CarmaLifecycleNode and implemented as components. + * * Every function which uses callbacks (subscribers, publishers, services, timers) is wrapped in a try catch block * In the event an exception makes it to this class it will result in a SystemAlert message being published and the node shutting down. * NOTE: As the subscription/timer/service methods of the base LifeCycle node class are not virtual the override of these methods here - * will not properly handle polymorphism. Therefore try catches must be manually added when passing this object to a method which - * takes a regular node to setup callbacks. - * + * will not properly handle polymorphism. Therefore try catches must be manually added when passing this object to a method which + * takes a regular node to setup callbacks. + * * The user can add their own exception, alert, and shutdown handling using corresponding handler functions: * handle_on_error, handle_on_system_alert, handle_on_shutdown - * + * * This class is thread safe */ class CarmaLifecycleNode : public rclcpp_lifecycle::LifecycleNode @@ -88,7 +95,7 @@ namespace carma_ros2_utils public: /** * \brief Constructor which is used by component loaders - * + * * \param options The configurations options for this node */ CARMA_ROS2_UTILS_PUBLIC @@ -101,7 +108,7 @@ namespace carma_ros2_utils /** * \brief Overrides: See https://github.com/ros2/rclcpp/blob/foxy/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp for their details - * NOTE: These methods are NOT meant to be used by extending classes. Instead the corresponding handle_ methods should be used + * NOTE: These methods are NOT meant to be used by extending classes. Instead the corresponding handle_ methods should be used * to ensure the full CARMALifecycleNode functionality is employed. */ carma_ros2_utils::CallbackReturn on_configure(const rclcpp_lifecycle::State &prev_state) override; @@ -114,20 +121,20 @@ namespace carma_ros2_utils /** * \brief Callback triggered when transitioning from UNCONFIGURED to INACTIVE due to the configure signal. * This method should be overriden to load parameters and setup callbacks - * - * \param state The previous state - * - * \return A callback success flag. If SUCCESS then the state will transition to INACTIVE. + * + * \param state The previous state + * + * \return A callback success flag. If SUCCESS then the state will transition to INACTIVE. * If FAILURE or ERROR occurs the state will transition to ErrorProcessing via the handle_on_error method */ virtual carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &prev_state); /** * \brief Callback triggered when transitioning from INACTIVE to ACTIVE due to the activate signal. * This method should be overriden to set any states or callbacks that should only trigger in the ACTIVE state - * - * \param state The previous state - * - * \return A callback success flag. If SUCCESS then the state will transition to ACTIVE. + * + * \param state The previous state + * + * \return A callback success flag. If SUCCESS then the state will transition to ACTIVE. * If FAILURE or ERROR occurs the state will transition to ErrorProcessing via the handle_on_error method */ virtual carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &prev_state); @@ -135,12 +142,12 @@ namespace carma_ros2_utils * \brief Callback triggered when transitioning from ACTIVE to INACTIVE due to the deactivate signal. * This method should be overriden to clear any runtime state data that would prevent reconfiguring of the node * and or deactivate any timers/callbacks that should only trigger in the ACTIVE state - * - * NOTE: CarmaLifecycleNode will automatically deactivate publishers on this callback. - * - * \param state The previous state - * - * \return A callback success flag. If SUCCESS then the state will transition to INACTIVE. + * + * NOTE: CarmaLifecycleNode will automatically deactivate publishers on this callback. + * + * \param state The previous state + * + * \return A callback success flag. If SUCCESS then the state will transition to INACTIVE. * If FAILURE or ERROR occurs the state will transition to ErrorProcessing via the handle_on_error method */ virtual carma_ros2_utils::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &prev_state); @@ -149,12 +156,12 @@ namespace carma_ros2_utils * \brief Callback triggered when transitioning from INACTIVE to UNCONFIGURED due to the cleanup signal. * This method should be overriden to clear any state and memory allocation that occurred. * When completed the node should be fully unconfigured. - * + * * NOTE: CarmaLifecycleNode will automatically deactivate publishers and clear timer pointers - * - * \param state The previous state - * - * \return A callback success flag. If SUCCESS then the state will transition to UNCONFIGURED. + * + * \param state The previous state + * + * \return A callback success flag. If SUCCESS then the state will transition to UNCONFIGURED. * If FAILURE or ERROR occurs the state will transition to ErrorProcessing via the handle_on_error method */ virtual carma_ros2_utils::CallbackReturn handle_on_cleanup(const rclcpp_lifecycle::State &prev_state); @@ -162,12 +169,12 @@ namespace carma_ros2_utils /** * \brief Callback triggered when transitioning from any state to ErrorProcessing due to the error signal. * This method should be overriden to add any exception handling logic. - * - * NOTE: CarmaLifecycleNode will automatically publish a FATAL SystemAlert before this is triggered. - * - * \param state The previous state - * - * \return A callback success flag. If SUCCESS then the state will transition to UNCONFIGURED. + * + * NOTE: CarmaLifecycleNode will automatically publish a FATAL SystemAlert before this is triggered. + * + * \param state The previous state + * + * \return A callback success flag. If SUCCESS then the state will transition to UNCONFIGURED. * If FAILURE or ERROR occurs the state will transition to FINALIZED and the process will shutdown */ virtual carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &prev_state, const std::string &exception_string); @@ -175,18 +182,18 @@ namespace carma_ros2_utils /** * \brief Callback triggered when transitioning from any state to ShuttingDown due to the error shutdown. * This method should be overriden to add any shutdown logic - * - * - * \param state The previous state - * - * \return A callback success flag. If SUCCESS then the state will transition to FINALIZED. + * + * + * \param state The previous state + * + * \return A callback success flag. If SUCCESS then the state will transition to FINALIZED. * If FAILURE or ERROR occurs the state will transition to FINALIZED and the process will shutdown */ virtual carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &prev_state); /** * \brief Convenience method to build a shared pointer from this object. - * + * * \return A shared pointer which points to this object */ std::shared_ptr shared_from_this(); @@ -201,8 +208,8 @@ namespace carma_ros2_utils /** * \brief Override of parent method. See descriptive comments here: * https://github.com/ros2/rclcpp/blob/4859c4e43576d0c6fe626679b2c2604a9a8b336c/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp#L230 - * - * NOTE: The function object passed to this method will be moved using std::move. + * + * NOTE: The function object passed to this method will be moved using std::move. * The user should therefore assume ownership of this function object has been relinquished */ template < @@ -238,11 +245,11 @@ namespace carma_ros2_utils /** * \brief Override of parent method. See descriptive comments here: * https://github.com/ros2/rclcpp/blob/4859c4e43576d0c6fe626679b2c2604a9a8b336c/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp#L463 - * - * NOTE: The function object passed to this method will be moved using std::move. + * + * NOTE: The function object passed to this method will be moved using std::move. * The user should therefore assume ownership of this function object has been relinquished - * - * NOTE: The returned callback handle is also cached inside CarmaLifecycleNode, so the user need not track it + * + * NOTE: The returned callback handle is also cached inside CarmaLifecycleNode, so the user need not track it * unless they intend to use it */ rclcpp_lifecycle::LifecycleNode::OnSetParametersCallbackHandle::SharedPtr @@ -253,7 +260,7 @@ namespace carma_ros2_utils * \brief Override of parent method. See descriptive comments here: * https://github.com/ros2/rclcpp/blob/4859c4e43576d0c6fe626679b2c2604a9a8b336c/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp#L249 * - * NOTE: The function object passed to this method will be moved using std::move. + * NOTE: The function object passed to this method will be moved using std::move. * The user should therefore assume ownership of this function object has been relinquished */ template @@ -266,19 +273,19 @@ namespace carma_ros2_utils /** * \brief Method to create a timer whose lifecycle can be managed by this node. - * + * * NOTE: In foxy the LifecycleNode api is slightly out of sync with the node api so there is not a create_timer method there. We use rclcpp directly here - * + * * \param clock The underlying clock to use for the timer. * \param period The period of trigger of the timer. * \param callback The callback to execute on timer trigger * \param group The callback group to use for processing the callback. - * + * * \return A pointer to an intialized timer. The timer will be cancled when this node transitions through a deactivate/cleanup sequence * - * NOTE: The function object passed to this method will be moved using std::move. + * NOTE: The function object passed to this method will be moved using std::move. * The user should therefore assume ownership of this function object has been relinquished - * + * */ template typename rclcpp::TimerBase::SharedPtr @@ -291,10 +298,10 @@ namespace carma_ros2_utils /** * \brief Override of rclcpp method. See descriptive comments here: * https://github.com/ros2/rclcpp/blob/4859c4e43576d0c6fe626679b2c2604a9a8b336c/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp#L271 - * + * * NOTE: In foxy the LifecycleNode api is slightly out of sync with the node api so there is not a create_service method there. We use rclcpp directly here * - * NOTE: The function object passed to this method will be moved using std::move. + * NOTE: The function object passed to this method will be moved using std::move. * The user should therefore assume ownership of this function object has been relinquished */ template @@ -308,33 +315,33 @@ namespace carma_ros2_utils /** * \brief Override of parent method. See descriptive comments here: * https://github.com/ros2/rclcpp/blob/d7804e1b3fd9676d302ec72f02c49ba04cbed5e6/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp#L260 - * - * VERY IMPORTANT NOTE: While the callback group default appears to be nullptr this is not actually the case. - * On call the group will be set to the Reentrant CallbackGroup this->service_callback_group_ - * This group is created explicitly for services to support a synchronous callback paradigm similar to ROS1. + * + * VERY IMPORTANT NOTE: While the callback group default appears to be nullptr this is not actually the case. + * On call the group will be set to the Reentrant CallbackGroup this->service_callback_group_ + * This group is created explicitly for services to support a synchronous callback paradigm similar to ROS1. * Care should be taken when changing this default group value. * If the user needs nullptr as the actual input they can call the parent method rclcpp_lifecycle::LifecycleNode::create_client */ template typename rclcpp::Client::SharedPtr - create_client(const std::string service_name, + create_client(const std::string service_name, const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, rclcpp::CallbackGroup::SharedPtr group = nullptr); /** * \brief Helper method for setting parameters based on the input to a parameter callback as used by add_on_set_parameters_callback() * This method will take a map of parameter name and the parameter variable (passed as reference) and set the parameter. - * + * * NOTE: This method is templated and should be called once for each parameter type which needs to be set. - * + * * \tparam T The type of the parameter to set. Must be one of the ROS supported parameter types. If not then this method will return an error. * \param update_targets A map of parameter names and the parameter variable (passed as reference) to set - * \param new_params The list of new parameters that provided by the input to a parameter callback. + * \param new_params The list of new parameters that provided by the input to a parameter callback. * Parameters not listed in both the update_targets and new_params will be ignored. - * - * \return boost::optional A string containing the error description if a parameter could not be set. + * + * \return boost::optional A string containing the error description if a parameter could not be set. * If boost::none then no error occurred - */ + */ template boost::optional update_params(const std::unordered_map>& update_targets, const std::vector< rclcpp::Parameter > & new_params); @@ -342,17 +349,17 @@ namespace carma_ros2_utils /** * \brief Helper method for setting parameters based on the input to a parameter callback as used by add_on_set_parameters_callback() * This method will take a map of parameter name and a setter function which will set the named parameter. - * + * * NOTE: This method is templated and should be called once for each parameter type which needs to be set. - * + * * \tparam T The type of the parameter to set. Must be one of the ROS supported parameter types. If not then this method will return an error. * \param update_targets A map of parameter names and a setter function which will set the named parameter. The setter function should return the old value; - * \param new_params The list of new parameters that provided by the input to a parameter callback. + * \param new_params The list of new parameters that provided by the input to a parameter callback. * Parameters not listed in both the update_targets and new_params will be ignored. - * - * \return boost::optional A string containing the error description if a parameter could not be set. + * + * \return boost::optional A string containing the error description if a parameter could not be set. * If boost::none then no error occurred - */ + */ template boost::optional update_params(const std::unordered_map>& update_targets, const std::vector< rclcpp::Parameter > & new_params); @@ -361,16 +368,16 @@ namespace carma_ros2_utils /** * \brief Helper method to handle exceptions which occurred in primary states. * This is needed as the life cycle diagram described here https://design.ros2.org/articles/node_lifecycle.html - * does not reflect current implementation where error handling is only handled by transition states. + * does not reflect current implementation where error handling is only handled by transition states. * Current ROS2 Issue/PR: https://github.com/ros2/rclcpp/pull/1064 - * + * * \param e The exception to be handled. Defaults to runtime error, thrown in case error isn't specified. */ void handle_primary_state_exception(const std::exception &e = std::runtime_error("Encountered Error")); /** * \brief Helper method to publish a system alert of type FATAL with the provided error string. - * + * * \param alert_string The message description to send. */ void send_error_alert_msg_for_string(const std::string &alert_string); @@ -402,8 +409,12 @@ namespace carma_ros2_utils std::shared_ptr> system_alert_pub_; + /* + * TODO(CAR-6017): Refactor when we transition to Humble. + * LifecyclePublisherInterface was replaced with ManagedEntityInterface in Humble + */ //! A list of lifecycle publishers produced from this node whose lifetimes can be managed - std::vector> lifecycle_publishers_; + std::vector> lifecycle_publishers_; //! A list of timers produced from this node whose lifetimes can be managed std::vector> timers_; @@ -427,6 +438,6 @@ namespace carma_ros2_utils // Template functions cannot be linked unless the implementation is provided // Therefore include implementation to allow for template functions -#include "internal/carma_lifecycle_node.tpp" +#include "carma_ros2_utils/internal/carma_lifecycle_node.tpp" #endif // CARMA_ROS2_UTILS__CARMA_LIFECYCLE_NODE_HPP_" diff --git a/carma_ros2_utils/test/CMakeLists.txt b/carma_ros2_utils/test/CMakeLists.txt deleted file mode 100644 index 63adb384..00000000 --- a/carma_ros2_utils/test/CMakeLists.txt +++ /dev/null @@ -1,113 +0,0 @@ -find_package(ament_cmake_gtest REQUIRED) -find_package(ament_cmake_pytest REQUIRED) -find_package(ros2_lifecycle_manager REQUIRED) -find_package(std_srvs REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(ament_cmake_pytest REQUIRED) -find_package(launch_testing_ament_cmake REQUIRED) - -set(dependencies ${dependencies} ros2_lifecycle_manager std_srvs) - - -add_executable(test_carma_lifecycle_node - test_node.cpp -) - -target_link_libraries(test_carma_lifecycle_node ${PROJECT_NAME}) - -ament_target_dependencies(test_carma_lifecycle_node ${dependencies} ) - -target_include_directories(test_carma_lifecycle_node - PUBLIC - $ - $ -) - -add_library(test_minimal_node SHARED - test_minimal_node.cpp -) - -ament_target_dependencies(test_minimal_node - rclcpp - rclcpp_components -) - -add_launch_test( - launch_test_lifecycle_wrapper.py -) - - -ament_add_gtest_executable(test_lifecycle_gtest - test_lifecycle_manager.cpp -) - -target_include_directories(test_lifecycle_gtest - PUBLIC - $ - $ -) - -ament_target_dependencies(test_lifecycle_gtest - ${dependencies} -) - -ament_add_test(test_lifecycle - GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/launch_lifecycle_test.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - TIMEOUT 30 - ENV - TEST_EXECUTABLE=$ -) - -ament_add_gtest(test_carma_lifecycle_gtest - test_carma_lifecycle_exceptions.cpp - test_carma_lifecycle_helpers.cpp -) - -target_include_directories(test_carma_lifecycle_gtest - PUBLIC - $ - $ -) - -target_link_libraries(test_carma_lifecycle_gtest ${PROJECT_NAME}) - -ament_target_dependencies(test_carma_lifecycle_gtest - ${dependencies} -) - -ament_add_gtest(test_timers - timers/TestingTimers.cpp -) - -target_include_directories(test_timers - PUBLIC - $ - $ -) - -target_link_libraries(test_timers carma_ros2_utils_timers ${PROJECT_NAME}) - -ament_target_dependencies(test_timers - ${dependencies} -) - -ament_add_gtest(test_containers - containers/containers_test.cpp -) - -target_link_libraries(test_containers ${PROJECT_NAME}) - -install( - TARGETS test_lifecycle_gtest test_carma_lifecycle_node test_carma_lifecycle_gtest test_minimal_node test_timers - # EXPORT should not be needed for unit tests - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib/${PROJECT_NAME} - RUNTIME DESTINATION lib/${PROJECT_NAME} # In Ament, Executables are installed to lib/${PROJECT_NAME} not bin - INCLUDES DESTINATION include -) - -# This is a bit of a hack to allow for rclcpp_component registration in this subdirectory -# Based off sloretz's answer from ROS Answers here https://answers.ros.org/question/361289/ros2-components-registration-from-subdirectory-cmakelists-file/ -#set(AMENT_EXTENSIONS_ament_package ${AMENT_EXTENSIONS_ament_package} PARENT_SCOPE) \ No newline at end of file diff --git a/carma_ros2_utils/test/launch_test_lifecycle_wrapper.py b/carma_ros2_utils/test/launch_test_lifecycle_wrapper.py index aa63d183..187739bf 100755 --- a/carma_ros2_utils/test/launch_test_lifecycle_wrapper.py +++ b/carma_ros2_utils/test/launch_test_lifecycle_wrapper.py @@ -48,7 +48,7 @@ def generate_test_description(): namespace="/", exec_name='wrapper_container', composable_node_descriptions=[ - + ComposableNode( package='carma_ros2_utils', plugin='carma_ros2_utils_testing::MinimalNode', @@ -69,10 +69,10 @@ def generate_test_description(): class TestRuntime(unittest.TestCase): def test_nominal_lifecycle(self, proc_output): - + # Check that that the wrapper has started and did not load any nodes on startup proc_output.assertWaitFor("Got request to load node: minimal_node but we are not in the active state, caching for later lifecycle based activation.", process='wrapper_container', strict_proc_matching=True,timeout=5) - + d = dict(os.environ) # Make a copy of the current environment completed_proc = subprocess.run(['ros2', 'lifecycle', 'set', '/wrapper_container', 'configure'], env=d, capture_output=True, timeout=8) @@ -88,7 +88,7 @@ def test_nominal_lifecycle(self, proc_output): proc_output.assertWaitFor("libtest_minimal_node.so", process='wrapper_container', strict_proc_matching=True,timeout=5) proc_output.assertWaitFor("Found class: rclcpp_components::NodeFactoryTemplate", process='wrapper_container', strict_proc_matching=True,timeout=5) proc_output.assertWaitFor("Instantiate class: rclcpp_components::NodeFactoryTemplate", process='wrapper_container', strict_proc_matching=True,timeout=5) - + completed_proc = subprocess.run(['ros2', 'lifecycle', 'set', '/wrapper_container', 'deactivate'], env=d, capture_output=True, timeout=8) output_str = completed_proc.stdout.decode() @@ -113,7 +113,3 @@ def test_exit_code(self, proc_info): # Check that all processes in the launch (in this case, there's just one) exit # with code 0 launch_testing.asserts.assertExitCodes(proc_info, allowable_exit_codes=[0]) - - - - diff --git a/carma_ros2_utils/test/test_carma_lifecycle_exceptions.cpp b/carma_ros2_utils/test/test_carma_lifecycle_exceptions.cpp index e7a7c196..4eca3fb9 100644 --- a/carma_ros2_utils/test/test_carma_lifecycle_exceptions.cpp +++ b/carma_ros2_utils/test/test_carma_lifecycle_exceptions.cpp @@ -31,7 +31,7 @@ namespace test_1 { // This is a test node to support unit tests for the carma_lifecycle_node // NOTE: To make unit testing easier this node by default returns to unconfigured from ErrorProcessing unless an exception occurs in ErrorProcessing - // This is different from the default CarmaLifecycleNode behavior which shuts the node down on exceptions + // This is different from the default CarmaLifecycleNode behavior which shuts the node down on exceptions class CarmaLifecycleNodeTest : public carma_ros2_utils::CarmaLifecycleNode { public: @@ -235,7 +235,7 @@ TEST(CARMALifecycleNode, PrimaryStateErrors) // Evaluate timers auto exception_timer = node->create_timer( node->get_clock(), - rclcpp::Duration(10.0), + rclcpp::Duration(std::chrono::nanoseconds{10}), []() { throw std::runtime_error("Timer exception"); @@ -263,7 +263,7 @@ TEST(CARMALifecycleNode, PrimaryStateErrors) { throw std::runtime_error("Wall Timer exception"); }); - + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, node->get_current_state().id()); // Trigger the callback @@ -337,7 +337,7 @@ TEST(CARMALifecycleNode, PrimaryStateErrors) // Force the service callback to trigger the exception service->handle_request(header, request); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, node->get_current_state().id()); - + } diff --git a/carma_ros2_utils/test/test_lifecycle_manager.cpp b/carma_ros2_utils/test/test_lifecycle_manager.cpp index 1ef8f5ec..086c885a 100644 --- a/carma_ros2_utils/test/test_lifecycle_manager.cpp +++ b/carma_ros2_utils/test/test_lifecycle_manager.cpp @@ -19,100 +19,94 @@ #include #include #include -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "ros2_lifecycle_manager/ros2_lifecycle_manager.hpp" -#include "boost/core/ignore_unused.hpp" +#include +#include +#include +#include using std_msec = std::chrono::milliseconds; /** - * This test is meant to exercise the CARMA Lifecycle Node with the standard flow of the lifecycle manager. - * To allow for callbacks to be processed the test makes use of a rather roundabout thread paradigm + * This test is meant to exercise the CARMA Lifecycle Node with the standard flow of the lifecycle manager. + * To allow for callbacks to be processed the test makes use of a rather roundabout thread paradigm * Care should be taken to understand this code before trying to duplicate it in other test scenarios. * There may be better approaches such as the launch_testing framework. - * - */ + * + */ TEST(LifecycleManagerTest, BasicTest) { // Test constructor auto node = std::make_shared("lifecycle_manager_test_node"); // Node to connect to ROS network - auto ret = rcutils_logging_set_logger_level( - node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); - boost::ignore_unused(ret); - - std::promise promise_test_result; // Promise which will be used to spin the node until the test completes - std::shared_future future_test_result(promise_test_result.get_future()); // Future to check for spin + std::ignore = rcutils_logging_set_logger_level( + node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); - std::thread test_thread([&promise_test_result, node](){ - - ros2_lifecycle_manager::Ros2LifecycleManager lifecycle_mgr_( // Construct lifecycle manager - node->get_node_base_interface(), - node->get_node_graph_interface(), - node->get_node_logging_interface(), + ros2_lifecycle_manager::Ros2LifecycleManager lifecycle_mgr_( // Construct lifecycle manager + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_logging_interface(), node->get_node_services_interface() ); - // Test set_managed_nodes - lifecycle_mgr_.set_managed_nodes( { "test_carma_lifecycle_node_1", "test_carma_lifecycle_node_2" } ); - - std::this_thread::sleep_for(std_msec(2000)); - - // Test get_managed_nodes - auto managed_nodes = lifecycle_mgr_.get_managed_nodes(); - - ASSERT_EQ((uint8_t)2, managed_nodes.size()); - ASSERT_EQ((uint8_t)0, managed_nodes[0].compare("test_carma_lifecycle_node_1")); - ASSERT_EQ((uint8_t)0, managed_nodes[1].compare("test_carma_lifecycle_node_2")); - - // Test get managed node states - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_1")); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_2")); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_mgr_.get_managed_node_state("unknown_node")); - - // Test Configure - ASSERT_TRUE(lifecycle_mgr_.configure(std_msec(2000), std_msec(2000)).empty()); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_1")); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_2")); - - // Test Activate - ASSERT_TRUE(lifecycle_mgr_.activate(std_msec(2000), std_msec(2000)).empty()); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE , lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_1")); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE , lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_2")); - - // Test Deactivate - ASSERT_TRUE(lifecycle_mgr_.deactivate(std_msec(2000), std_msec(2000)).empty()); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_1")); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_2")); - - // Test cleanup - ASSERT_TRUE(lifecycle_mgr_.cleanup(std_msec(2000), std_msec(2000)).empty()); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED , lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_1")); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED , lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_2")); - - // Bring back to active then shutdown - ASSERT_TRUE(lifecycle_mgr_.configure(std_msec(2000), std_msec(2000)).empty()); - ASSERT_TRUE(lifecycle_mgr_.activate(std_msec(2000), std_msec(2000)).empty()); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE , lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_1")); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE , lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_2")); - - ASSERT_TRUE(lifecycle_mgr_.shutdown(std_msec(2000), std_msec(2000)).empty()); - ASSERT_TRUE(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED == lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_1") - || lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN == lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_1")); - ASSERT_TRUE(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED == lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_2") - || lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN == lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_2")); - - promise_test_result.set_value(true); - }); + // Test set_managed_nodes + lifecycle_mgr_.set_managed_nodes( { "test_carma_lifecycle_node_1", "test_carma_lifecycle_node_2" } ); - RCLCPP_INFO(node->get_logger(), "Spinning"); - rclcpp::spin_until_future_complete(node, future_test_result); + std::this_thread::sleep_for(std_msec(2000)); + + // Test get_managed_nodes + auto managed_nodes = lifecycle_mgr_.get_managed_nodes(); - test_thread.join(); // Ensure thread closes + RCLCPP_INFO(node->get_logger(), "Spinning"); - + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + auto spin_future{std::async(std::launch::async, [&executor] + { executor.spin(); })}; + + ASSERT_EQ((uint8_t)2, managed_nodes.size()); + ASSERT_EQ((uint8_t)0, managed_nodes[0].compare("test_carma_lifecycle_node_1")); + ASSERT_EQ((uint8_t)0, managed_nodes[1].compare("test_carma_lifecycle_node_2")); + + // Test get managed node states + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_1")); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_2")); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_mgr_.get_managed_node_state("unknown_node")); + + // Test Configure + ASSERT_TRUE(lifecycle_mgr_.configure(std_msec(2000), std_msec(2000)).empty()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_1")); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_2")); + + // Test Activate + ASSERT_TRUE(lifecycle_mgr_.activate(std_msec(2000), std_msec(2000)).empty()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE , lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_1")); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE , lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_2")); + + // Test Deactivate + ASSERT_TRUE(lifecycle_mgr_.deactivate(std_msec(2000), std_msec(2000)).empty()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_1")); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_2")); + + // Test cleanup + ASSERT_TRUE(lifecycle_mgr_.cleanup(std_msec(2000), std_msec(2000)).empty()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED , lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_1")); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED , lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_2")); + + // Bring back to active then shutdown + ASSERT_TRUE(lifecycle_mgr_.configure(std_msec(2000), std_msec(2000)).empty()); + ASSERT_TRUE(lifecycle_mgr_.activate(std_msec(2000), std_msec(2000)).empty()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE , lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_1")); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE , lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_2")); + + ASSERT_TRUE(lifecycle_mgr_.shutdown(std_msec(2000), std_msec(2000)).empty()); + ASSERT_TRUE(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED == lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_1") + || lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN == lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_1")); + ASSERT_TRUE(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED == lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_2") + || lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN == lifecycle_mgr_.get_managed_node_state("test_carma_lifecycle_node_2")); + + executor.cancel(); } int main(int argc, char ** argv) @@ -123,7 +117,7 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); - bool success = RUN_ALL_TESTS(); + bool success = RUN_ALL_TESTS(); // shutdown ROS rclcpp::shutdown(); diff --git a/carma_ros2_utils/test/timers/TestingTimers.cpp b/carma_ros2_utils/test/timers/TestingTimers.cpp index 397e15df..4e29897c 100644 --- a/carma_ros2_utils/test/timers/TestingTimers.cpp +++ b/carma_ros2_utils/test/timers/TestingTimers.cpp @@ -19,6 +19,7 @@ #include #include #include +#include namespace carma_ros2_utils @@ -27,17 +28,20 @@ namespace timers { namespace testing { - + TEST(TestingTimers, buildTimer) { + using std::chrono_literals::operator""s; + using std::chrono_literals::operator""ns; + std::shared_ptr factory(new TestTimerFactory()); // Verify casting behavior - + uint32_t id = 1; std::atomic call_count(0); std::unique_ptr timer = factory->buildTimer( - id, rclcpp::Duration(1.0e9), [&]() -> void { call_count++; }, true); - + id, rclcpp::Duration(1s), [&]() -> void { call_count++; }, true); + // Test setting and getting the timer id ASSERT_EQ(id, timer->getId()); @@ -59,7 +63,7 @@ TEST(TestingTimers, buildTimer) id = 3; call_count.store(0); timer = factory->buildTimer( - id, rclcpp::Duration(1.0), [&]() -> void { call_count++; }, true, false); + id, rclcpp::Duration(1ns), [&]() -> void { call_count++; }, true, false); ASSERT_EQ(0, call_count.load()); factory->setNow(rclcpp::Time(3.2e9)); @@ -73,7 +77,7 @@ TEST(TestingTimers, buildTimer) id = 4; call_count.store(0); timer = factory->buildTimer( - id, rclcpp::Duration(1.0), [&]() -> void { call_count++; }, false, true); + id, rclcpp::Duration(1ns), [&]() -> void { call_count++; }, false, true); ASSERT_EQ(0, call_count.load()); factory->setNow(rclcpp::Time(5.2e9)); @@ -92,7 +96,7 @@ TEST(TestingTimers, buildTimer) timer->start(); // Run start twice; This requires visual inspection of output to verify // // Verify duplicate initialization throw - ASSERT_THROW(timer->initializeTimer(rclcpp::Duration(1.0), [&](){}), std::invalid_argument); + ASSERT_THROW(timer->initializeTimer(rclcpp::Duration(1ns), [&](){}), std::invalid_argument); } } // namespace testing diff --git a/carma_utils/include/carma_utils/CARMANodeHandle.h b/carma_utils/include/carma_utils/CARMANodeHandle.h index 22882841..d7f0e376 100644 --- a/carma_utils/include/carma_utils/CARMANodeHandle.h +++ b/carma_utils/include/carma_utils/CARMANodeHandle.h @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include #include diff --git a/motion_predict/README.md b/motion_predict/README.md index 56101ac3..5e1ffc17 100644 --- a/motion_predict/README.md +++ b/motion_predict/README.md @@ -1,5 +1,16 @@ # motion_predict -This library implements some basic motion prediction functions meant to support external object handling under the motion_predict namespace. Currently there are two motion models implements a Constant Velocity (CV) model and a Constant Turn-Rate and Velocity (CTRV) model. See the motion_predict.h and predict_ctrv.h files for the relevant interfaces. +This library implements some basic motion prediction functions meant to support external object handling under the motion_predict namespace. Currently there are two motion models implements a Constant Velocity (CV) model and a Constant Turn-Rate and Velocity (CTRV) model. See the motion_predict.h and predict_ctrv.h files for the relevant interfaces. -Link to detailed design on Confluence: https://usdot-carma.atlassian.net/l/c/e3U5DXZi \ No newline at end of file +> [!NOTE] +> Both motion models expect the objects' twist.linear data to be in a map frame as opposed to object's base_link commonly used in ROS https://www.ros.org/reps/rep-0105.html#base-link. +> For example: +> - pose.pose.x and pose.pose.y expected to be the location of the object in a map frame +> - twist.linear.x and twist.linear.y indicate the heading of the object in a map frame. Therefore, yaw value in CTRV or CV is the angle of the vector from this x,y, which is respect to the map frame. +> +> This means that: +> - pose.orientation is not meaningfully used except to pass it on. Usually this value aligns with the yaw calculated before, but can be different. +> +> See this open issue for the reasoning and current limitation: https://github.com/usdot-fhwa-stol/carma-platform/issues/2401 + +Link to detailed design on Confluence: https://usdot-carma.atlassian.net/l/c/e3U5DXZi diff --git a/motion_predict/include/motion_predict/motion_predict.hpp b/motion_predict/include/motion_predict/motion_predict.hpp index a2e6ff65..41174847 100644 --- a/motion_predict/include/motion_predict/motion_predict.hpp +++ b/motion_predict/include/motion_predict/motion_predict.hpp @@ -30,37 +30,35 @@ namespace motion_predict{ namespace cv{ - /*! + /*! \brief Mapping is used to map input range to an output range of different bandwidth. \param input is the current value of the process noise. \param process_noise_max is the maxium process noise of the system */ - double Mapping(const double input,const double process_noise_max); - /*! + /*! \brief predictState is used to predict future state. \param pose is position and orientation (m). \param twist is velocity (m/s). \param delta_t time predicted into the future (sec). + \note pose and twist are expected in the map frame. */ - carma_perception_msgs::msg::PredictedState predictState(const geometry_msgs::msg::Pose& pose, const geometry_msgs::msg::Twist& twist,const double delta_t); - /*! + /*! \brief externalPredict populates motion prediction with future pose and velocity. - \param obj external object. + \param obj external object, whose pose and twist are expected in map frame \param delta_t prediciton time into the future (sec) \param ax acceleration noise along x-axis (m^2/s^4) \param ay acceleration noise along y-axis (m^2/s^4) \param process_noise_max is the maximum process noise of the system */ - carma_perception_msgs::msg::PredictedState externalPredict(const carma_perception_msgs::msg::ExternalObject &obj,const double delta_t,const double ax,const double ay,const double process_noise_max); - /*! + /*! \brief externalPeriod populates sequence of predicted motion of the object. - \param obj external object. + \param obj external object, whose pose and twist are expected in map frame \param delta_t prediciton time into the future (sec) \param period sequence/time steps (sec) \param ax acceleration noise along x-axis (m^2/s^4) @@ -68,21 +66,20 @@ namespace cv{ \param process_noise_max is the maximum process noise of the system \param confidence_drop_rate rate of drop in confidence with time */ - std::vector predictPeriod(const carma_perception_msgs::msg::ExternalObject& obj, const double delta_t, const double period,const double ax,const double ay ,const double process_noise_max,const double confidence_drop_rate); - /*! + /*! \brief Mapping is used to map input range to an output range of different bandwidth. - \param obj predicted object + \param obj predicted object whose pose and twist are expected in map frame \param delta_t time predicted into the future (sec) \param confidence_drop_rate rate of drop in confidence with time */ - carma_perception_msgs::msg::PredictedState predictStep(const carma_perception_msgs::msg::PredictedState& obj, const double delta_t, const double confidence_drop_rate); - + /*Constant for conversion from seconds to nanoseconds*/ + constexpr int64_t SEC_TO_NANOSEC = 1e9; }//cv }//motion_predict -#endif /* MOTION_PREDICT_H */ \ No newline at end of file +#endif /* MOTION_PREDICT_H */ diff --git a/motion_predict/include/motion_predict/predict_ctrv.hpp b/motion_predict/include/motion_predict/predict_ctrv.hpp index 68f9a2f3..04905864 100644 --- a/motion_predict/include/motion_predict/predict_ctrv.hpp +++ b/motion_predict/include/motion_predict/predict_ctrv.hpp @@ -28,11 +28,10 @@ namespace ctrv * \brief Generates a set of motion predictions seperated by the given time step size for the given period. * Predictions are made using a CTRV motion model. * - * \param obj external object to predict + * \param obj external object to predict whose twist and pose are expected in map frame * \param delta_t prediction time step size in seconds * \param period The total prediction period in seconds * \param process_noise_max is the maximum process noise of the system - * * \return The predicted state of the external object at time t + delta_t */ std::vector predictPeriod(const carma_perception_msgs::msg::ExternalObject& obj, const double delta_t, @@ -42,10 +41,9 @@ std::vector predictPeriod(const carm * \brief predictStep populates motion prediction with future pose and velocity. * The predicted motion is created using a CTRV motion model * - * \param obj external object. + * \param obj external object whose twist and pose are expected in map frame * \param delta_t prediction time into the future in seconds * \param process_noise_max is the maximum process noise of the system - * * \return The predicted state of the external object at time t + delta_t */ @@ -56,10 +54,9 @@ carma_perception_msgs::msg::PredictedState predictStep(const carma_perception_ms * \brief predictStep populates motion prediction with future pose and velocity. * The predicted motion is created using a CTRV motion model. * - * \param obj previous prediction object. + * \param obj previous prediction object whose twist and pose are expected in map frame * \param delta_t prediction time into the future in seconds * \param process_noise_max is the maximum process noise of the system - * * \return The predicted state of the external object at time t + delta_t */ @@ -70,4 +67,4 @@ carma_perception_msgs::msg::PredictedState predictStep(const carma_perception_ms } // namespace predict -#endif /* PREDICT_CTRV_H */ \ No newline at end of file +#endif /* PREDICT_CTRV_H */ diff --git a/motion_predict/src/motion_predict.cpp b/motion_predict/src/motion_predict.cpp index 7f2481e8..723f4e3b 100644 --- a/motion_predict/src/motion_predict.cpp +++ b/motion_predict/src/motion_predict.cpp @@ -29,9 +29,9 @@ double Mapping(const double input,const double process_noise_max) double output_start = 1; // The lowest number of the range output. double output_end = 0; // The largest number of the range ouput. double output = (input - input_start) / (input_end - input_start) * (output_end - output_start) + output_start; - + return output; -} +} carma_perception_msgs::msg::PredictedState predictState(const geometry_msgs::msg::Pose& pose, const geometry_msgs::msg::Twist& twist,const double delta_t) { @@ -39,15 +39,18 @@ carma_perception_msgs::msg::PredictedState predictState(const geometry_msgs::msg x(0)=pose.position.x; // Position X x(1)=pose.position.y; // Position Y + + // TODO: Need a logic here to possible detect whether if twist.linear.x,y + // is in map frame. https://github.com/usdot-fhwa-stol/carma-platform/issues/2407 x(2)=twist.linear.x; // Linear Velocity X x(3)=twist.linear.y; // Linear Velocity Y - + Eigen::MatrixXd F=Eigen::MatrixXd::Identity(x.size(), x.size()); // Generate identity matrix for state transition matrix - F(0,2)=delta_t; + F(0,2)=delta_t; F(1,3)=delta_t; - - x = F * x; // Predict + + x = F * x; // Predict carma_perception_msgs::msg::PredictedState pobj; @@ -66,10 +69,10 @@ carma_perception_msgs::msg::PredictedState predictState(const geometry_msgs::msg pobj.predicted_velocity.angular.x=twist.angular.x; pobj.predicted_velocity.angular.y=twist.angular.y; - pobj.predicted_velocity.angular.z=twist.angular.y; + pobj.predicted_velocity.angular.z=twist.angular.z; return pobj; -} +} // Forward predict an external object @@ -80,7 +83,7 @@ carma_perception_msgs::msg::PredictedState externalPredict(const carma_perceptio Eigen::MatrixXd F=Eigen::MatrixXd::Identity(4,4); // Generate identity matrix for state transition matrix - F(0,2)=delta_t; + F(0,2)=delta_t; F(1,3)=delta_t; Eigen::MatrixXd P(4,4); // State Covariance Matrix @@ -95,13 +98,13 @@ carma_perception_msgs::msg::PredictedState externalPredict(const carma_perceptio Eigen::MatrixXd Q(4,4); // Process Noise Matrix double delta_t2 = delta_t * delta_t; //t^2 - double delta_t3 = delta_t2 * delta_t;//t^3 + double delta_t3 = delta_t2 * delta_t;//t^3 double delta_t4 = delta_t3 * delta_t;//t^4 Q << (delta_t4 / 4 * ax), 0,( delta_t3 / 2 * ax), 0, 0, (delta_t4 / 4 * ay), 0,( delta_t3 / 2 * ay), (delta_t3 / 2 * ax), 0,(delta_t2*ax), 0 , 0 , (delta_t3 / 2 * ay), 0 , (delta_t2*ay); // Process Noise Matrix - + Eigen::MatrixXd Ft = F.transpose(); // Transpose of State Transition Function - + P = F * P * Ft + Q; //State Covariance Matrix double position_process_noise_avg=(P(0,0)+P(1,1))/2; // Position process noise average @@ -114,7 +117,7 @@ carma_perception_msgs::msg::PredictedState externalPredict(const carma_perceptio // Update header pobj.header = obj.header; - rclcpp::Time updated_time = rclcpp::Time(obj.header.stamp) + rclcpp::Duration(delta_t * 1e9); + rclcpp::Time updated_time = rclcpp::Time(obj.header.stamp) + rclcpp::Duration(std::chrono::nanoseconds(int64_t(delta_t * SEC_TO_NANOSEC))); pobj.header.stamp = builtin_interfaces::msg::Time(updated_time); return pobj; @@ -133,9 +136,9 @@ carma_perception_msgs::msg::PredictedState predictStep(const carma_perception_ms // Update header pobj.header = obj.header; - rclcpp::Time updated_time = rclcpp::Time(obj.header.stamp) + rclcpp::Duration(delta_t * 1e9); + rclcpp::Time updated_time = rclcpp::Time(obj.header.stamp) + rclcpp::Duration(std::chrono::nanoseconds(int64_t(delta_t * SEC_TO_NANOSEC))); pobj.header.stamp = builtin_interfaces::msg::Time(updated_time); - + return pobj; } diff --git a/motion_predict/src/predict_ctrv.cpp b/motion_predict/src/predict_ctrv.cpp index 796a6a6d..01d8666a 100644 --- a/motion_predict/src/predict_ctrv.cpp +++ b/motion_predict/src/predict_ctrv.cpp @@ -45,25 +45,26 @@ std::tuple localVelOrientationAndMagnitude(const double v_x, con } else { - local_v_orientation = asin(v_y / v_mag); + local_v_orientation = atan2(v_y, v_x); } return std::make_tuple(local_v_orientation, v_mag); } CTRV_State buildCTRVState(const geometry_msgs::msg::Pose& pose, const geometry_msgs::msg::Twist& twist) { - geometry_msgs::msg::Quaternion quat = pose.orientation; - Eigen::Quaternionf e_quat(quat.w, quat.x, quat.y, quat.z); - Eigen::Vector3f rpy = e_quat.toRotationMatrix().eulerAngles(0, 1, 2); - + // TODO: Need a logic here to possible detect whether if twist.linear.x,y + // is in map frame. https://github.com/usdot-fhwa-stol/carma-platform/issues/2407 auto vel_angle_and_mag = localVelOrientationAndMagnitude(twist.linear.x, twist.linear.y); CTRV_State state; state.x = pose.position.x; state.y = pose.position.y; - state.yaw = - rpy[2] + std::get<0>(vel_angle_and_mag); // The yaw is relative to the velocity vector so take the heading and - // add it to the angle of the velocity vector in the local frame + state.yaw = std::get<0>(vel_angle_and_mag); // Currently, object's linear velocity is already in map frame. + // Orientation of the object cannot be trusted for objects + // as it could be drifting sideways while facing other direction. + // This may not be what the user expect, and below issue tracks it. + // https://github.com/usdot-fhwa-stol/carma-platform/issues/2401 + state.v = std::get<1>(vel_angle_and_mag); state.yaw_rate = twist.angular.z; @@ -81,21 +82,9 @@ carma_perception_msgs::msg::PredictedState buildPredictionFromCTRVState(const CT pobj.predicted_position.position.z = original_pose.position.z; // Map orientation - Eigen::Quaternionf original_quat(original_pose.orientation.w, original_pose.orientation.x, - original_pose.orientation.y, original_pose.orientation.z); - Eigen::Vector3f original_rpy = original_quat.toRotationMatrix().eulerAngles(0, 1, 2); - - auto vel_angle_and_mag = localVelOrientationAndMagnitude(original_twist.linear.x, original_twist.linear.y); - - Eigen::Quaternionf final_quat; - final_quat = Eigen::AngleAxisf(original_rpy[0], Eigen::Vector3f::UnitX()) * - Eigen::AngleAxisf(original_rpy[1], Eigen::Vector3f::UnitY()) * - Eigen::AngleAxisf(state.yaw - std::get<0>(vel_angle_and_mag), Eigen::Vector3f::UnitZ()); - - pobj.predicted_position.orientation.x = final_quat.x(); - pobj.predicted_position.orientation.y = final_quat.y(); - pobj.predicted_position.orientation.z = final_quat.z(); - pobj.predicted_position.orientation.w = final_quat.w(); + pobj.predicted_position.orientation = original_pose.orientation; + // TODO: Take another look at orientation calculation after addressing this: + // https://github.com/usdot-fhwa-stol/carma-platform/issues/2401 // Map twist // Constant velocity model means twist remains unchanged @@ -122,11 +111,12 @@ CTRV_State CTRVPredict(const CTRV_State& state, const double delta_t) double v_w = state.v / state.yaw_rate; double sin_yaw = sin(state.yaw); + double cos_yaw = cos(state.yaw); double wT = state.yaw_rate * delta_t; - next_state.x = v_w * sin(wT + state.yaw) - v_w * sin_yaw + state.x; - next_state.y = -v_w * cos(wT + state.yaw) + v_w * sin_yaw + state.y; - next_state.yaw = wT + state.yaw; + next_state.x = state.x + v_w * (sin(state.yaw + wT) - sin_yaw); + next_state.y = state.y + v_w * (cos_yaw - cos(state.yaw + wT)); + next_state.yaw = state.yaw + wT; next_state.v = state.v; next_state.yaw_rate = state.yaw_rate; @@ -165,7 +155,7 @@ carma_perception_msgs::msg::PredictedState predictStep(const carma_perception_ms // Update header pobj.header = obj.header; - rclcpp::Time updated_time = rclcpp::Time(obj.header.stamp) + rclcpp::Duration(delta_t * 1e9); + rclcpp::Time updated_time = rclcpp::Time(obj.header.stamp) + rclcpp::Duration(std::chrono::nanoseconds(int32_t(delta_t * 1e9))); pobj.header.stamp = builtin_interfaces::msg::Time(updated_time); return pobj; @@ -191,9 +181,9 @@ carma_perception_msgs::msg::PredictedState predictStep(const carma_perception_ms // Update header pobj.header = obj.header; - rclcpp::Time updated_time = rclcpp::Time(obj.header.stamp) + rclcpp::Duration(delta_t * 1e9); + rclcpp::Time updated_time = rclcpp::Time(obj.header.stamp) + rclcpp::Duration(std::chrono::nanoseconds(int32_t(delta_t * 1e9))); pobj.header.stamp = builtin_interfaces::msg::Time(updated_time); - + return pobj; } diff --git a/motion_predict/test/main.cpp b/motion_predict/test/main.cpp index 82234975..cdd3f866 100644 --- a/motion_predict/test/main.cpp +++ b/motion_predict/test/main.cpp @@ -15,7 +15,7 @@ */ #include -#include "motion_predict/motion_predict.hpp" +#include // Run all the tests int main(int argc, char **argv) { diff --git a/motion_predict/test/test_motion_predict.cpp b/motion_predict/test/test_motion_predict.cpp index b4975e27..3454802f 100644 --- a/motion_predict/test/test_motion_predict.cpp +++ b/motion_predict/test/test_motion_predict.cpp @@ -25,25 +25,25 @@ namespace cv{ { double input=500; double process_noise_max=1000; - + EXPECT_NEAR(0.500501,Mapping(input,process_noise_max),0.00001); - + } TEST(MotionPredictTest, testMappingLow) { double input=25; double process_noise_max=1000; - + EXPECT_NEAR(0.975976,Mapping(input,process_noise_max),0.00001); } TEST(MotionPredictTest, testMappingHigh) { - + double input=750; double process_noise_max=1000; - + EXPECT_NEAR(0.25025,Mapping(input,process_noise_max),0.00001); } @@ -53,28 +53,40 @@ namespace cv{ pose.position.x = 1.3; pose.position.y = 1.4; pose.position.z = 2.5; + pose.orientation.x = 1.3; + pose.orientation.y = 1.4; + pose.orientation.z = 2.5; + pose.orientation.w = 2.5; geometry_msgs::msg::Twist twist; twist.linear.x = 4.5; twist.linear.y = 2; twist.linear.z = 5; - + twist.angular.x = 4.5; + twist.angular.y = 2; + twist.angular.z = 5; + double delta_t=0.1; carma_perception_msgs::msg::PredictedState pobj = predictState(pose,twist,delta_t); - - EXPECT_NEAR(1.75, pobj.predicted_position.position.x ,0.00001); - EXPECT_NEAR(1.6, pobj.predicted_position.position.y,0.00001); - EXPECT_NEAR(2.5, pobj.predicted_position.position.z,0.00001); - EXPECT_NEAR(4.5,pobj.predicted_velocity.linear.x ,0.00001); - EXPECT_NEAR(2, pobj.predicted_velocity.linear.y,0.00001); - EXPECT_NEAR(5, pobj.predicted_velocity.linear.z,0.00001); + EXPECT_NEAR(1.75, pobj.predicted_position.position.x, 0.00001); + EXPECT_NEAR(1.6, pobj.predicted_position.position.y, 0.00001); + EXPECT_NEAR(2.5, pobj.predicted_position.position.z, 0.00001); + + EXPECT_NEAR(pose.orientation.x, pobj.predicted_position.orientation.x, 0.00001); + EXPECT_NEAR(pose.orientation.y, pobj.predicted_position.orientation.y, 0.00001); + EXPECT_NEAR(pose.orientation.z, pobj.predicted_position.orientation.z, 0.00001); + EXPECT_NEAR(pose.orientation.w, pobj.predicted_position.orientation.w, 0.00001); + + EXPECT_NEAR(twist.angular.x, pobj.predicted_velocity.angular.x, 0.00001); + EXPECT_NEAR(twist.angular.y, pobj.predicted_velocity.angular.y, 0.00001); + EXPECT_NEAR(twist.angular.z, pobj.predicted_velocity.angular.z, 0.00001); } - + TEST(MotionPredictTest, testexternalPredictLowCovariancePositionHighCovarianceVelocity) { - + double delta_t=0.1; double ax=9; double ay=9; @@ -87,8 +99,8 @@ namespace cv{ obj.velocity.covariance[0]=999; // Vx obj.velocity.covariance[7]=999; // Vy - carma_perception_msgs::msg::PredictedState pobj=externalPredict(obj,delta_t,ax,ay,process_noise_max); - + carma_perception_msgs::msg::PredictedState pobj=externalPredict(obj,delta_t,ax,ay,process_noise_max); + EXPECT_NEAR(0.99, pobj.predicted_position_confidence ,0.00001); EXPECT_NEAR(0.000910911, pobj.predicted_velocity_confidence,0.00001); @@ -109,8 +121,8 @@ namespace cv{ obj.velocity.covariance[0]=1; // Vx obj.velocity.covariance[7]=1; // Vy - carma_perception_msgs::msg::PredictedState pobj=externalPredict(obj,delta_t,ax,ay,process_noise_max); - + carma_perception_msgs::msg::PredictedState pobj=externalPredict(obj,delta_t,ax,ay,process_noise_max); + EXPECT_NEAR(0.000990766, pobj.predicted_position_confidence ,0.00001); EXPECT_NEAR(0.99991, pobj.predicted_velocity_confidence,0.00001); @@ -130,8 +142,8 @@ namespace cv{ obj.velocity.covariance[0]=999; // Vx obj.velocity.covariance[7]=999; // Vy - carma_perception_msgs::msg::PredictedState pobj=externalPredict(obj,delta_t,ax,ay,process_noise_max); - + carma_perception_msgs::msg::PredictedState pobj=externalPredict(obj,delta_t,ax,ay,process_noise_max); + EXPECT_NEAR(-0.00225225, pobj.predicted_position_confidence ,0.00001); EXPECT_NEAR(-0.00800801, pobj.predicted_velocity_confidence,0.00001); @@ -140,7 +152,7 @@ namespace cv{ TEST(MotionPredictTest, testpredictStep) { - + double delta_t=0.1; double confidence_drop_rate=0.1; @@ -157,7 +169,7 @@ namespace cv{ pobj.predicted_position_confidence=0.99; // Position process noise confidence pobj.predicted_velocity_confidence=0.000910911; // Velocity process noise confidence - pobj=predictStep(pobj,delta_t,confidence_drop_rate); + pobj=predictStep(pobj,delta_t,confidence_drop_rate); EXPECT_NEAR(1.75, pobj.predicted_position.position.x ,0.00001); EXPECT_NEAR(1.6, pobj.predicted_position.position.y,0.0001); @@ -165,8 +177,8 @@ namespace cv{ EXPECT_NEAR(4.5,pobj.predicted_velocity.linear.x ,0.0001); EXPECT_NEAR(2, pobj.predicted_velocity.linear.y,0.0001); - EXPECT_NEAR(5, pobj.predicted_velocity.linear.z,0.0001); - + EXPECT_NEAR(5, pobj.predicted_velocity.linear.z,0.0001); + EXPECT_NEAR(0.099, pobj.predicted_position_confidence ,0.0001); EXPECT_NEAR(0.0000910911, pobj.predicted_velocity_confidence,0.0001); } @@ -193,17 +205,17 @@ namespace cv{ obj.pose.covariance[7]=1; // Y obj.velocity.covariance[0]=999; // Vx obj.velocity.covariance[7]=999; // Vy - - std::vector plist=predictPeriod(obj,delta_t,period,ax,ay ,process_noise_max,confidence_drop_rate); - + + std::vector plist=predictPeriod(obj,delta_t,period,ax,ay ,process_noise_max,confidence_drop_rate); + EXPECT_NEAR(1.75, plist[0].predicted_position.position.x ,0.00001); EXPECT_NEAR(1.6, plist[0].predicted_position.position.y,0.0001); EXPECT_NEAR(2.5, plist[0].predicted_position.position.z,0.0001); EXPECT_NEAR(4.5,plist[0].predicted_velocity.linear.x ,0.0001); EXPECT_NEAR(2, plist[0].predicted_velocity.linear.y,0.0001); - EXPECT_NEAR(5, plist[0].predicted_velocity.linear.z,0.0001); - + EXPECT_NEAR(5, plist[0].predicted_velocity.linear.z,0.0001); + EXPECT_NEAR(0.99, plist[0].predicted_position_confidence ,0.0001); EXPECT_NEAR(0.000910911, plist[0].predicted_velocity_confidence,0.0001); @@ -213,8 +225,8 @@ namespace cv{ EXPECT_NEAR(4.5,plist[1].predicted_velocity.linear.x ,0.0001); EXPECT_NEAR(2, plist[1].predicted_velocity.linear.y,0.0001); - EXPECT_NEAR(5, plist[1].predicted_velocity.linear.z,0.0001); - + EXPECT_NEAR(5, plist[1].predicted_velocity.linear.z,0.0001); + EXPECT_NEAR(0.099, plist[1].predicted_position_confidence ,0.0001); EXPECT_NEAR(0.0000910911, plist[1].predicted_velocity_confidence,0.0001); } diff --git a/motion_predict/test/test_predict_ctrv.cpp b/motion_predict/test/test_predict_ctrv.cpp index b64bb30a..424b7044 100644 --- a/motion_predict/test/test_predict_ctrv.cpp +++ b/motion_predict/test/test_predict_ctrv.cpp @@ -14,7 +14,6 @@ * the License. */ -#include "motion_predict/predict_ctrv.hpp" #include "../src/predict_ctrv.cpp" #include @@ -48,7 +47,7 @@ TEST(predict_ctrv, buildCTRVState) CTRV_State result = buildCTRVState(pose, twist); ASSERT_NEAR(result.x, 1.3, 0.000001); ASSERT_NEAR(result.y, 1.4, 0.000001); - ASSERT_NEAR(result.yaw, 1.98902433, 0.00001); + ASSERT_NEAR(result.yaw, 0.41822, 0.00001); // directly uses direction of x, y ASSERT_NEAR(result.v, 4.9244289009, 0.000001); ASSERT_NEAR(result.yaw_rate, 0.0872665, 0.0000001); } @@ -100,8 +99,7 @@ TEST(predict_ctrv, buildPredictionFromCTRVState) ASSERT_NEAR(result.predicted_velocity.angular.z, twist.angular.z, 0.00001); } -TEST(predict_ctrv, CTRVPredict) -{ +TEST(predict_ctrv, CTRVPredict) { // Regular prediction CTRV_State state; state.x = 1.3; @@ -113,7 +111,7 @@ TEST(predict_ctrv, CTRVPredict) CTRV_State result = CTRVPredict(state, 0.1); ASSERT_NEAR(result.x, 1.29785, 0.00001); - ASSERT_NEAR(result.y, 58.3224, 0.0001); + ASSERT_NEAR(result.y, 1.89244, 0.0001); ASSERT_NEAR(result.yaw, 1.57953, 0.00001); ASSERT_NEAR(result.v, state.v, 0.00001); ASSERT_NEAR(result.yaw_rate, state.yaw_rate, 0.00001); @@ -127,7 +125,7 @@ TEST(predict_ctrv, CTRVPredict) result = CTRVPredict(state, 0.1); - ASSERT_NEAR(result.x, 1.2999819, 0.0001); + ASSERT_NEAR(result.x, 1.3, 0.0001); ASSERT_NEAR(result.y, 1.89244, 0.0001); ASSERT_NEAR(result.yaw, 1.5708, 0.00001); ASSERT_NEAR(result.v, state.v, 0.00001); @@ -140,26 +138,37 @@ TEST(predict_ctrv, predictStepExternal) obj.header.stamp = builtin_interfaces::msg::Time(rclcpp::Time(5, 0)); obj.header.frame_id = "my_frame"; obj.pose.pose.position.x = 5.0; - obj.pose.pose.orientation.w = 1.0; + obj.pose.pose.position.y = 1.4; // Added y position + // Set the orientation quaternion for 0.79 radians yaw + double yaw_angle = 0.79; + obj.pose.pose.orientation.w = cos(yaw_angle / 2); + obj.pose.pose.orientation.x = 0.0; + obj.pose.pose.orientation.y = 0.0; + obj.pose.pose.orientation.z = sin(yaw_angle / 2); obj.pose.covariance[0] = 1; obj.pose.covariance[7] = 1; obj.pose.covariance[35] = 1; + obj.velocity.twist.linear.x = 4.9244289009 * cos(yaw_angle); // Matching velocity to orientation, as currently map frame is expected in the velocity + obj.velocity.twist.linear.y = 4.9244289009 * sin(yaw_angle); // Matching velocity to orientation, as currently map frame is expected in the velocity obj.velocity.covariance[0] = 999; obj.velocity.covariance[7] = 999; obj.velocity.covariance[35] = 999; carma_perception_msgs::msg::PredictedState result = motion_predict::ctrv::predictStep(obj, 0.1, 1000, 0.99); - ASSERT_NEAR(5.0, result.predicted_position.position.x, 0.00001); // Verify update functions were called - ASSERT_NEAR(0.99, result.predicted_position_confidence, 0.01); - ASSERT_NEAR(0.001, result.predicted_velocity_confidence, 0.001); - - rclcpp::Time new_time = rclcpp::Time(obj.header.stamp) + rclcpp::Duration(0.1 * 1e9); // Increase by 0.1 sec + EXPECT_NEAR(5.3466, result.predicted_position.position.x, 0.00001); // Verify x position update + EXPECT_NEAR(1.7498, result.predicted_position.position.y, 0.00001); // Verify y position update + EXPECT_NEAR(4.9244289009 * cos(yaw_angle), result.predicted_velocity.linear.x, 0.00001); // Verify velocity speed + EXPECT_NEAR(4.9244289009 * sin(yaw_angle), result.predicted_velocity.linear.y, 0.00001); // Verify velocity speed + EXPECT_NEAR(0.99, result.predicted_position_confidence, 0.01); + EXPECT_NEAR(0.001, result.predicted_velocity_confidence, 0.001); + + rclcpp::Time new_time = rclcpp::Time(obj.header.stamp) + rclcpp::Duration(std::chrono::nanoseconds(int32_t(0.1 * 1e9))); // Increase by 0.1 sec int32_t new_time_sec = int32_t(new_time.nanoseconds() / 1e9); - uint32_t new_time_nanosec = new_time.nanoseconds() - (new_time_sec*1e9); - ASSERT_EQ(result.header.stamp.sec, new_time_sec); - ASSERT_EQ(result.header.stamp.nanosec, new_time_nanosec); - ASSERT_EQ(result.header.frame_id, obj.header.frame_id); + uint32_t new_time_nanosec = new_time.nanoseconds() - (new_time_sec * 1e9); + EXPECT_EQ(result.header.stamp.sec, new_time_sec); + EXPECT_EQ(result.header.stamp.nanosec, new_time_nanosec); + EXPECT_EQ(result.header.frame_id, obj.header.frame_id); } TEST(predict_ctrv, predictStep) @@ -178,7 +187,7 @@ TEST(predict_ctrv, predictStep) ASSERT_NEAR(0.99, result.predicted_position_confidence, 0.01); ASSERT_NEAR(0.495, result.predicted_velocity_confidence, 0.0001); - rclcpp::Time new_time = rclcpp::Time(obj.header.stamp) + rclcpp::Duration(0.1 * 1e9); // Increase by 0.1 sec + rclcpp::Time new_time = rclcpp::Time(obj.header.stamp) + rclcpp::Duration(std::chrono::nanoseconds(int32_t(0.1 * 1e9))); // Increase by 0.1 sec int32_t new_time_sec = int32_t(new_time.nanoseconds() / 1e9); uint32_t new_time_nanosec = new_time.nanoseconds() - (new_time_sec*1e9); ASSERT_EQ(result.header.stamp.sec, new_time_sec); @@ -206,10 +215,10 @@ TEST(predict_ctrv, predictPeriod) ASSERT_NEAR(5.1, results[0].predicted_position.position.x, 0.00001); // Verify update functions were called ASSERT_NEAR(0.99, results[0].predicted_position_confidence, 0.01); ASSERT_NEAR(0.001, results[0].predicted_velocity_confidence, 0.001); - - rclcpp::Time new_time = rclcpp::Time(obj.header.stamp) + rclcpp::Duration(0.1 * 1e9); // Increase by 0.1 sec + + rclcpp::Time new_time = rclcpp::Time(obj.header.stamp) + rclcpp::Duration(std::chrono::nanoseconds(int32_t(0.1 * 1e9))); // Increase by 0.1 sec int32_t new_time_sec = int32_t(new_time.nanoseconds() / 1e9); - uint32_t new_time_nanosec = new_time.nanoseconds() - (new_time_sec*1e9); + uint32_t new_time_nanosec = new_time.nanoseconds() - (new_time_sec*1e9); ASSERT_EQ(results[0].header.stamp.sec, new_time_sec); ASSERT_EQ(results[0].header.stamp.nanosec, new_time_nanosec); ASSERT_EQ(results[0].header.frame_id, obj.header.frame_id); @@ -218,13 +227,13 @@ TEST(predict_ctrv, predictPeriod) ASSERT_NEAR(0.9801, results[1].predicted_position_confidence, 0.01); ASSERT_NEAR(0.00099, results[1].predicted_velocity_confidence, 0.00001); - new_time = rclcpp::Time(obj.header.stamp) + rclcpp::Duration(0.2 * 1e9); // Increase by 0.2 sec + new_time = rclcpp::Time(obj.header.stamp) + rclcpp::Duration(std::chrono::nanoseconds(int32_t(0.2 * 1e9))); // Increase by 0.2 sec new_time_sec = int32_t(new_time.nanoseconds() / 1e9); - new_time_nanosec = new_time.nanoseconds() - (new_time_sec*1e9); + new_time_nanosec = new_time.nanoseconds() - (new_time_sec*1e9); ASSERT_EQ(results[1].header.stamp.sec, new_time_sec); ASSERT_EQ(results[1].header.stamp.nanosec, new_time_nanosec); ASSERT_EQ(results[1].header.frame_id, obj.header.frame_id); } } // namespace ctrv -} // namespace motion_predict \ No newline at end of file +} // namespace motion_predict diff --git a/ros2_lifecycle_manager/CMakeLists.txt b/ros2_lifecycle_manager/CMakeLists.txt index a39c60e0..a8163744 100644 --- a/ros2_lifecycle_manager/CMakeLists.txt +++ b/ros2_lifecycle_manager/CMakeLists.txt @@ -1,40 +1,65 @@ -cmake_minimum_required(VERSION 3.5) +# Copyright 2024 Leidos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +cmake_minimum_required(VERSION 3.16) project(ros2_lifecycle_manager) +# CARMA builds packages in an environment with both ROS 1 and ROS 2 installed. +# This check gracefully skips the current package if the sourced ROS environment +# is not the specified version. This call must come before any other ROS +# dependencies becasue ROS 1 does not have some of the required packages. find_package(carma_cmake_common REQUIRED) carma_check_ros_version(2) -carma_package() -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(lifecycle_msgs REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(rcutils REQUIRED) - -set(dependencies - lifecycle_msgs - rclcpp - rclcpp_action - rclcpp_lifecycle - rcutils - rclcpp_components -) +include(dependencies.cmake) -set(cpp_files +carma_package() + +ament_auto_add_library(ros2_lifecycle_manager SHARED src/ros2_lifecycle_manager.cpp ) -carma_define_ament_library(${PROJECT_NAME} "${dependencies}" "${cpp_files}") +if(BUILD_TESTING) + # These CMake commands were added to ament_cmake_auto in ROS 2 Humble. Until + # CARMA supports ROS 2 Humble, we will use package-local copies. + include(cmake/ament_auto_find_test_dependencies.cmake) + include(cmake/ament_auto_add_gtest.cmake) + ament_auto_add_executable(test_lifecycle_node + test/test_node.cpp + ) -if(BUILD_TESTING) + ament_add_gtest_executable(test_lifecycle_gtest + test/test_lifecycle_manager.cpp + ) - find_package(ament_cmake_gtest REQUIRED) - find_package(ament_cmake_pytest REQUIRED) + # ament_add_gtest_executable() does not automaticallly link libraries + target_link_libraries(test_lifecycle_gtest + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ros2_lifecycle_manager + ) - add_subdirectory(test) + # TODO(CAR-6015): Replace this with launch_testing package + ament_add_test(test_lifecycle + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${PROJECT_SOURCE_DIR}/test/launch_lifecycle_test.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 20 + ENV + TEST_EXECUTABLE=$ + ) endif() -ament_package() +ament_auto_package() diff --git a/ros2_lifecycle_manager/cmake/ament_auto_add_gtest.cmake b/ros2_lifecycle_manager/cmake/ament_auto_add_gtest.cmake new file mode 100644 index 00000000..f723f9c9 --- /dev/null +++ b/ros2_lifecycle_manager/cmake/ament_auto_add_gtest.cmake @@ -0,0 +1,112 @@ +# Copyright 2021 Whitley Software Services, LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# +# Add a gtest with all found test dependencies. +# +# Call add_executable(target ARGN), link it against the gtest libraries +# and all found test dependencies, and then register the executable as a test. +# +# If gtest is not available the specified target is not being created and +# therefore the target existence should be checked before being used. +# +# :param target: the target name which will also be used as the test name +# :type target: string +# :param ARGN: the list of source files +# :type ARGN: list of strings +# :param RUNNER: the path to the test runner script (default: +# see ament_add_test). +# :type RUNNER: string +# :param TIMEOUT: the test timeout in seconds, +# default defined by ``ament_add_test()`` +# :type TIMEOUT: integer +# :param WORKING_DIRECTORY: the working directory for invoking the +# executable in, default defined by ``ament_add_test()`` +# :type WORKING_DIRECTORY: string +# :param SKIP_LINKING_MAIN_LIBRARIES: if set skip linking against the gtest +# main libraries +# :type SKIP_LINKING_MAIN_LIBRARIES: option +# :param SKIP_TEST: if set mark the test as being skipped +# :type SKIP_TEST: option +# :param ENV: list of env vars to set; listed as ``VAR=value`` +# :type ENV: list of strings +# :param APPEND_ENV: list of env vars to append if already set, otherwise set; +# listed as ``VAR=value`` +# :type APPEND_ENV: list of strings +# :param APPEND_LIBRARY_DIRS: list of library dirs to append to the appropriate +# OS specific env var, a la LD_LIBRARY_PATH +# :type APPEND_LIBRARY_DIRS: list of strings +# +# @public +# +macro(ament_auto_add_gtest target) + cmake_parse_arguments(_ARG + "SKIP_LINKING_MAIN_LIBRARIES;SKIP_TEST" + "RUNNER;TIMEOUT;WORKING_DIRECTORY" + "APPEND_ENV;APPEND_LIBRARY_DIRS;ENV" + ${ARGN}) + if(NOT _ARG_UNPARSED_ARGUMENTS) + message(FATAL_ERROR + "ament_auto_add_gtest() must be invoked with at least one source file") + endif() + + # add executable + set(_arg_executable ${_ARG_UNPARSED_ARGUMENTS}) + if(_ARG_SKIP_LINKING_MAIN_LIBRARIES) + list(APPEND _arg_executable "SKIP_LINKING_MAIN_LIBRARIES") + endif() + ament_add_gtest_executable("${target}" ${_arg_executable}) + + # add include directory of this package if it exists + if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/include") + target_include_directories("${target}" PUBLIC + "${CMAKE_CURRENT_SOURCE_DIR}/include") + endif() + + # link against other libraries of this package + if(NOT ${PROJECT_NAME}_LIBRARIES STREQUAL "") + target_link_libraries("${target}" ${${PROJECT_NAME}_LIBRARIES}) + endif() + + # add exported information from found dependencies + ament_target_dependencies(${target} + ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS} + ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} + ) + + # add test + set(_arg_test "") + if(_ARG_RUNNER) + list(APPEND _arg_test "RUNNER" "${_ARG_RUNNER}") + endif() + if(_ARG_TIMEOUT) + list(APPEND _arg_test "TIMEOUT" "${_ARG_TIMEOUT}") + endif() + if(_ARG_WORKING_DIRECTORY) + list(APPEND _arg_test "WORKING_DIRECTORY" "${_ARG_WORKING_DIRECTORY}") + endif() + if(_ARG_SKIP_TEST) + list(APPEND _arg_test "SKIP_TEST") + endif() + if(_ARG_ENV) + list(APPEND _arg_test "ENV" ${_ARG_ENV}) + endif() + if(_ARG_APPEND_ENV) + list(APPEND _arg_test "APPEND_ENV" ${_ARG_APPEND_ENV}) + endif() + if(_ARG_APPEND_LIBRARY_DIRS) + list(APPEND _arg_test "APPEND_LIBRARY_DIRS" ${_ARG_APPEND_LIBRARY_DIRS}) + endif() + ament_add_gtest_test("${target}" ${_arg_test}) +endmacro() diff --git a/ros2_lifecycle_manager/cmake/ament_auto_find_test_dependencies.cmake b/ros2_lifecycle_manager/cmake/ament_auto_find_test_dependencies.cmake new file mode 100644 index 00000000..8320b26e --- /dev/null +++ b/ros2_lifecycle_manager/cmake/ament_auto_find_test_dependencies.cmake @@ -0,0 +1,41 @@ +# Copyright 2021 Whitley Software Services, LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# +# Invoke find_package() for all test dependencies. +# +# All found package names are appended to the +# ``${PROJECT_NAME}_FOUND_TEST_DEPENDS`` variables. +# +# @public +# +macro(ament_auto_find_test_dependencies) + set(_ARGN "${ARGN}") + if(_ARGN) + message(FATAL_ERROR "ament_auto_find_test_dependencies() called with " + "unused arguments: ${_ARGN}") + endif() + + if(NOT _AMENT_PACKAGE_NAME) + ament_package_xml() + endif() + + # try to find_package() all test dependencies + foreach(_dep ${${PROJECT_NAME}_TEST_DEPENDS}) + find_package(${_dep} QUIET) + if(${_dep}_FOUND) + list(APPEND ${PROJECT_NAME}_FOUND_TEST_DEPENDS ${_dep}) + endif() + endforeach() +endmacro() diff --git a/ros2_lifecycle_manager/dependencies.cmake b/ros2_lifecycle_manager/dependencies.cmake new file mode 100644 index 00000000..f4e1264b --- /dev/null +++ b/ros2_lifecycle_manager/dependencies.cmake @@ -0,0 +1,34 @@ +# Copyright 2024 Leidos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # TODO(CAR-6016): Integrate addional tests into package + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_uncrustify + ament_cmake_copyright + ament_cmake_cppcheck + ament_cmake_cpplint + ament_cmake_flake8 + ament_cmake_lint_cmake + ament_cmake_pep257 + ament_cmake_xmllint + ) + + ament_lint_auto_find_test_dependencies() +endif() diff --git a/ros2_lifecycle_manager/src/ros2_lifecycle_manager.cpp b/ros2_lifecycle_manager/src/ros2_lifecycle_manager.cpp index 209c7d53..255994d8 100644 --- a/ros2_lifecycle_manager/src/ros2_lifecycle_manager.cpp +++ b/ros2_lifecycle_manager/src/ros2_lifecycle_manager.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 LEIDOS. + * Copyright (C) 2021-2024 LEIDOS. * * Licensed under the Apache License, Version 2.0 (the "License"); you may not * use this file except in compliance with the License. You may obtain a copy of @@ -28,7 +28,7 @@ namespace ros2_lifecycle_manager rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services) : node_base_(node_base), node_graph_(node_graph), node_logging_(node_logging), node_services_(node_services) { - service_callback_group_ = node_base_->create_callback_group(rclcpp::callback_group::CallbackGroupType::Reentrant); + service_callback_group_ = node_base_->create_callback_group(rclcpp::CallbackGroupType::Reentrant); } void Ros2LifecycleManager::set_managed_nodes(const std::vector &nodes) @@ -138,7 +138,7 @@ namespace ros2_lifecycle_manager { RCLCPP_ERROR_STREAM( node_logging_->get_logger(), "transition_node_to_state does not support transitioning to temporary states."); - + return get_managed_node_state(node); // Return whatever the current state is } @@ -155,19 +155,19 @@ namespace ros2_lifecycle_manager // This object represents a mapping of all source->target combinations for primary states to the function call path required to reach that state // It is implemented as a map of maps where the keys for each map are source state and target state respectively - // The inner map contains a list of transition functions to call in order to move a node to the target state from the source state + // The inner map contains a list of transition functions to call in order to move a node to the target state from the source state static std::unordered_map>> transitions_paths = { - { UNCONFIGURED, { - { INACTIVE, { configure_func } }, - { ACTIVE, {configure_func, activate_func } }, + { UNCONFIGURED, { + { INACTIVE, { configure_func } }, + { ACTIVE, {configure_func, activate_func } }, { FINALIZED, { shutdown_func } } } }, - { INACTIVE, { - { UNCONFIGURED, { cleanup_func } }, - { ACTIVE, { activate_func } }, + { INACTIVE, { + { UNCONFIGURED, { cleanup_func } }, + { ACTIVE, { activate_func } }, { FINALIZED, { shutdown_func } } } }, - { ACTIVE, { - { UNCONFIGURED, { deactivate_func, cleanup_func } }, - { INACTIVE, { deactivate_func } }, + { ACTIVE, { + { UNCONFIGURED, { deactivate_func, cleanup_func } }, + { INACTIVE, { deactivate_func } }, { FINALIZED, { shutdown_func } } } } }; @@ -176,8 +176,8 @@ namespace ros2_lifecycle_manager auto current_state = get_managed_node_state(node); // If the states are the same or current state is finalized, or unknown then no need for further transition - if (current_state == state - || current_state == UNKNOWN + if (current_state == state + || current_state == UNKNOWN || current_state == FINALIZED) { return current_state; @@ -199,7 +199,7 @@ namespace ros2_lifecycle_manager for (const auto transition_func : transitions_paths.at(current_state).at(state)) { bool success = transition_func(connection_timeout, call_timeout, true, {node}).empty(); - + // If a transition failed then return whatever the resulting state is if (!success) { @@ -208,10 +208,10 @@ namespace ros2_lifecycle_manager } - // Since all our transitions succeeded + // Since all our transitions succeeded // we can be confident that the node is in the desired state and just return that without an extra service call return state; - + } std::vector Ros2LifecycleManager::configure(const std_nanosec &connection_timeout, const std_nanosec &call_timeout, bool ordered, std::vector nodes) @@ -237,12 +237,12 @@ namespace ros2_lifecycle_manager std::vector Ros2LifecycleManager::shutdown(const std_nanosec &connection_timeout, const std_nanosec &call_timeout, bool ordered, std::vector nodes) { // To avoid having to track all the node sates and pick the appropriate shutdown we will simply shut down in order of most dangerous - // Active is shutdown first to avoid more data being published, this is followed by inactive and finally unconfigured + // Active is shutdown first to avoid more data being published, this is followed by inactive and finally unconfigured auto failed_nodes = transition_multiplex(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVE_SHUTDOWN, ordered, connection_timeout, call_timeout, nodes); transition_multiplex(lifecycle_msgs::msg::Transition::TRANSITION_INACTIVE_SHUTDOWN, ordered, connection_timeout, call_timeout, nodes); transition_multiplex(lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN, ordered, connection_timeout, call_timeout, nodes); - - return failed_nodes; + + return failed_nodes; } std::vector Ros2LifecycleManager::transition_multiplex(uint8_t transition, bool ordered, const std_nanosec &connection_timeout, const std_nanosec &call_timeout, std::vector nodes) @@ -259,7 +259,7 @@ namespace ros2_lifecycle_manager { // If empty then transition all nodes nodes_to_transition = managed_nodes_; - } + } else { nodes_to_transition.reserve(nodes.size()); @@ -292,7 +292,7 @@ namespace ros2_lifecycle_manager node_logging_->get_logger(), "Calling node: " << node.node_name); // Call service - ChangeStateSharedFutureWithRequest future_result = node.change_state_client->async_send_request(request, [](ChangeStateSharedFutureWithRequest) {}); + auto future_result{node.change_state_client->async_send_request(request, [](ChangeStateSharedFutureWithRequest) {})}; // Wait for response if (!wait_on_change_state_future(future_result, call_timeout)) @@ -320,14 +320,15 @@ namespace ros2_lifecycle_manager node_logging_->get_logger(), "Calling node a-sync: " << node.node_name); // Call service and record future - futures.emplace_back(node.change_state_client->async_send_request(request, [](ChangeStateSharedFutureWithRequest) {})); + // TODO(CAR-6014): Remove static cast when CARMA Platform drops ROS Foxy support + futures.emplace_back(static_cast(node.change_state_client->async_send_request(request, [](ChangeStateSharedFutureWithRequest) {}))); future_node_map.emplace(futures.size() - 1, node.node_name); } size_t i = 0; for (auto future : futures) { RCLCPP_INFO_STREAM(node_logging_->get_logger(), "Waiting on future for node: " << future_node_map.at(i)); - + if (!wait_on_change_state_future(future, call_timeout)) { failed_nodes.push_back(future_node_map.at(i)); @@ -346,7 +347,7 @@ namespace ros2_lifecycle_manager // If the request times out, we return an unknown state. RCLCPP_INFO( node_logging_->get_logger(), "Waiting for future"); - + auto future_status = future.wait_for(timeout); if (future_status != std::future_status::ready) diff --git a/ros2_lifecycle_manager/test/CMakeLists.txt b/ros2_lifecycle_manager/test/CMakeLists.txt deleted file mode 100644 index 0ce4011b..00000000 --- a/ros2_lifecycle_manager/test/CMakeLists.txt +++ /dev/null @@ -1,37 +0,0 @@ -add_executable(test_lifecycle_node - test_node.cpp -) - -target_link_libraries(test_lifecycle_node - ${lifecycle_msgs_LIBRARIES} - ${rclcpp_LIBRARIES} - ${rclcpp_lifecycle_LIBRARIES} -) - - -ament_add_gtest_executable(test_lifecycle_gtest - test_lifecycle_manager.cpp -) - -target_link_libraries(test_lifecycle_gtest - ${PROJECT_NAME} -) - -ament_target_dependencies(test_lifecycle_gtest - ${dependencies} -) - -ament_add_test(test_lifecycle - GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/launch_lifecycle_test.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - TIMEOUT 20 - ENV - TEST_EXECUTABLE=$ -) - -install( - TARGETS test_lifecycle_node - DESTINATION lib/${PROJECT_NAME} -) - diff --git a/ros2_lifecycle_manager/test/test_lifecycle_manager.cpp b/ros2_lifecycle_manager/test/test_lifecycle_manager.cpp index 50b164c2..eea5145a 100644 --- a/ros2_lifecycle_manager/test/test_lifecycle_manager.cpp +++ b/ros2_lifecycle_manager/test/test_lifecycle_manager.cpp @@ -15,121 +15,114 @@ */ #include -#include + #include -#include #include -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "ros2_lifecycle_manager/ros2_lifecycle_manager.hpp" -#include "boost/core/ignore_unused.hpp" +#include +#include +#include +#include +#include + +#include "ros2_lifecycle_manager/ros2_lifecycle_manager.hpp" -using std_msec = std::chrono::milliseconds; +using std::chrono_literals::operator""ms; /** - * This test is meant to exercise the standard flow of the lifecycle manager. - * To allow for callbacks to be processed the test makes use of a rather roundabout thread paradigm + * This test is meant to exercise the standard flow of the lifecycle manager. + * To allow for callbacks to be processed the test makes use of a rather roundabout thread paradigm * Care should be taken to understand this code before trying to duplicate it in other test scenarios. * There may be better approaches such as the launch_testing framework. - */ + */ TEST(LifecycleManagerTest, BasicTest) { // Test constructor auto node = std::make_shared("lifecycle_manager_test_node"); // Node to connect to ROS network - auto ret = rcutils_logging_set_logger_level( - node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); - boost::ignore_unused(ret); - - std::promise promise_test_result; // Promise which will be used to spin the node until the test completes - std::shared_future future_test_result(promise_test_result.get_future()); // Future to check for spin - - std::thread test_thread([&promise_test_result, node](){ - - ros2_lifecycle_manager::Ros2LifecycleManager lifecycle_mgr_( // Construct lifecycle manager - node->get_node_base_interface(), - node->get_node_graph_interface(), - node->get_node_logging_interface(), - node->get_node_services_interface() - ); - - // Test set_managed_nodes - lifecycle_mgr_.set_managed_nodes( { "test_lifecycle_node_1", "test_lifecycle_node_2" } ); - - std::this_thread::sleep_for(std_msec(2000)); - - // Test get_managed_nodes - auto managed_nodes = lifecycle_mgr_.get_managed_nodes(); - - ASSERT_EQ((uint8_t)2, managed_nodes.size()); - ASSERT_EQ((uint8_t)0, managed_nodes[0].compare("test_lifecycle_node_1")); - ASSERT_EQ((uint8_t)0, managed_nodes[1].compare("test_lifecycle_node_2")); - - // Test get managed node states - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_1")); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_2")); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_mgr_.get_managed_node_state("unknown_node")); - - // Test Configure - ASSERT_TRUE(lifecycle_mgr_.configure(std_msec(2000), std_msec(2000)).empty()); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_1")); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_2")); - - // Test Activate - ASSERT_TRUE(lifecycle_mgr_.activate(std_msec(2000), std_msec(2000)).empty()); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE , lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_1")); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE , lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_2")); - - // Test Deactivate - ASSERT_TRUE(lifecycle_mgr_.deactivate(std_msec(2000), std_msec(2000)).empty()); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_1")); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_2")); - - // Test cleanup - ASSERT_TRUE(lifecycle_mgr_.cleanup(std_msec(2000), std_msec(2000)).empty()); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED , lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_1")); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED , lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_2")); - - RCLCPP_INFO(node->get_logger(), "Using requested state methods"); - - // Bring to active via requested state - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_mgr_.transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "test_lifecycle_node_1", std_msec(2000), std_msec(2000))); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_1")); - - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_mgr_.transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "test_lifecycle_node_2", std_msec(2000), std_msec(2000))); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_2")); - - // Bring to unconfigured via requested state - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_mgr_.transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, "test_lifecycle_node_1", std_msec(2000), std_msec(2000))); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_1")); - - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_mgr_.transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, "test_lifecycle_node_2", std_msec(2000), std_msec(2000))); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_2")); - - RCLCPP_INFO(node->get_logger(), "Done with requested state methods"); - - // Bring back to active then shutdown via transitions - ASSERT_TRUE(lifecycle_mgr_.configure(std_msec(2000), std_msec(2000)).empty()); - ASSERT_TRUE(lifecycle_mgr_.activate(std_msec(2000), std_msec(2000)).empty()); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE , lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_1")); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE , lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_2")); - - ASSERT_TRUE(lifecycle_mgr_.shutdown(std_msec(2000), std_msec(2000)).empty()); - ASSERT_TRUE(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED == lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_1") - || lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN == lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_1")); - ASSERT_TRUE(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED == lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_2") - || lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN == lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_2")); - - promise_test_result.set_value(true); - }); + std::ignore = rcutils_logging_set_logger_level( + node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + + ros2_lifecycle_manager::Ros2LifecycleManager lifecycle_mgr_( // Construct lifecycle manager + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_logging_interface(), + node->get_node_services_interface()); + + // Test set_managed_nodes + lifecycle_mgr_.set_managed_nodes({"test_lifecycle_node_1", "test_lifecycle_node_2"}); + + std::this_thread::sleep_for(2000ms); + + // Test get_managed_nodes + auto managed_nodes = lifecycle_mgr_.get_managed_nodes(); RCLCPP_INFO(node->get_logger(), "Spinning"); - rclcpp::spin_until_future_complete(node, future_test_result); - test_thread.join(); // Ensure thread closes + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + auto spin_future{std::async(std::launch::async, [&executor] + { executor.spin(); })}; + + ASSERT_EQ(static_cast(2U), managed_nodes.size()); + ASSERT_EQ(0, managed_nodes[0].compare("test_lifecycle_node_1")); + ASSERT_EQ(0, managed_nodes[1].compare("test_lifecycle_node_2")); - + // Test get managed node states + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_1")); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_2")); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_mgr_.get_managed_node_state("unknown_node")); + + // Test Configure + ASSERT_TRUE(lifecycle_mgr_.configure(2000ms, 2000ms).empty()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_1")); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_2")); + + // Test Activate + ASSERT_TRUE(lifecycle_mgr_.activate(2000ms, 2000ms).empty()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_1")); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_2")); + + // Test Deactivate + ASSERT_TRUE(lifecycle_mgr_.deactivate(2000ms, 2000ms).empty()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_1")); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_2")); + + // Test cleanup + ASSERT_TRUE(lifecycle_mgr_.cleanup(2000ms, 2000ms).empty()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_1")); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_2")); + + RCLCPP_INFO(node->get_logger(), "Using requested state methods"); + + // Bring to active via requested state + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_mgr_.transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "test_lifecycle_node_1", 2000ms, 2000ms)); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_1")); + + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_mgr_.transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "test_lifecycle_node_2", 2000ms, 2000ms)); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_2")); + + // Bring to unconfigured via requested state + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_mgr_.transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, "test_lifecycle_node_1", 2000ms, 2000ms)); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_1")); + + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_mgr_.transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, "test_lifecycle_node_2", 2000ms, 2000ms)); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_2")); + + RCLCPP_INFO(node->get_logger(), "Done with requested state methods"); + + // Bring back to active then shutdown via transitions + ASSERT_TRUE(lifecycle_mgr_.configure(2000ms, 2000ms).empty()); + ASSERT_TRUE(lifecycle_mgr_.activate(2000ms, 2000ms).empty()); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_1")); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_2")); + + ASSERT_TRUE(lifecycle_mgr_.shutdown(2000ms, 2000ms).empty()); + ASSERT_TRUE(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED == lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_1") || lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN == lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_1")); + ASSERT_TRUE(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED == lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_2") || lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN == lifecycle_mgr_.get_managed_node_state("test_lifecycle_node_2")); + + executor.cancel(); } int main(int argc, char ** argv) @@ -139,8 +132,7 @@ int main(int argc, char ** argv) // initialize ROS rclcpp::init(argc, argv); - - bool success = RUN_ALL_TESTS(); + bool success = RUN_ALL_TESTS(); // shutdown ROS rclcpp::shutdown(); diff --git a/trajectory_utils/src/trajectory_utils/conversions/conversions.cpp b/trajectory_utils/src/trajectory_utils/conversions/conversions.cpp index febf1d2e..76de67aa 100644 --- a/trajectory_utils/src/trajectory_utils/conversions/conversions.cpp +++ b/trajectory_utils/src/trajectory_utils/conversions/conversions.cpp @@ -15,7 +15,6 @@ */ #include -#include #include #include #include diff --git a/trajectory_utils/src/trajectory_utils/quintic_coefficient_calculator.cpp b/trajectory_utils/src/trajectory_utils/quintic_coefficient_calculator.cpp index fe70f40a..b9180372 100644 --- a/trajectory_utils/src/trajectory_utils/quintic_coefficient_calculator.cpp +++ b/trajectory_utils/src/trajectory_utils/quintic_coefficient_calculator.cpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include namespace quintic_coefficient_calculator { diff --git a/trajectory_utils/src/trajectory_utils/trajectory_utils.cpp b/trajectory_utils/src/trajectory_utils/trajectory_utils.cpp index 9fd8e2ac..1030eeef 100644 --- a/trajectory_utils/src/trajectory_utils/trajectory_utils.cpp +++ b/trajectory_utils/src/trajectory_utils/trajectory_utils.cpp @@ -16,7 +16,6 @@ #include #include -#include #include #include #include diff --git a/trajectory_utils_ros2/src/trajectory_utils/conversions/conversions.cpp b/trajectory_utils_ros2/src/trajectory_utils/conversions/conversions.cpp index adbf061c..8185b2bb 100644 --- a/trajectory_utils_ros2/src/trajectory_utils/conversions/conversions.cpp +++ b/trajectory_utils_ros2/src/trajectory_utils/conversions/conversions.cpp @@ -14,7 +14,6 @@ * the License. */ -#include #include #include diff --git a/trajectory_utils_ros2/src/trajectory_utils/quintic_coefficient_calculator.cpp b/trajectory_utils_ros2/src/trajectory_utils/quintic_coefficient_calculator.cpp index da79476b..5536d9b9 100644 --- a/trajectory_utils_ros2/src/trajectory_utils/quintic_coefficient_calculator.cpp +++ b/trajectory_utils_ros2/src/trajectory_utils/quintic_coefficient_calculator.cpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include namespace quintic_coefficient_calculator { diff --git a/trajectory_utils_ros2/src/trajectory_utils/trajectory_utils.cpp b/trajectory_utils_ros2/src/trajectory_utils/trajectory_utils.cpp index d6874d56..a083b545 100644 --- a/trajectory_utils_ros2/src/trajectory_utils/trajectory_utils.cpp +++ b/trajectory_utils_ros2/src/trajectory_utils/trajectory_utils.cpp @@ -16,7 +16,6 @@ #include #include -#include #include #include #include diff --git a/uncertainty_tools/src/uncertainty_tools/uncertainty_tools.cpp b/uncertainty_tools/src/uncertainty_tools/uncertainty_tools.cpp index 82071072..6a984fe3 100644 --- a/uncertainty_tools/src/uncertainty_tools/uncertainty_tools.cpp +++ b/uncertainty_tools/src/uncertainty_tools/uncertainty_tools.cpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include