diff --git a/CMakeLists.txt b/CMakeLists.txt index bd1a0d96..855be57e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,116 +1,47 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(am_super) -## Compile as C++11, supported in ROS melodic and newer -add_compile_options(-std=c++17) +set(CMAKE_BUILD_TYPE Debug) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED - am_utils - diagnostic_msgs - rosbag - roscpp - rospy - std_msgs - std_srvs - am_rostest -) +if(POLICY CMP0074) + cmake_policy(SET CMP0074 NEW) +endif() + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() -#find_package(CUDA) - -#if(CUDA_FOUND) -# add_definitions(-DCUDA_FLAG) -# message([STATUS] "CUDA FOUND, am_super will be built with GPU monitoring") -# SET(CUDA_NVCC_FLAGS "-arch=sm_62" CACHE STRING "nvcc flags" FORCE) -# SET(CUDA_VERBOSE_BUILD ON CACHE BOOL "nvcc verbose" FORCE) -# CUDA_ADD_LIBRARY(cuda_utility ${LIB_TYPE} src/cuda/cuda_utility.cu) -#endif(CUDA_FOUND) - - -## System dependencies are found with CMake's conventions - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include - LIBRARIES super_lib - CATKIN_DEPENDS + +set(dependencies am_utils + brain_box_msgs + builtin_interfaces diagnostic_msgs - rosbag - roscpp - rospy + diagnostic_updater + rclcpp + rclpy + rosbag2 + rosbag2_cpp std_msgs - std_srvs ) -########### -## Build ## -########### +# find dependencies +find_package(ament_cmake REQUIRED) + +foreach(Dependency IN ITEMS ${dependencies}) + find_package(${Dependency} REQUIRED) +endforeach() -## Specify additional locations of header files -## Your package locations should be listed before other locations include_directories( - include - ${catkin_INCLUDE_DIRS} + include ) - file(GLOB super_lib_cpp_files src/super_lib/*.cpp ) @@ -127,119 +58,69 @@ file(GLOB SUPER_MEDIATOR_FILES src/am_super/*_mediator.cpp ) -## Declare a C++ library add_library(super_lib ${super_lib_cpp_files} ) -target_link_libraries(super_lib ${catkin_LIBRARIES}) -add_dependencies(super_lib ${catkin_EXPORTED_TARGETS}) +target_link_libraries(super_lib) -#if(CUDA_FOUND) -# add_executable(am_super ${am_super_cpp_files} ${cuda_cpp_files} ) -# target_link_libraries(am_super ${catkin_LIBRARIES} super_lib cuda_utility) -#else() -# add_executable(am_super ${am_super_cpp_files}) -# target_link_libraries(am_super ${catkin_LIBRARIES} super_lib) -#endif(CUDA_FOUND) +ament_target_dependencies(super_lib ${dependencies}) +ament_export_targets(super_libTargets HAS_LIBRARY_TARGET) +ament_export_include_directories(include) +ament_export_libraries(super_lib) add_executable(am_super ${am_super_cpp_files}) -target_link_libraries(am_super ${catkin_LIBRARIES} super_lib) -add_dependencies(am_super ${catkin_EXPORTED_TARGETS}) - -############# -## Install ## -############# -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html - install(TARGETS ${PROJECT_NAME} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) - -# Mark libraries for installation -# See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html - install(TARGETS super_lib - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} - ) - -#Mark cpp header files for installation -install(DIRECTORY include/super_lib - DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -if(CATKIN_ENABLE_TESTING AND ENABLE_COVERAGE_TESTING) - find_package(code_coverage REQUIRED) - # Add compiler flags for coverage instrumentation before defining any targets - APPEND_COVERAGE_COMPILER_FLAGS() -endif() +target_link_libraries(am_super super_lib) +ament_target_dependencies(am_super ${dependencies}) + +install(DIRECTORY include/ + DESTINATION include/ +) + +install( + TARGETS super_lib + EXPORT super_libTargets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +install(TARGETS + am_super + DESTINATION lib/${PROJECT_NAME} +) -file(GLOB TEST_FILES - test/*_tests.cpp +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ ) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + find_package(ros_testing REQUIRED) + + # the following lines skip linters + set(ament_cmake_cppcheck_FOUND TRUE) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + set(ament_cmake_flake8_FOUND TRUE) + set(ament_cmake_uncrustify_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + # Run all lint tests in package.xml except those listed above + ament_lint_auto_find_test_dependencies() -# code coverage setup: https://github.com/mikeferguson/code_coverage -if (CATKIN_ENABLE_TESTING) - - ## Add gtest based cpp test target and link libraries - - catkin_add_gtest(${PROJECT_NAME}_test ${TEST_FILES} ${SUPER_MEDIATOR_FILES}) - - if(TARGET ${PROJECT_NAME}_test) - target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES} super_lib) - endif() - - # For ros testing - find_package(rostest REQUIRED) - - #follow test naming convention of files in a folder and the cpp and launch have the test name with _rostest - set(TEST_NAMES abort_to_disarming - abort_to_manual - armed_to_ready - auto_to_abort - auto_to_manual - auto_to_semiauto - error_configure_tolerance - error_forced - error_status - error_status_without_stats - error_terminal_before_config - error_tolerant_before_config - hz_config - manual_to_disarming - param - platform_app_required_fail - platform_app_required_pass - platform_required_fail - platform_required_pass - primary - ready_to_shutdown - semi_auto_to_manual + ament_add_gtest_executable(abort_to_disarming + super_test/abort_to_disarming/abort_to_disarming_rostest.cpp ) - foreach(TEST_NAME ${TEST_NAMES}) - set(PROJECT_TEST_NAME ${TEST_NAME}_${PROJECT_NAME}_rostest) - add_rostest_gtest(${PROJECT_TEST_NAME} rostest/${TEST_NAME}/${TEST_NAME}_rostest.test rostest/${TEST_NAME}/${TEST_NAME}_rostest.cpp) - target_link_libraries(${PROJECT_TEST_NAME} ${catkin_LIBRARIES} super_lib) - endforeach() -endif() \ No newline at end of file + + target_link_libraries(abort_to_disarming super_lib) + ament_target_dependencies(abort_to_disarming am_utils ${dependencies}) + add_ros_test(super_test/abort_to_disarming/launch/abort_to_disarming.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") +endif() + +ament_export_include_directories(include) +ament_export_libraries(super_lib) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/CMakeLists_ros1.txt b/CMakeLists_ros1.txt new file mode 100644 index 00000000..bd1a0d96 --- /dev/null +++ b/CMakeLists_ros1.txt @@ -0,0 +1,245 @@ +cmake_minimum_required(VERSION 2.8.3) +project(am_super) + +## Compile as C++11, supported in ROS melodic and newer +add_compile_options(-std=c++17) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED + am_utils + diagnostic_msgs + rosbag + roscpp + rospy + std_msgs + std_srvs + am_rostest +) + +#find_package(CUDA) + +#if(CUDA_FOUND) +# add_definitions(-DCUDA_FLAG) +# message([STATUS] "CUDA FOUND, am_super will be built with GPU monitoring") +# SET(CUDA_NVCC_FLAGS "-arch=sm_62" CACHE STRING "nvcc flags" FORCE) +# SET(CUDA_VERBOSE_BUILD ON CACHE BOOL "nvcc verbose" FORCE) +# CUDA_ADD_LIBRARY(cuda_utility ${LIB_TYPE} src/cuda/cuda_utility.cu) +#endif(CUDA_FOUND) + + +## System dependencies are found with CMake's conventions + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES super_lib + CATKIN_DEPENDS + am_utils + diagnostic_msgs + rosbag + roscpp + rospy + std_msgs + std_srvs +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + + +file(GLOB super_lib_cpp_files + src/super_lib/*.cpp +) + +file(GLOB cuda_cpp_files + src/cuda/*.cpp +) + +file(GLOB am_super_cpp_files + src/am_super/*.cpp +) + +file(GLOB SUPER_MEDIATOR_FILES + src/am_super/*_mediator.cpp +) + +## Declare a C++ library +add_library(super_lib + ${super_lib_cpp_files} +) +target_link_libraries(super_lib ${catkin_LIBRARIES}) +add_dependencies(super_lib ${catkin_EXPORTED_TARGETS}) + +#if(CUDA_FOUND) +# add_executable(am_super ${am_super_cpp_files} ${cuda_cpp_files} ) +# target_link_libraries(am_super ${catkin_LIBRARIES} super_lib cuda_utility) +#else() +# add_executable(am_super ${am_super_cpp_files}) +# target_link_libraries(am_super ${catkin_LIBRARIES} super_lib) +#endif(CUDA_FOUND) + +add_executable(am_super ${am_super_cpp_files}) +target_link_libraries(am_super ${catkin_LIBRARIES} super_lib) +add_dependencies(am_super ${catkin_EXPORTED_TARGETS}) + +############# +## Install ## +############# +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html + install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + +# Mark libraries for installation +# See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html + install(TARGETS super_lib + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) + +#Mark cpp header files for installation +install(DIRECTORY include/super_lib + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +if(CATKIN_ENABLE_TESTING AND ENABLE_COVERAGE_TESTING) + find_package(code_coverage REQUIRED) + # Add compiler flags for coverage instrumentation before defining any targets + APPEND_COVERAGE_COMPILER_FLAGS() +endif() + +file(GLOB TEST_FILES + test/*_tests.cpp +) + + +# code coverage setup: https://github.com/mikeferguson/code_coverage +if (CATKIN_ENABLE_TESTING) + + ## Add gtest based cpp test target and link libraries + + catkin_add_gtest(${PROJECT_NAME}_test ${TEST_FILES} ${SUPER_MEDIATOR_FILES}) + + if(TARGET ${PROJECT_NAME}_test) + target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES} super_lib) + endif() + + # For ros testing + find_package(rostest REQUIRED) + + #follow test naming convention of files in a folder and the cpp and launch have the test name with _rostest + set(TEST_NAMES abort_to_disarming + abort_to_manual + armed_to_ready + auto_to_abort + auto_to_manual + auto_to_semiauto + error_configure_tolerance + error_forced + error_status + error_status_without_stats + error_terminal_before_config + error_tolerant_before_config + hz_config + manual_to_disarming + param + platform_app_required_fail + platform_app_required_pass + platform_required_fail + platform_required_pass + primary + ready_to_shutdown + semi_auto_to_manual + ) + foreach(TEST_NAME ${TEST_NAMES}) + set(PROJECT_TEST_NAME ${TEST_NAME}_${PROJECT_NAME}_rostest) + add_rostest_gtest(${PROJECT_TEST_NAME} rostest/${TEST_NAME}/${TEST_NAME}_rostest.test rostest/${TEST_NAME}/${TEST_NAME}_rostest.cpp) + target_link_libraries(${PROJECT_TEST_NAME} ${catkin_LIBRARIES} super_lib) + endforeach() +endif() \ No newline at end of file diff --git a/include/am_super/baby_sitter.h b/include/am_super/baby_sitter.h index ead1fe76..cf5d8745 100644 --- a/include/am_super/baby_sitter.h +++ b/include/am_super/baby_sitter.h @@ -3,16 +3,17 @@ #include #include -#include +#include -#include -#include +#include +#include #include #include +#include namespace am { -template +template class BabySitter { private: @@ -45,10 +46,10 @@ class BabySitter int curr_max_ms_; std::string node_name_; - ros::NodeHandle nh_; - ros::Subscriber device_data_sub_; - ros::Publisher node_status_pub_; - ros::Timer heartbeat_timer_; + rclcpp::Node::SharedPtr nh_; + rclcpp::GenericSubscription::SharedPtr device_data_sub_; + rclcpp::Publisher::SharedPtr node_status_pub_; + rclcpp::TimerBase::SharedPtr heartbeat_timer_; BagLogger* logger_; @@ -63,7 +64,7 @@ class BabySitter /* * Constructor with ros::NodeHandle */ - BabySitter(const ros::NodeHandle& nh, BagLogger* logger, const std::string& node_name, const std::string& topic, + BabySitter(rclcpp::Node::SharedPtr nh, BagLogger* logger, const std::string& node_name, const std::string& topic, int warn_ms, int error_ms, int warn_count_thresh = 5, int timeout_ms = 2000, int start_delay_ms = 0 /*, ErrorCB error_cb */); @@ -81,8 +82,8 @@ class BabySitter int getAveLatencyMs(); private: - void deviceCB(const ros::MessageEvent& event); - void heartbeatCB(const ros::TimerEvent& event); + void deviceCB(std::shared_ptr msg); + void heartbeatCB(); void checkNodeState(); void setNodeState(LifeCycleState node_state); std::string parseNodeState(LifeCycleState state); @@ -92,29 +93,30 @@ class BabySitter }; template -BabySitter::BabySitter(const ros::NodeHandle& nh, BagLogger* logger, const std::string& node_name, +BabySitter::BabySitter(const rclcpp::Node::SharedPtr nh, BagLogger* logger, const std::string& node_name, const std::string& topic, int warn_ms, int error_ms, int warn_count_thresh, int timeout_ms, - int start_delay_ms) + int start_delay_ms): nh_(nh) { - ROS_INFO_STREAM(NODE_FUNC << node_name); + RCLCPP_INFO_STREAM(nh_->get_logger(), nh_->get_name() << node_name); nh_ = nh; std::string parm = "~" + node_name + "/warn_ms"; - ros::param::param(parm, warn_ms_, warn_ms); - ROS_INFO_STREAM(NODE_FUNC << parm << " = " << warn_ms_); + + am::getParam(nh_, parm, warn_ms_, warn_ms); + RCLCPP_INFO_STREAM(nh_->get_logger(), nh_->get_name() << parm << " = " << warn_ms_); parm = "~" + node_name + "/error_ms"; - ros::param::param(parm, error_ms_, error_ms); - ROS_INFO_STREAM(NODE_FUNC << parm << " = " << error_ms_); + am::getParam(nh_,parm, error_ms_, error_ms); + RCLCPP_INFO_STREAM(nh_->get_logger(), nh_->get_name() << parm << " = " << error_ms_); parm = "~" + node_name + "/warn_count_thresh"; - ros::param::param(parm, warn_count_thresh_, warn_count_thresh); - ROS_INFO_STREAM(NODE_FUNC << parm << " = " << warn_count_thresh_); + am::getParam(nh_,parm, warn_count_thresh_, warn_count_thresh); + RCLCPP_INFO_STREAM(nh_->get_logger(), nh_->get_name() << parm << " = " << warn_count_thresh_); parm = "~" + node_name + "/timeout_ms"; - ros::param::param(parm, timeout_ms_, timeout_ms); - ROS_INFO_STREAM(NODE_FUNC << parm << " = " << timeout_ms_); + am::getParam(nh_,parm, timeout_ms_, timeout_ms); + RCLCPP_INFO_STREAM(nh_->get_logger(), nh_->get_name() << parm << " = " << timeout_ms_); min_ms_ = 1000; max_ms_ = 0; @@ -135,11 +137,12 @@ BabySitter::BabySitter(const ros::NodeHandle& nh, BagLogger* logger, const st start_delay_ms_ = start_delay_ms; logger_ = logger; - node_status_pub_ = nh_.advertise("/process/status", 1000); - device_data_sub_ = nh_.subscribe(topic, 10, &BabySitter::deviceCB, this); + node_status_pub_ = nh_->create_publisher("/process/status", 1000); - heartbeat_timer_ = nh_.createTimer(ros::Duration(1.0), &BabySitter::heartbeatCB, this); + device_data_sub_ = nh_->create_generic_subscription(topic, am::getMessageName(), 10, std::bind(&BabySitter::deviceCB, this, std::placeholders::_1)); + + heartbeat_timer_ = nh_->create_wall_timer(am::toDuration(1.0), std::bind(&BabySitter::heartbeatCB, this)); } template @@ -211,7 +214,7 @@ std::string BabySitter::parseDeviceState(DeviceState state) template void BabySitter::printStatus() { - ROS_INFO_STREAM(NODE_FUNC << node_name_ << ", node state:" << parseNodeState(node_state_) + RCLCPP_INFO_STREAM(nh_->get_logger(), nh_->get_name() << node_name_ << ", node state:" << parseNodeState(node_state_) << ", device state: " << parseDeviceState(device_state_)); } @@ -233,7 +236,7 @@ void BabySitter::checkNodeState() } break; default: - ROS_WARN_STREAM_THROTTLE(10, NODE_FUNC << node_name_ << ": unknown node state: " << parseNodeState(node_state_)); + RCLCPP_WARN_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 10, nh_->get_name() << node_name_ << ": unknown node state: " << parseNodeState(node_state_)); break; } } @@ -241,7 +244,7 @@ void BabySitter::checkNodeState() template void BabySitter::setNodeState(LifeCycleState node_state) { - ROS_INFO_STREAM(NODE_FUNC << node_name_ << ": changing state from: " << parseNodeState(node_state_) + RCLCPP_INFO_STREAM(nh_->get_logger(), nh_->get_name() << node_name_ << ": changing state from: " << parseNodeState(node_state_) << " to: " << parseNodeState(node_state)); switch (node_state_) @@ -253,20 +256,20 @@ void BabySitter::setNodeState(LifeCycleState node_state) node_state_ = node_state; break; default: - ROS_WARN_STREAM_THROTTLE(10, NODE_FUNC << node_name_ << ": unknown node state: " << parseNodeState(node_state_)); + RCLCPP_WARN_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 10, nh_->get_name() << node_name_ << ": unknown node state: " << parseNodeState(node_state_)); break; } printStatus(); } template -void BabySitter::deviceCB(const ros::MessageEvent& event) +void BabySitter::deviceCB(std::shared_ptr msg) { message_count_++; long now_ms = nowMS(); if (now_ms - start_time_ms_ < start_delay_ms_) { - ROS_WARN_STREAM_THROTTLE(1.0, NODE_FUNC << node_name_ << ":message received during start delay"); + RCLCPP_WARN_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 1.0, nh_->get_name() << node_name_ << ":message received during start delay"); } long latency_ms = now_ms - last_contact_ms_; @@ -282,35 +285,35 @@ void BabySitter::deviceCB(const ros::MessageEvent& event) } if (latency_ms < curr_min_ms_) { - curr_min_ms_ = latency_ms; + curr_min_ms_ = latency_ms; } if (latency_ms >= error_ms_) { - ROS_ERROR_STREAM_THROTTLE(1.0, NODE_FUNC << node_name_ << ": max latency error: " << latency_ms << "(" << error_ms_ - << ")"); - device_state_ = DeviceState::ERROR; + RCLCPP_ERROR_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 1.0, nh_->get_name()<< node_name_ << ": max latency error: " << latency_ms << "(" << error_ms_ + << ")"); + device_state_ = DeviceState::ERROR; } else if (latency_ms >= warn_ms_) { - ROS_WARN_STREAM_THROTTLE(1.0, NODE_FUNC << node_name_ << ": latency warning: " << latency_ms << "(" << warn_ms_ - << ")"); - device_state_ = DeviceState::WARN; - warn_count_++; - if (warn_count_ >= warn_count_thresh_) - { - ROS_ERROR_STREAM_THROTTLE(1.0, NODE_FUNC << node_name_ << ": count latency error: " << warn_count_ << "(" - << warn_count_thresh_ << ")"); - device_state_ = DeviceState::ERROR; - } + RCLCPP_WARN_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 1.0, nh_->get_name() << node_name_ << ": latency warning: " << latency_ms << "(" << warn_ms_ + << ")"); + device_state_ = DeviceState::WARN; + warn_count_++; + if (warn_count_ >= warn_count_thresh_) + { + RCLCPP_ERROR_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 1.0, nh_->get_name() << node_name_ << ": count latency error: " << warn_count_ << "(" + << warn_count_thresh_ << ")"); + device_state_ = DeviceState::ERROR; + } } else { - if (device_state_ != DeviceState::OK) - { - ROS_INFO_STREAM_THROTTLE(1.0, NODE_FUNC << node_name_ << ": latency ok: " << latency_ms); - device_state_ = DeviceState::OK; - } - warn_count_ = 0; + if (device_state_ != DeviceState::OK) + { + RCLCPP_INFO_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 1.0, nh_->get_name() << node_name_ << ": latency ok: " << latency_ms); + device_state_ = DeviceState::OK; + } + warn_count_ = 0; } last_contact_ms_ = now_ms; @@ -320,13 +323,13 @@ void BabySitter::deviceCB(const ros::MessageEvent& event) template long BabySitter::nowMS() { - ros::Time now = ros::Time().now(); - long now_ms = (long)(now.nsec / NSECS_IN_MSECS) + (long)now.sec * MSECS_IN_SECS; + rclcpp::Time now = nh_->now(); + long now_ms = (long)(now.seconds() / NSECS_IN_MSECS) + (long)now.seconds() * MSECS_IN_SECS; return now_ms; } template -void BabySitter::heartbeatCB(const ros::TimerEvent& event) +void BabySitter::heartbeatCB() { min_ms_ = curr_min_ms_; curr_min_ms_ = 1000; @@ -343,20 +346,20 @@ void BabySitter::heartbeatCB(const ros::TimerEvent& event) freq_hz_ = message_count_; message_count_ = 0; - brain_box_msgs::BabySitterStatus log_msg; + brain_box_msgs::msg::BabySitterStatus log_msg; log_msg.name = node_name_; log_msg.freq = freq_hz_; log_msg.max_min_ave.max = max_ms_; log_msg.max_min_ave.min = min_ms_; log_msg.max_min_ave.ave = ave_ms_; - LOG_MSG("/status/super/" + node_name_, log_msg, 1); + LOG_MSG(std::string("/status/super/" + node_name_), log_msg, 1); if (node_state_ == LifeCycleState::ACTIVE) { int time_since_contact = nowMS() - last_contact_ms_; if (time_since_contact > timeout_ms_) { - ROS_ERROR_STREAM(NODE_FUNC << node_name_ << ": timed out"); + RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() << node_name_ << ": timed out"); device_state_ = DeviceState::ERROR; checkNodeState(); } @@ -364,15 +367,15 @@ void BabySitter::heartbeatCB(const ros::TimerEvent& event) if (node_state_ == LifeCycleState::ACTIVE) { - brain_box_msgs::NodeStatus ns_msg; + brain_box_msgs::msg::NodeStatus ns_msg; ns_msg.node_name = node_name_; ns_msg.status = "ALIVE"; ns_msg.value = ""; ns_msg.process_id = 0; - node_status_pub_.publish(ns_msg); + node_status_pub_->publish(ns_msg); } - ROS_INFO_STREAM_THROTTLE(LOG_PERIOD_S, NODE_FUNC << node_name_ << " node:" << parseNodeState(node_state_) + RCLCPP_INFO_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), LOG_PERIOD_S, nh_->get_name() << node_name_ << " node:" << parseNodeState(node_state_) << ", state: " << parseDeviceState(device_state_) << ", max:" << max_ms_ << ", min: " << min_ms_ << ", ave: " << ave_ms_ << ", freq: " << freq_hz_); diff --git a/include/am_super/controller_state.h b/include/am_super/controller_state.h index 76c65625..c6b0d24f 100644 --- a/include/am_super/controller_state.h +++ b/include/am_super/controller_state.h @@ -1,14 +1,14 @@ #ifndef AM_SUPER_INCLUDE_CONTROLLER_STATE_H_ #define AM_SUPER_INCLUDE_CONTROLLER_STATE_H_ -#include +#include /** State of flight controller * https://automodality.atlassian.net/wiki/spaces/AMROS/pages/929234949/AMROS+System+States */ enum class ControllerState : std::uint8_t { - COMPLETED = brain_box_msgs::ControllerState::COMPLETED + COMPLETED = brain_box_msgs::msg::ControllerState::COMPLETED }; -#endif \ No newline at end of file +#endif diff --git a/include/am_super/operator_command.h b/include/am_super/operator_command.h index f37702a8..ce7da34a 100644 --- a/include/am_super/operator_command.h +++ b/include/am_super/operator_command.h @@ -1,22 +1,22 @@ #ifndef AM_SUPER_INCLUDE_OPERATOR_COMMAND_H_ #define AM_SUPER_INCLUDE_OPERATOR_COMMAND_H_ -#include +#include /** Commands sent by the human operator to transition SuperStates through the standard flow. * https://automodality.atlassian.net/wiki/spaces/AMROS/pages/929234949/AMROS+System+States */ enum class OperatorCommand : std::uint8_t { - ARM = brain_box_msgs::OperatorCommand::ARM, - CANCEL = brain_box_msgs::OperatorCommand::CANCEL, - LAUNCH = brain_box_msgs::OperatorCommand::LAUNCH, - PAUSE= brain_box_msgs::OperatorCommand::PAUSE, - RESUME=brain_box_msgs::OperatorCommand::RESUME, - MANUAL=brain_box_msgs::OperatorCommand::MANUAL, - LANDED=brain_box_msgs::OperatorCommand::LANDED, - ABORT=brain_box_msgs::OperatorCommand::ABORT, - SHUTDOWN=brain_box_msgs::OperatorCommand::SHUTDOWN, + ARM = brain_box_msgs::msg::OperatorCommand::ARM, + CANCEL = brain_box_msgs::msg::OperatorCommand::CANCEL, + LAUNCH = brain_box_msgs::msg::OperatorCommand::LAUNCH, + PAUSE= brain_box_msgs::msg::OperatorCommand::PAUSE, + RESUME=brain_box_msgs::msg::OperatorCommand::RESUME, + MANUAL=brain_box_msgs::msg::OperatorCommand::MANUAL, + LANDED=brain_box_msgs::msg::OperatorCommand::LANDED, + ABORT=brain_box_msgs::msg::OperatorCommand::ABORT, + SHUTDOWN=brain_box_msgs::msg::OperatorCommand::SHUTDOWN, }; -#endif \ No newline at end of file +#endif diff --git a/include/am_super/super_node_mediator.h b/include/am_super/super_node_mediator.h index 1ef762f1..df0773b1 100644 --- a/include/am_super/super_node_mediator.h +++ b/include/am_super/super_node_mediator.h @@ -2,8 +2,8 @@ #ifndef AM_SUPER_INCLUDE_AM_SUPER_NODE_MEDIATOR_H_ #define AM_SUPER_INCLUDE_AM_SUPER_NODE_MEDIATOR_H_ -#include -#include +#include +#include #include #include @@ -13,6 +13,7 @@ #include #include #include +#include using namespace std; @@ -21,7 +22,7 @@ namespace am class SuperNodeMediator { public: - SuperNodeMediator(const std::string& node_name); + SuperNodeMediator(rclcpp::Node::SharedPtr node, const std::string& node_name); /** * Instructions Super receives from flight controller. @@ -44,7 +45,7 @@ class SuperNodeMediator LifeCycleStatus status; // node lifecycle status bool manifested; // nodes was in manfiest bool online; // node is online - ros::Time last_contact; // last time a message was received from the node + rclcpp::Time last_contact; // last time a message was received from the node }; /** @@ -331,6 +332,9 @@ class SuperNodeMediator std::string platformVariantToConfig(const PlatformVariant &variant); private: + + rclcpp::Node::SharedPtr node_; + /** name of supervisor node */ const std::string SUPER_NODE_NAME; diff --git a/include/am_super/super_state.h b/include/am_super/super_state.h index 8188da58..92b8349f 100644 --- a/include/am_super/super_state.h +++ b/include/am_super/super_state.h @@ -1,22 +1,22 @@ #ifndef AM_SUPER_INCLUDE_SUPER_STATE_H_ #define AM_SUPER_INCLUDE_SUPER_STATE_H_ -#include +#include enum class SuperState : std::uint8_t { - OFF = brain_box_msgs::VxState::OFF, - BOOTING = brain_box_msgs::VxState::BOOTING, - READY = brain_box_msgs::VxState::READY, - ARMING = brain_box_msgs::VxState::ARMING, - ARMED = brain_box_msgs::VxState::ARMED, - AUTO = brain_box_msgs::VxState::AUTO, - DISARMING = brain_box_msgs::VxState::DISARMING, - SEMI_AUTO = brain_box_msgs::VxState::SEMI_AUTO, - HOLD = brain_box_msgs::VxState::HOLD, - ABORT = brain_box_msgs::VxState::ABORT, - MANUAL = brain_box_msgs::VxState::MANUAL, - SHUTDOWN = brain_box_msgs::VxState::SHUTDOWN, + OFF = brain_box_msgs::msg::VxState::OFF, + BOOTING = brain_box_msgs::msg::VxState::BOOTING, + READY = brain_box_msgs::msg::VxState::READY, + ARMING = brain_box_msgs::msg::VxState::ARMING, + ARMED = brain_box_msgs::msg::VxState::ARMED, + AUTO = brain_box_msgs::msg::VxState::AUTO, + DISARMING = brain_box_msgs::msg::VxState::DISARMING, + SEMI_AUTO = brain_box_msgs::msg::VxState::SEMI_AUTO, + HOLD = brain_box_msgs::msg::VxState::HOLD, + ABORT = brain_box_msgs::msg::VxState::ABORT, + MANUAL = brain_box_msgs::msg::VxState::MANUAL, + SHUTDOWN = brain_box_msgs::msg::VxState::SHUTDOWN, }; #endif diff --git a/include/am_super/super_state_mediator.h b/include/am_super/super_state_mediator.h index 658eb9ab..894925ef 100644 --- a/include/am_super/super_state_mediator.h +++ b/include/am_super/super_state_mediator.h @@ -4,6 +4,7 @@ #define AM_SUPER_INCLUDE_AM_SUPER_STATE_MEDIATOR_H_ #include +#include namespace am { diff --git a/include/super_lib/am_life_cycle.h b/include/super_lib/am_life_cycle.h index d1fca67a..e8940afc 100644 --- a/include/super_lib/am_life_cycle.h +++ b/include/super_lib/am_life_cycle.h @@ -3,9 +3,9 @@ #include -#include +#include -#include +#include #include #include @@ -53,7 +53,7 @@ class AMLifeCycle /**The moment configuration is requested for this node. Used with * max_configure_seconds_ to allow startup error tolerance.*/ - ros::Time configure_start_time_; + rclcpp::Time configure_start_time_; void setState(const LifeCycleState state); @@ -85,15 +85,15 @@ class AMLifeCycle diagnostic_updater::Updater updater_; AMStatList stats_list_; - ros::NodeHandle nh_; - ros::Timer heartbeat_timer_; - ros::Publisher state_pub_; - ros::Subscriber lifecycle_sub_; + rclcpp::Node::SharedPtr node_; + rclcpp::TimerBase::SharedPtr heartbeat_timer_; + rclcpp::Publisher::SharedPtr state_pub_; + rclcpp::Subscription::SharedPtr lifecycle_sub_; /** * @brief Default constructor */ - AMLifeCycle(); + AMLifeCycle(rclcpp::Node::SharedPtr node); /** * @brief Virtual destructor @@ -226,9 +226,9 @@ class AMLifeCycle * Useful for checking health regularly, but not during * callbacks which can affect performance and be too granular */ - virtual void heartbeatCB(const ros::TimerEvent& event); + virtual void heartbeatCB(); - void lifecycleCB(const brain_box_msgs::LifeCycleCommand::ConstPtr msg); + void lifecycleCB(const brain_box_msgs::msg::LifeCycleCommand::SharedPtr msg); double getThrottleS() const; diff --git a/include/super_lib/am_life_cycle_types.h b/include/super_lib/am_life_cycle_types.h index a623891b..7d006d77 100644 --- a/include/super_lib/am_life_cycle_types.h +++ b/include/super_lib/am_life_cycle_types.h @@ -2,8 +2,8 @@ #define AM_SUPER_INCLUDE_SUPER_LIB_AM_LIFECYCLE_TYPES_H_ #include -#include -#include +#include +#include namespace am { @@ -12,17 +12,17 @@ namespace am */ enum class LifeCycleState : std::uint8_t { - INVALID = brain_box_msgs::LifeCycleState::STATE_INVALID, - UNCONFIGURED = brain_box_msgs::LifeCycleState::STATE_UNCONFIGURED, - INACTIVE = brain_box_msgs::LifeCycleState::STATE_INACTIVE, - ACTIVE = brain_box_msgs::LifeCycleState::STATE_ACTIVE, - FINALIZED = brain_box_msgs::LifeCycleState::STATE_FINALIZED, - CONFIGURING = brain_box_msgs::LifeCycleState::STATE_CONFIGURING, - CLEANING_UP = brain_box_msgs::LifeCycleState::STATE_CLEANING_UP, - SHUTTING_DOWN = brain_box_msgs::LifeCycleState::STATE_SHUTTING_DOWN, - ACTIVATING = brain_box_msgs::LifeCycleState::STATE_ACTIVATING, - DEACTIVATING = brain_box_msgs::LifeCycleState::STATE_DEACTIVATING, - ERROR_PROCESSING = brain_box_msgs::LifeCycleState::STATE_ERROR_PROCESSING + INVALID = brain_box_msgs::msg::LifeCycleState::STATE_INVALID, + UNCONFIGURED = brain_box_msgs::msg::LifeCycleState::STATE_UNCONFIGURED, + INACTIVE = brain_box_msgs::msg::LifeCycleState::STATE_INACTIVE, + ACTIVE = brain_box_msgs::msg::LifeCycleState::STATE_ACTIVE, + FINALIZED = brain_box_msgs::msg::LifeCycleState::STATE_FINALIZED, + CONFIGURING = brain_box_msgs::msg::LifeCycleState::STATE_CONFIGURING, + CLEANING_UP = brain_box_msgs::msg::LifeCycleState::STATE_CLEANING_UP, + SHUTTING_DOWN = brain_box_msgs::msg::LifeCycleState::STATE_SHUTTING_DOWN, + ACTIVATING = brain_box_msgs::msg::LifeCycleState::STATE_ACTIVATING, + DEACTIVATING = brain_box_msgs::msg::LifeCycleState::STATE_DEACTIVATING, + ERROR_PROCESSING = brain_box_msgs::msg::LifeCycleState::STATE_ERROR_PROCESSING }; /** @@ -30,9 +30,9 @@ enum class LifeCycleState : std::uint8_t */ enum class LifeCycleStatus : std::uint8_t { - OK = brain_box_msgs::LifeCycleState::STATUS_OK, - WARN = brain_box_msgs::LifeCycleState::STATUS_WARN, - ERROR = brain_box_msgs::LifeCycleState::STATUS_ERROR + OK = brain_box_msgs::msg::LifeCycleState::STATUS_OK, + WARN = brain_box_msgs::msg::LifeCycleState::STATUS_WARN, + ERROR = brain_box_msgs::msg::LifeCycleState::STATUS_ERROR }; /** @@ -40,16 +40,16 @@ enum class LifeCycleStatus : std::uint8_t */ enum class LifeCycleCommand : std::uint8_t { - CREATE = brain_box_msgs::LifeCycleCommand::COMMAND_CREATE, - CONFIGURE = brain_box_msgs::LifeCycleCommand::COMMAND_CONFIGURE, - CLEANUP = brain_box_msgs::LifeCycleCommand::COMMAND_CLEANUP, - ACTIVATE = brain_box_msgs::LifeCycleCommand::COMMAND_ACTIVATE, - DEACTIVATE = brain_box_msgs::LifeCycleCommand::COMMAND_DEACTIVATE, - SHUTDOWN = brain_box_msgs::LifeCycleCommand::COMMAND_SHUTDOWN, - DESTROY = brain_box_msgs::LifeCycleCommand::COMMAND_DESTROY, + CREATE = brain_box_msgs::msg::LifeCycleCommand::COMMAND_CREATE, + CONFIGURE = brain_box_msgs::msg::LifeCycleCommand::COMMAND_CONFIGURE, + CLEANUP = brain_box_msgs::msg::LifeCycleCommand::COMMAND_CLEANUP, + ACTIVATE = brain_box_msgs::msg::LifeCycleCommand::COMMAND_ACTIVATE, + DEACTIVATE = brain_box_msgs::msg::LifeCycleCommand::COMMAND_DEACTIVATE, + SHUTDOWN = brain_box_msgs::msg::LifeCycleCommand::COMMAND_SHUTDOWN, + DESTROY = brain_box_msgs::msg::LifeCycleCommand::COMMAND_DESTROY, //FIXME: there should be no last command - LAST_COMMAND = brain_box_msgs::LifeCycleCommand::COMMAND_LAST + LAST_COMMAND = brain_box_msgs::msg::LifeCycleCommand::COMMAND_LAST }; }; // namespace am diff --git a/include/super_lib/am_stat.h b/include/super_lib/am_stat.h index e1280304..1260f81d 100644 --- a/include/super_lib/am_stat.h +++ b/include/super_lib/am_stat.h @@ -4,10 +4,10 @@ #include #include -#include +#include #include -#include +#include namespace am { @@ -38,26 +38,26 @@ class AMStat bool validate_max_ = false; bool sample_received_ = false; bool sample_required_ = false; + rclcpp::Node::SharedPtr node_; private: AMStat(); - public: - AMStat(const std::string& short_name, const std::string& long_name) + AMStat(rclcpp::Node::SharedPtr node, const std::string& short_name, const std::string& long_name): node_(node) { short_name_ = short_name; long_name_ = long_name; } - AMStat(const std::string& short_name, const std::string& long_name, uint32_t max_warn, uint32_t max_error) - : AMStat(short_name, long_name) + AMStat(rclcpp::Node::SharedPtr node, const std::string& short_name, const std::string& long_name, uint32_t max_warn, uint32_t max_error) + : AMStat(node, short_name, long_name) { setMaxWarn(max_warn); setMaxError(max_error); } - AMStat(const std::string& short_name, const std::string& long_name, uint32_t min_error, uint32_t min_warn, + AMStat(rclcpp::Node::SharedPtr node, const std::string& short_name, const std::string& long_name, uint32_t min_error, uint32_t min_warn, uint32_t max_warn, uint32_t max_error) - : AMStat(short_name, long_name,max_warn,max_error) + : AMStat(node, short_name, long_name,max_warn,max_error) { setMinError(min_error); setMinWarn(min_warn); @@ -77,13 +77,13 @@ class AMStat { if (value_ > max_error_) { - ROS_ERROR_STREAM_THROTTLE(error_throttle_s, long_name_ << " exceeding max_error: " << value_ + RCLCPP_ERROR_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(),error_throttle_s, long_name_ << " exceeding max_error: " << value_ << " (max:" << max_error_ << ") [TF5R]"); compoundStatus(status, LifeCycleStatus::ERROR); } else if (value_ > max_warn_) { - ROS_WARN_STREAM_THROTTLE(warn_throttle_s, long_name_ << " exceeding max_warn: " << value_ << " (max:" << max_warn_ + RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), warn_throttle_s, long_name_ << " exceeding max_warn: " << value_ << " (max:" << max_warn_ << ") [PO9P]"); compoundStatus(status, LifeCycleStatus::WARN); } @@ -93,13 +93,13 @@ class AMStat { if (value_ < min_error_) { - ROS_ERROR_STREAM_THROTTLE(error_throttle_s, long_name_ << " exceeding min_error: " << value_ + RCLCPP_ERROR_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), error_throttle_s, long_name_ << " exceeding min_error: " << value_ << " (min:" << min_error_ << ") [K08K]"); compoundStatus(status, LifeCycleStatus::ERROR); } else if (value_ < min_warn_) { - ROS_WARN_STREAM_THROTTLE(warn_throttle_s, long_name_ << " exceeding min_warn: " << value_ << " (min:" << min_warn_ + RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), warn_throttle_s, long_name_ << " exceeding min_warn: " << value_ << " (min:" << min_warn_ << ") [H9H8]"); compoundStatus(status, LifeCycleStatus::WARN); } @@ -108,14 +108,14 @@ class AMStat if(!validate_max_ && !validate_min_) { //report this warning once during first validation - ROS_WARN_STREAM_THROTTLE(9999, long_name_ << " lacks validation since min/max is not set [TRB5]"); + RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 9999, long_name_ << " lacks validation since min/max is not set [TRB5]"); } } else { //sample is required and not yet received status = LifeCycleStatus::ERROR; - ROS_ERROR_STREAM_THROTTLE(error_throttle_s, long_name_ << " no samples received [NAQE] "); + RCLCPP_ERROR_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), error_throttle_s, long_name_ << " no samples received [NAQE] "); } return status; } diff --git a/include/super_lib/am_stat_list.h b/include/super_lib/am_stat_list.h index 97e69585..86c8d621 100644 --- a/include/super_lib/am_stat_list.h +++ b/include/super_lib/am_stat_list.h @@ -3,7 +3,7 @@ #include -#include +#include #include namespace am diff --git a/include/super_lib/am_stat_reset.h b/include/super_lib/am_stat_reset.h index 08097cd6..dea8827e 100644 --- a/include/super_lib/am_stat_reset.h +++ b/include/super_lib/am_stat_reset.h @@ -22,20 +22,20 @@ class AMStatReset : public AMStat sample_required_ = true; } public: - AMStatReset(const std::string& short_name, const std::string& long_name) : AMStat(short_name, long_name) + AMStatReset(rclcpp::Node::SharedPtr node, const std::string& short_name, const std::string& long_name) : AMStat(node, short_name, long_name) { init(); } - AMStatReset(const std::string& short_name, const std::string& long_name, uint32_t max_warn, uint32_t max_error) - : AMStat(short_name, long_name, max_warn, max_error) + AMStatReset(rclcpp::Node::SharedPtr node, const std::string& short_name, const std::string& long_name, uint32_t max_warn, uint32_t max_error) + : AMStat(node, short_name, long_name, max_warn, max_error) { init(); } - AMStatReset(const std::string& short_name, const std::string& long_name, uint32_t min_error, uint32_t min_warn, + AMStatReset(rclcpp::Node::SharedPtr node, const std::string& short_name, const std::string& long_name, uint32_t min_error, uint32_t min_warn, uint32_t max_warn, uint32_t max_error) - : AMStat(short_name, long_name,min_error,min_warn, max_warn, max_error) + : AMStat(node, short_name, long_name,min_error,min_warn, max_warn, max_error) { init(); } diff --git a/include/super_lib/am_super_test.h b/include/super_lib/am_super_test.h new file mode 100644 index 00000000..ffd6a346 --- /dev/null +++ b/include/super_lib/am_super_test.h @@ -0,0 +1,160 @@ +#ifndef _AM_SUPER_SUPER_LIB_AM_SUPER_TEST_H_ +#define _AM_SUPER_SUPER_LIB_AM_SUPER_TEST_H_ + +#include + +#include + +#include // googletest header file + +#include // to be armed, launch for state transitions +#include +#include // msg for status +#include // msg for status +#include +#include +#include +#include +#include + +using namespace std; +using namespace am; + +/** + * Base class for testing the LifeCycle transitions of a target node along with the + * state transition of the supervisor node. + */ +class AMSuperTest : public ::testing::Test +{ +private: + rclcpp::Subscription::SharedPtr nodeLifeCycleStateSubscription_; + rclcpp::Subscription::SharedPtr missionStateSubscription_; + rclcpp::Publisher::SharedPtr operatorCommandPublisher_; + rclcpp::Publisher::SharedPtr controllerStatePublisher_; + std::multimap node_states_; + std::vector mission_states_; + + /** Act as the operator (typically via a remote or ground station) to send one + * signal to transition to proceed, cancel, abort, etc. + * + * @param command one of brain_box_msgs::OperatorCommand::ARM; + * + * @see arm() + * @see launch() + */ + void publishOperatorCommand(uint8_t command); + + /** Acting as the controller, this publishes the controller state + * to signal transitions due to autonomous processing + * @param state one of the brain_box_msgs::ControllerState state enums + * @see landed() + * */ + void publishControllerState(uint8_t state); + +public: + rclcpp::Node::SharedPtr nh_; + string target_node_name_; + + AMSuperTest(); + AMSuperTest(string target_node_name); + + /** when configured and Inactive, arm is sent to prepare all nodes for takeoff*/ + void arm(); + + /** when armed, signals for the props to spin and takeoff */ + void launch(); + + /** to transition super into disarming, flight must be completed by landing or canceled */ + void landed(); + + /** to transition super into disarming from armed */ + void cancel(); + + /** to transition super into semi-auto from auto */ + void pause(); + + /** to transition super into auto from semi-auto */ + void resume(); + + /** to transition super into abort while in flight mode */ + void abort(); + + /** to transition super into manual from an auto mode*/ + void manual(); + + /** to shut super down. Must be in BOOTING or READY */ + void shutdown(); + + + /** searches the node states matching the lifecycle given. + * + * @return true if the state is found for the node given, false otherwise + */ + bool nodeStateReceived(string node_name,LifeCycleState state); + + /** + * @return true if the desired state is anywhere in the list, regardless of order + */ + bool missionStateReceived(uint8_t mission_state); + + /** + * Callback sniffing the state of nodes, as if it were am_super, to see + * if the target node is transitioning as expected. + * + * Simply registers states in multimap for later inspection. + */ + void nodeLifeCycleStateCallback(const brain_box_msgs::msg::LifeCycleState& msg); + + void missionStateCallback(const brain_box_msgs::msg::VxState& msg); + + /** Loop until a am_super is broadcasting the desired state or until + * ros says its time to quit. + * FIXME: export SuperState into a library and use instead of the untyped messages. + * @param log_code to corrleate the log output to the source code + */ + void waitUntilMissionState(const uint8_t mssion_state, std::string log_code, float sleep = 1); + + [[deprecated("use waitUntilMissionState(status,log_code,sleep)")]] + void waitUntilMissionState(const uint8_t mssion_state, float sleep = 1); + + /** + * spin until the desired state is found or until the test times out. + */ + void waitUntil(const LifeCycleState state, float sleep = 1); + + /** + * @param log_code makes it easy to find the source of the log messages + */ + void waitUntil(const LifeCycleState state, const std::string log_code, float sleep = 1); + + + /** + * Wait until status is received or the test times out. + */ + [[deprecated("use waitUntil(status,log_code,sleep)")]] + void waitUntilStatus(const LifeCycleStatus& status, float sleep = 1); + + /** + * @brief look for status periodically, based on the sleep, or timeout based on the test time limit. + * @param log_code makes it easy to find the source of the log messages + */ + void waitUntil(const LifeCycleStatus& status, const std::string log_code,float sleep = 1); + + /** + * Looks to see if the given status has ever been received. + */ + bool nodeStatusReceived(string node_name, LifeCycleStatus status); + + /** + * Generate publishers and subscriptions. + */ + void createPubsSubs(); + +#define TEST_LOG( args, code) \ + do \ + { \ + RCLCPP_WARN_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 1.0, args << " [" << code << "]"); \ + } while (0) +}; + +#endif \ No newline at end of file diff --git a/launch/am_super.yaml b/launch/am_super.yaml index e4502276..e69de29b 100644 --- a/launch/am_super.yaml +++ b/launch/am_super.yaml @@ -1,3 +0,0 @@ - -platform: - actual: $(arg platform) \ No newline at end of file diff --git a/launch/am_super_launch.py b/launch/am_super_launch.py new file mode 100644 index 00000000..f0c4b456 --- /dev/null +++ b/launch/am_super_launch.py @@ -0,0 +1,21 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + + ld = LaunchDescription() + config = os.path.join( + get_package_share_directory('am_super'), + 'launch', + 'am_super.yaml' + ) + node=Node( + package = 'am_super', + name = 'am_super', + executable = 'am_super', + parameters = [config] + ) + ld.add_action(node) + return ld diff --git a/package.xml b/package.xml index 1baec1cf..98c3e4af 100644 --- a/package.xml +++ b/package.xml @@ -1,26 +1,32 @@ - + + am_super 0.0.0 - AutoModality Supervisor - AutoModality + TODO: Package description + alireza + TODO: License declaration + ament_cmake + + ros2launch - - - - TODO - - catkin am_utils brain_box_msgs + builtin_interfaces diagnostic_msgs - rosbag - roscpp - rospy + diagnostic_updater + rosbag2 + rosbag2_cpp + rclcpp + rclpy std_msgs std_srvs - rosunit - code_coverage - am_rostest + + ament_lint_auto + ament_lint_common + + + ament_cmake + diff --git a/package_ros1.xml b/package_ros1.xml new file mode 100644 index 00000000..1baec1cf --- /dev/null +++ b/package_ros1.xml @@ -0,0 +1,26 @@ + + + am_super + 0.0.0 + AutoModality Supervisor + AutoModality + + + + + + TODO + + catkin + am_utils + brain_box_msgs + diagnostic_msgs + rosbag + roscpp + rospy + std_msgs + std_srvs + rosunit + code_coverage + am_rostest + diff --git a/rostest/abort_to_disarming/abort_to_disarming_rostest.cpp b/rostest/abort_to_disarming/abort_to_disarming_rostest.cpp deleted file mode 100644 index 8c6b7126..00000000 --- a/rostest/abort_to_disarming/abort_to_disarming_rostest.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include - -class AbortToDisarming : public RostestBase, am::AMLifeCycle -{ -protected: - AbortToDisarming() : RostestBase() {} -}; - -TEST_F(AbortToDisarming, testState_AbortToDisarming) -{ - waitUntilMissionState(brain_box_msgs::VxState::READY,"N3DJ"); - arm(); - waitUntilMissionState(brain_box_msgs::VxState::ARMED,"XX3X"); - launch(); - waitUntilMissionState(brain_box_msgs::VxState::AUTO,"YYUI"); - abort(); - waitUntilMissionState(brain_box_msgs::VxState::ABORT,"NSKE"); - landed(); - waitUntilMissionState(brain_box_msgs::VxState::DISARMING,"XXCV"); -} - -int main(int argc, char** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, ros::this_node::getName()); - return RUN_ALL_TESTS(); -} \ No newline at end of file diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index 1fa3de2d..e2708911 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -1,28 +1,29 @@ #include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include @@ -35,6 +36,8 @@ #include #include #include +#include + #if CUDA_FLAG #include #endif @@ -59,26 +62,26 @@ class AMSuper : AMLifeCycle /** * the ros node handle */ - ros::NodeHandle nh_; + rclcpp::Node::SharedPtr nh_; /* * see constructor for details */ - ros::Publisher lifecycle_pub_; - ros::Publisher vstate_summary_pub_; - ros::Publisher system_state_pub_; - ros::Publisher super_status_pub_; - ros::Publisher led_pub_; + rclcpp::Publisher::SharedPtr lifecycle_pub_; + rclcpp::Publisher::SharedPtr vstate_summary_pub_; + rclcpp::Publisher::SharedPtr system_state_pub_; + rclcpp::Publisher::SharedPtr super_status_pub_; + rclcpp::Publisher::SharedPtr led_pub_; /** stops the flight plan when SHUTDOWN state */ - ros::Publisher flight_plan_deactivation_pub_; - ros::Subscriber node_state_sub_; - ros::Subscriber operator_command_sub_; - ros::Subscriber controller_state_sub; - ros::Subscriber diagnostics_sub; - ros::Subscriber current_enu_sub; - ros::Timer heartbeat_timer_; - - ros::Subscriber log_control_sub_; + rclcpp::Publisher::SharedPtr flight_plan_deactivation_pub_; + rclcpp::Subscription::SharedPtr node_state_sub_; + rclcpp::Subscription::SharedPtr operator_command_sub_; + rclcpp::Subscription::SharedPtr controller_state_sub; + rclcpp::Subscription::SharedPtr diagnostics_sub; + rclcpp::Subscription::SharedPtr current_enu_sub; + rclcpp::TimerBase::SharedPtr heartbeat_timer_; + + rclcpp::Subscription::SharedPtr log_control_sub_; BagLogger::BagLoggerLevel log_level_; /** manage logic for SuperState transitions */ @@ -108,13 +111,13 @@ class AMSuper : AMLifeCycle // const std::string NODE_BS_ALTIMETER = "can_node"; // TODO: replace with system global const - typedef brain_box_msgs::StampedAltimeter altimeter_bs_msg_type; + typedef brain_box_msgs::msg::StampedAltimeter altimeter_bs_msg_type; am::BabySitter* altimeter_bs_; const std::string ALTIMETER_BS_TOPIC = "/sensor/distance/agl_lw"; // TODO: replace with system global const const int ALTIMETER_HZ = 20; const std::string NODE_BS_DJI = "dji_sdk"; // TODO: replace with system global const - typedef sensor_msgs::Joy dji_bs_msg_type; + typedef sensor_msgs::msg::Joy dji_bs_msg_type; am::BabySitter* dji_bs_; const std::string DJI_BS_TOPIC = "/dji_sdk/rc"; // TODO: replace with system global const const int DJI_HZ = 50; @@ -124,12 +127,12 @@ class AMSuper : AMLifeCycle #endif public: - AMSuper() : nh_("~"), node_mediator_(SuperNodeMediator::nodeNameStripped(ros::this_node::getName())) + AMSuper(rclcpp::Node::SharedPtr nh) : nh_(nh), AMLifeCycle(nh), node_mediator_(nh, SuperNodeMediator::nodeNameStripped(nh->get_name())) { - ROS_INFO_STREAM(NODE_FUNC); + RCLCPP_INFO_STREAM(nh_->get_logger(), nh_->get_name()); - ros::param::param("~node_timeout_s", node_timeout_s_, 2.0); - ROS_INFO_STREAM("node_timeout_s = " << node_timeout_s_); + am::getParam(nh_, "node_timeout_s", node_timeout_s_, 2.0); + RCLCPP_INFO_STREAM(nh_->get_logger(), "node_timeout_s = " << node_timeout_s_); /* * create initial node list from manifest and create babysitters as needed @@ -137,7 +140,7 @@ class AMSuper : AMLifeCycle supervisor_.system_state = SuperState::OFF; // strip spaces from manifest param string manifest_param; - ros::param::param("~manifest", manifest_param, ""); + am::getParam(nh_, "manifest", manifest_param, ""); node_mediator_.parseManifest(supervisor_, manifest_param); @@ -147,13 +150,13 @@ class AMSuper : AMLifeCycle // if a manifest has been specified if (!supervisor_.manifest.empty()) { - ROS_INFO_STREAM("configuring nodes from manifest: " << manifest_param); + RCLCPP_INFO_STREAM(nh_->get_logger(), "configuring nodes from manifest: " << manifest_param); for (string& name : supervisor_.manifest) { // create a new node in the list for each name in manifest SuperNodeMediator::SuperNodeInfo nr = node_mediator_.initializeManifestedNode(name); supervisor_.nodes.insert(pair(name, nr)); - ROS_INFO_STREAM(" " << name); + RCLCPP_INFO_STREAM(nh_->get_logger(), " " << name); // create babysitters based on hard coded node names if (!name.compare(NODE_BS_ALTIMETER)) @@ -174,7 +177,7 @@ class AMSuper : AMLifeCycle } else { - ROS_WARN_STREAM("Manifest is empty. No nodes will be monitored."); + RCLCPP_WARN_STREAM(nh_->get_logger(), "Manifest is empty. No nodes will be monitored."); } reportSystemState(); @@ -187,22 +190,22 @@ class AMSuper : AMLifeCycle /** * system status pub */ - vstate_summary_pub_ = nh_.advertise(am_super_topics::SUPER_STATE, 1000); - system_state_pub_ = nh_.advertise(am_topics::SYSTEM_STATE, 1000); + vstate_summary_pub_ = nh_->create_publisher(am_super_topics::SUPER_STATE, 1000); + system_state_pub_ = nh_->create_publisher(am_topics::SYSTEM_STATE, 1000); /**Super * node lifecycle state pub. used to tell nodes to change their lifecycle state. */ - lifecycle_pub_ = nh_.advertise(am_super_topics::NODE_LIFECYCLE, 100); + lifecycle_pub_ = nh_->create_publisher(am_super_topics::NODE_LIFECYCLE, 100); /** * led control pub */ - led_pub_ = nh_.advertise(am::am_topics::LED_BLINK, 1000); + led_pub_ = nh_->create_publisher(am::am_topics::LED_BLINK, 1000); /** * super status contains online naode list for gcs_comms */ - super_status_pub_ = nh_.advertise(am_super_topics::SUPER_STATUS, 1000); + super_status_pub_ = nh_->create_publisher(am_super_topics::SUPER_STATUS, 1000); - flight_plan_deactivation_pub_ = nh_.advertise(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, 1000); + flight_plan_deactivation_pub_ = nh_->create_publisher(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, 1000); supervisor_.system_state = SuperState::BOOTING; supervisor_.flt_ctrl_state = SuperNodeMediator::SuperFltCtrlState::INIT; @@ -210,10 +213,11 @@ class AMSuper : AMLifeCycle /** * amros log control */ - log_control_sub_ = nh_.subscribe(am::am_topics::CTRL_LOG_CONTROL, 10, &AMSuper::logControlCB, this); + log_control_sub_ = nh_->create_subscription(am::am_topics::CTRL_LOG_CONTROL, 10, + std::bind(&AMSuper::logControlCB, this, std::placeholders::_1)); // startup bagfile - gets closed after frist log control command - ROS_INFO_STREAM("start logging to ST, level " << SU_LOG_LEVEL); + RCLCPP_INFO_STREAM(nh_->get_logger(), "start logging to ST, level " << SU_LOG_LEVEL); BagLogger::instance()->startLogging("ST", SU_LOG_LEVEL); log_level_ = intToLoggerLevel (SU_LOG_LEVEL); @@ -222,20 +226,25 @@ class AMSuper : AMLifeCycle /** * node status via LifeCycle */ - node_state_sub_ = nh_.subscribe(am_super_topics::LIFECYCLE_STATE, 100, &AMSuper::nodeStateCB, this); + node_state_sub_ = nh_->create_subscription(am_super_topics::LIFECYCLE_STATE, 100, + std::bind(&AMSuper::nodeStateCB, this, std::placeholders::_1)); /** * commands from operator */ - operator_command_sub_ = nh_.subscribe(am_super_topics::OPERATOR_COMMAND, 100, &AMSuper::operatorCommandCB, this); + operator_command_sub_ = nh_->create_subscription(am_super_topics::OPERATOR_COMMAND, 100, + std::bind(&AMSuper::operatorCommandCB, this, std::placeholders::_1)); - controller_state_sub = nh_.subscribe(am_super_topics::CONTROLLER_STATE, 100, &AMSuper::controllerStateCB, this); + controller_state_sub = nh_->create_subscription(am_super_topics::CONTROLLER_STATE, 100, + std::bind(&AMSuper::controllerStateCB, this, std::placeholders::_1)); - diagnostics_sub = nh_.subscribe("/diagnostics", 100, &AMSuper::diagnosticsCB, this); + diagnostics_sub = nh_->create_subscription("/diagnostics", 100, + std::bind(&AMSuper::diagnosticsCB, this, std::placeholders::_1)); - current_enu_sub = nh_.subscribe(am_topics::CTRL_VX_VEHICLE_CURRENTENU, 100, &AMSuper::currentENUCB, this); + current_enu_sub = nh_->create_subscription(am_topics::CTRL_VX_VEHICLE_CURRENTENU, 100, + std::bind(&AMSuper::currentENUCB, this, std::placeholders::_1)); - heartbeat_timer_ = nh_.createTimer(ros::Duration(1.0), &AMSuper::heartbeatCB, this); + heartbeat_timer_ = nh_->create_wall_timer(am::toDuration(1.0), std::bind(&AMSuper::heartbeatCB, this)); } ~AMSuper() @@ -253,9 +262,10 @@ class AMSuper : AMLifeCycle * LifeCycle messages are sent once a second by the LifeCycle heartbeat, but may * come more frequently if a node chooses. */ - void nodeStateCB(const ros::MessageEvent& event) + //void nodeStateCB(const rclcpp::MessageEvent& event) + void nodeStateCB(const brain_box_msgs::msg::LifeCycleState::SharedPtr rmsg) { - const brain_box_msgs::LifeCycleState::ConstPtr& rmsg = event.getMessage(); + //const brain_box_msgs::LifeCycleState::ConstPtr& rmsg = event.getMessage(); /* * process the message @@ -265,29 +275,31 @@ class AMSuper : AMLifeCycle * for the timeout. */ processState(rmsg->node_name, (LifeCycleState)(rmsg->state), (LifeCycleStatus)(rmsg->status), rmsg->subsystem, - rmsg->value, rmsg->process_id, event.getReceiptTime()); + rmsg->value, rmsg->process_id, rmsg->header.stamp); // TODO: topic name should come from vb_util_lib::topics.h - LOG_MSG(am_super_topics::LIFECYCLE_STATE, rmsg, SU_LOG_LEVEL); + LOG_MSG(am_super_topics::LIFECYCLE_STATE, *rmsg, SU_LOG_LEVEL); } - void controllerStateCB(const ros::MessageEvent& event) + //void controllerStateCB(const ros::MessageEvent& event) + void controllerStateCB(const brain_box_msgs::msg::ControllerState::SharedPtr rmsg) { - const brain_box_msgs::ControllerState::ConstPtr& rmsg = event.getMessage(); + //const brain_box_msgs::ControllerState::ConstPtr& rmsg = event.getMessage(); - ROS_INFO("Received Controller State: %s sent %i", rmsg->node_name.c_str(), rmsg->state); + RCLCPP_INFO(nh_->get_logger(), "Received Controller State: %s sent %i", rmsg->node_name.c_str(), rmsg->state); node_mediator_.setControllerState(supervisor_, (ControllerState)rmsg->state); } - void operatorCommandCB(const ros::MessageEvent& event) + //void operatorCommandCB(const ros::MessageEvent& event) + void operatorCommandCB(const brain_box_msgs::msg::OperatorCommand::SharedPtr rmsg) { - const brain_box_msgs::OperatorCommand::ConstPtr& rmsg = event.getMessage(); + //const brain_box_msgs::OperatorCommand::ConstPtr& rmsg = event.getMessage(); - ROS_INFO("Received Operator Command: %s sent '%i'",rmsg->node_name.c_str(),rmsg->command ); + RCLCPP_INFO(nh_->get_logger(), "Received Operator Command: %s sent '%i'",rmsg->node_name.c_str(),rmsg->command ); node_mediator_.setOperatorCommand(supervisor_, (OperatorCommand)rmsg->command); // TODO: topic name should come from vb_util_lib::topics. - LOG_MSG("/operator/command", rmsg, SU_LOG_LEVEL); + LOG_MSG("/operator/command", *rmsg, SU_LOG_LEVEL); } /** * process state @@ -301,7 +313,7 @@ class AMSuper : AMLifeCycle */ void processState(const std::string& node_name_in, const am::LifeCycleState state, const am::LifeCycleStatus status, const std::string& subsystem, const std::string& value, const int pid, - const ros::Time& last_contact) + const rclcpp::Time& last_contact) { // strip leading '/' from the node name if needed string node_name = node_mediator_.nodeNameStripped(node_name_in); @@ -316,25 +328,25 @@ class AMSuper : AMLifeCycle SuperNodeMediator::SuperNodeInfo& nr = it->second; if (!nr.online) { - ROS_INFO_STREAM("manifested node '" << node_name << "' came online [PGPG]"); + RCLCPP_INFO_STREAM(nh_->get_logger(), "manifested node '" << node_name << "' came online [PGPG]"); nr.online = true; nodes_changed = true; } if (nr.state != state) { - ROS_INFO_STREAM(node_name << " changed state to = " << life_cycle_mediator_.stateToString(state) << " [38S8]"); + RCLCPP_INFO_STREAM(nh_->get_logger(), node_name << " changed state to = " << life_cycle_mediator_.stateToString(state) << " [38S8]"); nr.state = state; nodes_changed = true; } if (nr.status != status) { - ROS_INFO_STREAM(node_name << " changed status to = " << life_cycle_mediator_.statusToString(status) << " [09SI]"); + RCLCPP_INFO_STREAM(nh_->get_logger(), node_name << " changed status to = " << life_cycle_mediator_.statusToString(status) << " [09SI]"); nr.status = status; nodes_changed = true; if(nr.manifested && nr.status == LifeCycleStatus::ERROR) { supervisor_.status_error = true; - ROS_ERROR_STREAM("Manifested node " << nr.name << " changed status to ERROR. Shutting down nodes... [JHRE]"); + RCLCPP_INFO_STREAM(nh_->get_logger(), "Manifested node " << nr.name << " changed status to ERROR. Shutting down nodes... [JHRE]"); stopFlightPlan(); } } @@ -343,11 +355,11 @@ class AMSuper : AMLifeCycle //process id = 0 observed to be a node coming online. -1 appears to be offline if(pid == 0) { - ROS_INFO_STREAM(node_name << " process is alive [UIRE]"); + RCLCPP_INFO_STREAM(nh_->get_logger(), node_name << " process is alive [UIRE]"); } else { - ROS_WARN_STREAM(node_name << " changed process id from: " << nr.pid << " to: " << pid << " [WNEW]"); + RCLCPP_INFO_STREAM(nh_->get_logger(), node_name << " changed process id from: " << nr.pid << " to: " << pid << " [WNEW]"); } nr.pid = pid; nodes_changed = true; @@ -357,7 +369,7 @@ class AMSuper : AMLifeCycle else { // if we get here, the node is not in the manifest and we've never heard from it before - ROS_WARN_STREAM("unknown node " << node_name << " came online. state: " << life_cycle_mediator_.stateToString(state) + RCLCPP_WARN_STREAM(nh_->get_logger(), "unknown node " << node_name << " came online. state: " << life_cycle_mediator_.stateToString(state) << ", status: " << life_cycle_mediator_.statusToString(status)); SuperNodeMediator::SuperNodeInfo nr; nr.name = node_name; @@ -393,7 +405,7 @@ class AMSuper : AMLifeCycle } if (flt_ctrl_state_changed) { - ROS_INFO_STREAM_THROTTLE(1.0, "flight status: " << value); + RCLCPP_INFO_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 1.0, "flight status: " << value); checkForSystemStateTransition(); } } @@ -404,7 +416,7 @@ class AMSuper : AMLifeCycle * * times out nodes that haven't been heard from recently. reports on status to bag and trace logs. */ - void heartbeatCB(const ros::TimerEvent& event) override + void heartbeatCB() override { #if CUDA_FLAG gpu_info_->display(); @@ -412,33 +424,33 @@ class AMSuper : AMLifeCycle //publish deprecated topic { - brain_box_msgs::VxState state_msg; + brain_box_msgs::msg::VxState state_msg; state_msg.state = (uint8_t)supervisor_.system_state; - vstate_summary_pub_.publish(state_msg); + vstate_summary_pub_->publish(state_msg); } //publish the system state { - brain_box_msgs::SystemState system_state_msg; + brain_box_msgs::msg::SystemState system_state_msg; system_state_msg.state = (uint8_t)supervisor_.system_state; system_state_msg.state_string = state_mediator_.stateToString(supervisor_.system_state); - system_state_pub_.publish(system_state_msg); + system_state_pub_->publish(system_state_msg); } // cycle thru all the nodes in the list to look for a timeout - ros::Time now = ros::Time().now(); + rclcpp::Time now = nh_->now(); map::iterator it; for (it = supervisor_.nodes.begin(); it != supervisor_.nodes.end(); it++) { SuperNodeMediator::SuperNodeInfo& nr = (*it).second; if (nr.online) { - ros::Duration time_since_contact = now - nr.last_contact; - ros::Duration timeout_dur(node_timeout_s_); + rclcpp::Duration time_since_contact = (now - nr.last_contact); + rclcpp::Duration timeout_dur(am::toDuration(node_timeout_s_)); if (time_since_contact > timeout_dur) { nr.online = false; - ROS_ERROR_STREAM("node timed out:" << nr.name); + RCLCPP_ERROR_STREAM(nh_->get_logger(),"node timed out:" << nr.name); reportSystemState(); } } @@ -449,7 +461,7 @@ class AMSuper : AMLifeCycle int num_manifest_nodes_online = node_mediator_.manifestedNodesOnlineCount(supervisor_); // publish and bag log super status message - brain_box_msgs::Super2Status status_msg; + brain_box_msgs::msg::Super2Status status_msg; status_msg.man = supervisor_.manifest.size(); status_msg.man_run = num_manifest_nodes_online; status_msg.run = node_mediator_.nodesOnlineCount(supervisor_); @@ -460,9 +472,9 @@ class AMSuper : AMLifeCycle status_msg.nodes.push_back(nr.name); } LOG_MSG("/status/super", status_msg, 1); - if (super_status_pub_.getNumSubscribers() > 0) + if (super_status_pub_->get_subscription_count() > 0) { - super_status_pub_.publish(status_msg); + super_status_pub_->publish(status_msg); } // report current status to trace log @@ -472,13 +484,13 @@ class AMSuper : AMLifeCycle if (supervisor_.manifest.size() != num_manifest_nodes_online) { // if all manifested nodes aren't running, report as error - ROS_ERROR_STREAM(ss.str()); - ROS_ERROR_STREAM("not online: " << node_mediator_.manifestedNodesNotOnlineNamesList(supervisor_)); + RCLCPP_ERROR_STREAM(nh_->get_logger(),ss.str()); + RCLCPP_ERROR_STREAM(nh_->get_logger(),"not online: " << node_mediator_.manifestedNodesNotOnlineNamesList(supervisor_)); } else { // if all manifested nodes are running, report as info - ROS_INFO_STREAM_THROTTLE(LOG_THROTTLE_S, ss.str()); + RCLCPP_INFO_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), LOG_THROTTLE_S, ss.str()); } // log stats @@ -488,13 +500,13 @@ class AMSuper : AMLifeCycle { //checking whether the file is open string tp; getline(newfile, tp); - std_msgs::Int16 msg; + std_msgs::msg::Int16 msg; msg.data = std::stoi(tp); LOG_MSG("/watts", msg, SU_LOG_LEVEL); newfile.close(); //close the file object. } - AMLifeCycle::heartbeatCB(event); + AMLifeCycle::heartbeatCB(); } /** @@ -516,7 +528,7 @@ class AMSuper : AMLifeCycle { std::stringstream ss; genSystemState(ss); - ROS_INFO_STREAM_THROTTLE(LOG_THROTTLE_S, ss.str()); + RCLCPP_INFO_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), LOG_THROTTLE_S, ss.str()); } /** @@ -528,11 +540,11 @@ class AMSuper : AMLifeCycle */ void sendLifeCycleCommand(const std::string_view& node_name, const LifeCycleCommand command) { - ROS_DEBUG_STREAM("sending command: " << life_cycle_mediator_.commandToString(command) << " to " << node_name << " lifecycle"); - brain_box_msgs::LifeCycleCommand msg; + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "sending command: " << life_cycle_mediator_.commandToString(command) << " to " << node_name << " lifecycle"); + brain_box_msgs::msg::LifeCycleCommand msg; msg.node_name = node_name; - msg.command = (brain_box_msgs::LifeCycleCommand::_command_type)command; - lifecycle_pub_.publish(msg); + msg.command = (brain_box_msgs::msg::LifeCycleCommand::_command_type)command; + lifecycle_pub_->publish(msg); } /** @@ -554,7 +566,7 @@ class AMSuper : AMLifeCycle { for (const auto & [ node_name, error_message ] : result.second) { - ROS_WARN_STREAM(error_message); + RCLCPP_WARN_STREAM(nh_->get_logger(),error_message); } } return success; @@ -564,10 +576,10 @@ class AMSuper : AMLifeCycle /** Send signal to flight controller that flight is over. */ void stopFlightPlan() { - std_msgs::Bool msg; + std_msgs::msg::Bool msg; msg.data = false; //false means deactivate - flight_plan_deactivation_pub_.publish(msg); - ROS_ERROR_STREAM("Sending flight plan kill command."); + flight_plan_deactivation_pub_->publish(msg); + RCLCPP_ERROR_STREAM(nh_->get_logger(), "Sending flight plan kill command."); } /** @@ -596,7 +608,7 @@ class AMSuper : AMLifeCycle LifeCycleCommand command = transition_instructions.life_cycle_command; std::string failed_nodes_string = boost::algorithm::join(transition_instructions.failed_nodes, ", "); std::string failed_nodes_reasons_string = boost::algorithm::join(transition_instructions.failed_nodes_reasons, ", "); - ROS_INFO_STREAM_THROTTLE(5,state_mediator_.stateToString(supervisor_.system_state) + RCLCPP_INFO_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 5,state_mediator_.stateToString(supervisor_.system_state) << ": sending " << life_cycle_mediator_.commandToString(command) << " to " << failed_nodes_string << " because " << failed_nodes_reasons_string); @@ -617,7 +629,7 @@ class AMSuper : AMLifeCycle */ void setSystemState(SuperState state) { - ROS_INFO_STREAM(state_mediator_.stateToString(supervisor_.system_state) << " --> " + RCLCPP_INFO_STREAM(nh_->get_logger(),state_mediator_.stateToString(supervisor_.system_state) << " --> " << state_mediator_.stateToString(state)); bool legal = true; if(!node_mediator_.forceTransition(state)) @@ -625,7 +637,7 @@ class AMSuper : AMLifeCycle if (!legal) { - ROS_ERROR_STREAM("illegal state transition from " << state_mediator_.stateToString(supervisor_.system_state) + RCLCPP_ERROR_STREAM(nh_->get_logger(),"illegal state transition from " << state_mediator_.stateToString(supervisor_.system_state) << " to " << state_mediator_.stateToString(state)); } else @@ -638,9 +650,9 @@ class AMSuper : AMLifeCycle sendLEDMessage(); - brain_box_msgs::VxState state_msg; + brain_box_msgs::msg::VxState state_msg; state_msg.state = (uint8_t)supervisor_.system_state; - vstate_summary_pub_.publish(state_msg); + vstate_summary_pub_->publish(state_msg); } } @@ -654,8 +666,8 @@ class AMSuper : AMLifeCycle SuperNodeMediator::PlatformVariant required_platform; SuperNodeMediator::PlatformVariant actual_platform; configurePlatformRequirements(required_platform,actual_platform); - ROS_WARN_STREAM("required" << required_platform.maker); - ROS_WARN_STREAM("actual" << actual_platform.maker); + RCLCPP_WARN_STREAM(nh_->get_logger(),"required" << required_platform.maker); + RCLCPP_WARN_STREAM(nh_->get_logger(),"actual" << actual_platform.maker); if(!node_mediator_.isCorrectPlatform(required_platform,actual_platform)) { std::stringstream message; @@ -703,7 +715,7 @@ class AMSuper : AMLifeCycle } else { - ROS_WARN("platform requirements not set"); + RCLCPP_WARN(nh_->get_logger(),"platform requirements not set"); } } @@ -712,12 +724,12 @@ class AMSuper : AMLifeCycle */ void sendLEDMessage(int r, int g, int b, float period = 0.0) { - brain_box_msgs::BlinkMCommand led_msg; + brain_box_msgs::msg::BlinkMCommand led_msg; led_msg.rgb.red = r; led_msg.rgb.green = g; led_msg.rgb.blue = b; led_msg.blink_rate = period; - led_pub_.publish(led_msg); + led_pub_->publish(led_msg); } static constexpr double LED_SOLID = 0.0; @@ -818,14 +830,14 @@ class AMSuper : AMLifeCycle error_ms = (int)(1000.0 / hz * 3.0 + 0.5); } - void diagnosticsCB(const diagnostic_msgs::DiagnosticArray::ConstPtr &msg) + void diagnosticsCB(const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg) { - LOG_MSG("/diagnostics", msg, SU_LOG_LEVEL); + LOG_MSG("/diagnostics", *msg, SU_LOG_LEVEL); } - void currentENUCB(const nav_msgs::Odometry::ConstPtr &msg) + void currentENUCB(const nav_msgs::msg::Odometry::SharedPtr msg) { - LOG_MSG(am_topics::CTRL_VX_VEHICLE_CURRENTENU, msg, SU_LOG_LEVEL); + LOG_MSG(am_topics::CTRL_VX_VEHICLE_CURRENTENU, *msg, SU_LOG_LEVEL); } BagLogger::BagLoggerLevel intToLoggerLevel(int level) @@ -850,14 +862,14 @@ class AMSuper : AMLifeCycle } } - void logControlCB(const brain_box_msgs::LogControl::ConstPtr &msg) + void logControlCB(const brain_box_msgs::msg::LogControl::SharedPtr msg) { if (msg->enable) { - ROS_INFO_STREAM("stop logging"); + RCLCPP_INFO_STREAM(nh_->get_logger(),"stop logging"); BagLogger::instance()->stopLogging(); - ROS_INFO_STREAM("start logging to SU, level " << SU_LOG_LEVEL); + RCLCPP_INFO_STREAM(nh_->get_logger(),"start logging to SU, level " << SU_LOG_LEVEL); BagLogger::instance()->startLogging("SU", SU_LOG_LEVEL); log_level_ = intToLoggerLevel (SU_LOG_LEVEL); } @@ -868,14 +880,23 @@ class AMSuper : AMLifeCycle #ifdef TESTING #else + +std::shared_ptr am::Node::node; + int main(int argc, char** argv) { - ros::init(argc, argv, ros::this_node::getName()); + rclcpp::init(argc, argv); + + am::Node::node = std::make_shared("am_super"); + + std::shared_ptr am_super_node = std::make_shared(am::Node::node); + + ROS_INFO_STREAM(am::Node::node->get_name() << ": running..."); - am::AMSuper node; + rclcpp::spin(am::Node::node); - ROS_INFO_STREAM(ros::this_node::getName() << ": running..."); + rclcpp::shutdown(); - ros::spin(); + return 0; } #endif diff --git a/src/am_super/super_node_mediator.cpp b/src/am_super/super_node_mediator.cpp index 3481091e..4a854874 100644 --- a/src/am_super/super_node_mediator.cpp +++ b/src/am_super/super_node_mediator.cpp @@ -9,8 +9,8 @@ namespace am /** * The state of the system as the supervisor sees it.*/ -SuperNodeMediator::SuperNodeMediator(const std::string& node_name): - SUPER_NODE_NAME(node_name), +SuperNodeMediator::SuperNodeMediator(rclcpp::Node::SharedPtr node, const std::string& node_name): + SUPER_NODE_NAME(node_name), node_(node), state_transitions_({ { SuperState::BOOTING, { {SuperState::READY, {SuperState::READY, SuperNodeMediator::checkReadyToArm, LifeCycleCommand::CONFIGURE}}}}, @@ -81,7 +81,7 @@ SuperNodeMediator::SuperNodeInfo SuperNodeMediator::initializeManifestedNode(std nr.name = node_name; nr.pid = -1; nr.online = false; - nr.last_contact = ros::Time(0); + nr.last_contact = node_->now(); nr.manifested = true; nr.state = LifeCycleState::UNCONFIGURED; nr.status = LifeCycleStatus::OK; @@ -274,7 +274,7 @@ pair> SuperNodeMediator::allManifestedNodesCheck( failed_nodes.insert(pair(node.name, error_message)); } } // for each node - return pair(success, failed_nodes); + return std::pair(success, failed_nodes); } void SuperNodeMediator::parseManifest(Supervisor& supervisor, string manifest) @@ -405,4 +405,4 @@ bool SuperNodeMediator::isCorrectPlatform(const SuperNodeMediator::PlatformVaria return pass; } -} \ No newline at end of file +} diff --git a/src/am_super/super_state_mediator.cpp b/src/am_super/super_state_mediator.cpp index 7af11809..ba1f40ab 100644 --- a/src/am_super/super_state_mediator.cpp +++ b/src/am_super/super_state_mediator.cpp @@ -1,5 +1,5 @@ #include - +#include namespace am { /**Local data class providing capabilities for states. @@ -86,4 +86,4 @@ std::string_view SuperStateMediator::stateToString(SuperState state) return info(state).name_; } -} \ No newline at end of file +} diff --git a/src/super_lib/am_life_cycle.cpp b/src/super_lib/am_life_cycle.cpp index 9b157b9f..4058bf09 100644 --- a/src/super_lib/am_life_cycle.cpp +++ b/src/super_lib/am_life_cycle.cpp @@ -1,10 +1,8 @@ #include -#include +#include #include #include - - - +#include namespace am { @@ -13,7 +11,7 @@ namespace am // static constexpr std::string AMLifeCycle::STATE_INVALID_STRING; // static constexpr std::string AMLifeCycle::STATE_UNCONFIGURED_STRING; -AMLifeCycle::AMLifeCycle() : nh_("~") +AMLifeCycle::AMLifeCycle(rclcpp::Node::SharedPtr node) : node_(node), updater_(node) { std::string init_state_str; //FIXME: This string should come from the enum @@ -40,7 +38,7 @@ AMLifeCycle::AMLifeCycle() : nh_("~") life_cycle_info_.state = LifeCycleState::ACTIVE; } life_cycle_info_.status = LifeCycleStatus::OK; - state_pub_ = nh_.advertise("/node_state", 100); + state_pub_ = node_->create_publisher("/node_state", 100); updater_.setHardwareID("none"); updater_.broadcast(0, "Initializing node"); @@ -49,13 +47,13 @@ AMLifeCycle::AMLifeCycle() : nh_("~") // strip leading '/' if it is there // TODO: this might always be there so just strip it without checking? - if (ros::this_node::getName().at(0) == '/') + if (std::string(node_->get_name()).at(0) == '/') { - node_name_ = ros::this_node::getName().substr(1); + node_name_ = std::string(node_->get_name()).substr(1); } else { - node_name_ = ros::this_node::getName(); + node_name_ = node_->get_name(); } @@ -64,9 +62,10 @@ AMLifeCycle::AMLifeCycle() : nh_("~") /** * node status via LifeCycle */ - lifecycle_sub_ = nh_.subscribe("/node_lifecycle", 100, &AMLifeCycle::lifecycleCB, this); + lifecycle_sub_ = node_->create_subscription("/node_lifecycle", 100, + std::bind(&AMLifeCycle::lifecycleCB, this, std::placeholders::_1)); - heartbeat_timer_ = nh_.createTimer(ros::Duration(1.0), &AMLifeCycle::heartbeatCB, this); + heartbeat_timer_ = node_->create_wall_timer(am::toDuration(1.0), std::bind(&AMLifeCycle::heartbeatCB, this)); } @@ -77,14 +76,16 @@ AMLifeCycle::~AMLifeCycle() template bool AMLifeCycle::param(const std::string& param_name, T& param_val, const T& default_val) const { - bool result = nh_.param(param_name, param_val, default_val); - ROS_INFO_STREAM(param_name << " = " << param_val); + //todo: fix the parameter + //bool result = nh_.param(param_name, param_val, default_val); + bool result = true; + RCLCPP_INFO_STREAM(node_->get_logger(), param_name << " = " << param_val); return result; } -void AMLifeCycle::lifecycleCB(const brain_box_msgs::LifeCycleCommand::ConstPtr msg) +void AMLifeCycle::lifecycleCB(const brain_box_msgs::msg::LifeCycleCommand::SharedPtr msg) { - ROS_DEBUG_STREAM_THROTTLE(1.0, life_cycle_mediator_.commandToString((LifeCycleCommand)msg->command)); + RCLCPP_DEBUG_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 1.0, life_cycle_mediator_.commandToString((LifeCycleCommand)msg->command)); if (!msg->node_name.compare(AMLifeCycle::BROADCAST_NODE_NAME) || !msg->node_name.compare(node_name_)) { @@ -103,7 +104,7 @@ void AMLifeCycle::lifecycleCB(const brain_box_msgs::LifeCycleCommand::ConstPtr m configure(); break; case LifeCycleCommand::CREATE: - ROS_WARN_STREAM("illegal command " << life_cycle_mediator_.commandToString(LifeCycleCommand::CREATE) << " [7YT8]"); + RCLCPP_WARN_STREAM(node_->get_logger(),"illegal command " << life_cycle_mediator_.commandToString(LifeCycleCommand::CREATE) << " [7YT8]"); break; case LifeCycleCommand::DEACTIVATE: transition("deactivate", LifeCycleState::ACTIVE, LifeCycleState::DEACTIVATING, LifeCycleState::INACTIVE, @@ -124,17 +125,17 @@ void AMLifeCycle::transition(std::string transition_name, LifeCycleState initial { if (life_cycle_info_.state == initial_state) { - ROS_INFO_STREAM(transition_name << ", current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [ASWU]"); + RCLCPP_INFO_STREAM(node_->get_logger(), transition_name << ", current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [ASWU]"); setState(transition_state); on_function(); } else if (life_cycle_info_.state == transition_state || life_cycle_info_.state == final_state) { - ROS_DEBUG_STREAM("ignoring redundant " << transition_name << " [0393]"); + RCLCPP_DEBUG_STREAM(node_->get_logger(),"ignoring redundant " << transition_name << " [0393]"); } else { - ROS_WARN_STREAM("received illegal " << transition_name << " in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [JGV5]"); + RCLCPP_WARN_STREAM(node_->get_logger(),"received illegal " << transition_name << " in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [JGV5]"); } } @@ -144,12 +145,12 @@ void AMLifeCycle::doTransition(std::string transition_name, bool success, LifeCy logState(); if (success) { - ROS_INFO_STREAM(transition_name << " succeeded"); + RCLCPP_INFO_STREAM(node_->get_logger(), transition_name << " succeeded"); setState(success_state); } else { - ROS_INFO_STREAM(transition_name << " failed"); + RCLCPP_INFO_STREAM(node_->get_logger(),transition_name << " failed"); setState(failure_state); } } @@ -180,7 +181,7 @@ void AMLifeCycle::doCleanup(bool success) void AMLifeCycle::onConfigure() { - ROS_INFO("onConfigure called [POMH]"); + RCLCPP_INFO(node_->get_logger(),"onConfigure called [POMH]"); if(stats_list_.hasStats()) { LifeCycleStatus status = stats_list_.process(throttle_info_.warn_throttle_s, throttle_info_.error_throttle_s); @@ -190,7 +191,7 @@ void AMLifeCycle::onConfigure() } else if (!withinConfigureTolerance()) { - ROS_WARN_STREAM_THROTTLE(5, stats_list_.getStatsStr() << " blocked by stats past configure tolerance with status " << life_cycle_mediator_.statusToString(status) ); + RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(),5, stats_list_.getStatsStr() << " blocked by stats past configure tolerance with status " << life_cycle_mediator_.statusToString(status) ); } } //if there are no stats and request to configure, then configure @@ -214,7 +215,7 @@ void AMLifeCycle::onDeactivate() void AMLifeCycle::logState() { - ROS_INFO_STREAM("LifeCycle: " << life_cycle_mediator_.stateToString(life_cycle_info_.state)); + RCLCPP_INFO_STREAM(node_->get_logger(),"LifeCycle: " << life_cycle_mediator_.stateToString(life_cycle_info_.state)); } void AMLifeCycle::doDeactivate(bool success) @@ -227,7 +228,7 @@ void AMLifeCycle::configure() //mark the configuration start time once if(getState() != LifeCycleState::CONFIGURING) { - configure_start_time_=ros::Time().now(); + configure_start_time_=node_->now(); } transition("configure", LifeCycleState::UNCONFIGURED, LifeCycleState::CONFIGURING, LifeCycleState::INACTIVE, std::bind(&AMLifeCycle::onConfigure, this)); @@ -237,12 +238,12 @@ void AMLifeCycle::destroy() { if (life_cycle_mediator_.illegalDestroy(life_cycle_info_)) { - ROS_INFO_STREAM("received illegal activate in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [45RT]"); + RCLCPP_INFO_STREAM(node_->get_logger(),"received illegal activate in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [45RT]"); } /* This condition is hit only if state equals FINALIZED. Checking SHUTTING_DOWN is redundant */ else { - ROS_INFO_STREAM("current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [RE45]"); + RCLCPP_INFO_STREAM(node_->get_logger(),"current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [RE45]"); onDestroy(); } } @@ -265,8 +266,8 @@ bool AMLifeCycle::withinConfigureTolerance() //outside of configuring, we have no tolerance if(life_cycle_mediator_.unconfigured(life_cycle_info_)) { - ros::Duration duration_since_configure = ros::Time::now() - configure_start_time_; - if (life_cycle_info_.state == LifeCycleState::UNCONFIGURED || duration_since_configure <= ros::Duration(configure_tolerance_s) ) + rclcpp::Duration duration_since_configure = node_->now() - configure_start_time_; + if (life_cycle_info_.state == LifeCycleState::UNCONFIGURED || duration_since_configure <= rclcpp::Duration(am::toDuration(configure_tolerance_s)) ) { tolerated = true; } @@ -284,7 +285,7 @@ void AMLifeCycle::error(std::string message, std::string error_code, bool forced std::string error_code_message = "Error[" + error_code + "] "; if(withinConfigureTolerance() && !forced) { - ROS_WARN_STREAM_THROTTLE(throttle_info_.warn_throttle_s,"Ignoring tolerant error for (" << configure_tolerance_s << "s) `" << message << "` " << error_code_message << "[GFRT]"); + RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(),throttle_info_.warn_throttle_s,"Ignoring tolerant error for (" << configure_tolerance_s << "s) `" << message << "` " << error_code_message << "[GFRT]"); } else { @@ -302,7 +303,7 @@ void AMLifeCycle::error(std::string message, std::string error_code, bool forced repeat_prefix = "Repeated "; } std::string error_explanation=forced_prefix + repeat_prefix + error_code_message; - ROS_ERROR_STREAM(message << " -> " << error_explanation << " [R45Y]" ); + RCLCPP_ERROR_STREAM(node_->get_logger(), message << " -> " << error_explanation << " [R45Y]" ); } } @@ -341,17 +342,17 @@ void AMLifeCycle::shutdown() { if (life_cycle_mediator_.shutdown(life_cycle_info_)) { - ROS_INFO_STREAM("current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state)); + RCLCPP_INFO_STREAM(node_->get_logger(),"current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state)); setState(LifeCycleState::SHUTTING_DOWN); onShutdown(); } else if (life_cycle_mediator_.redundantShutdown(life_cycle_info_)) { - ROS_DEBUG_STREAM("ignoring redundant shutdown"); + RCLCPP_DEBUG_STREAM(node_->get_logger(),"ignoring redundant shutdown"); } else { - ROS_INFO_STREAM("received illegal activate in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state)); + RCLCPP_INFO_STREAM(node_->get_logger(),"received illegal activate in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state)); } } @@ -453,27 +454,33 @@ void AMLifeCycle::configureStat(AMStat& stat) AMStatReset& AMLifeCycle::configureHzStat(AMStatReset& stat) { configureStat(stat, stat.getLongName(), "hz"); + + return stat; + } AMStatReset& AMLifeCycle::configureHzStats(AMStatReset& stats) { configureStat(stats, stats.getShortName(), "hz"); + + return stats; } void AMLifeCycle::sendNodeUpdate() { - brain_box_msgs::LifeCycleState msg; - msg.node_name = ros::this_node::getName(); + brain_box_msgs::msg::LifeCycleState msg; + msg.header.stamp = node_->now(); + msg.node_name = node_->get_name(); msg.process_id = 0; msg.state = (uint8_t)life_cycle_info_.state; msg.status = (uint8_t)life_cycle_info_.status; msg.subsystem = ""; msg.value = ""; - state_pub_.publish(msg); + state_pub_->publish(msg); } -void AMLifeCycle::heartbeatCB(const ros::TimerEvent& event) +void AMLifeCycle::heartbeatCB() { updater_.force_update(); @@ -482,7 +489,7 @@ void AMLifeCycle::heartbeatCB(const ros::TimerEvent& event) << stats_list_.getStatsStrShort(); double throttle_s = getThrottle(); - ROS_INFO_STREAM_THROTTLE(throttle_s, "LifeCycle heartbeat: " << ss.str()); + RCLCPP_INFO_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), throttle_s, "LifeCycle heartbeat: " << ss.str()); stats_list_.reset(); @@ -505,12 +512,12 @@ void AMLifeCycle::setState(const LifeCycleState state) if (life_cycle_mediator_.setState(state, life_cycle_info_)) { - ROS_INFO_STREAM("changing state from " << life_cycle_mediator_.stateToString(initial_state) << " to " << life_cycle_mediator_.stateToString(state)); + RCLCPP_INFO_STREAM(node_->get_logger(), "changing state from " << life_cycle_mediator_.stateToString(initial_state) << " to " << life_cycle_mediator_.stateToString(state)); sendNodeUpdate(); } else { - ROS_ERROR_STREAM("illegal state: " << (int)state); + RCLCPP_ERROR_STREAM(node_->get_logger(), "illegal state: " << (int)state); } } @@ -524,7 +531,7 @@ bool AMLifeCycle::setStatus(const LifeCycleStatus status) //if we are in error and want to leave it if(life_cycle_info_.status == LifeCycleStatus::ERROR && status != LifeCycleStatus::ERROR) { - ROS_WARN_STREAM_THROTTLE(getThrottle(), "requested to change status from ERROR to " << life_cycle_mediator_.statusToString(status) << " [DFRE]"); + RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), getThrottle(), "requested to change status from ERROR to " << life_cycle_mediator_.statusToString(status) << " [DFRE]"); } else if (life_cycle_mediator_.setStatus(status, life_cycle_info_)) { @@ -533,8 +540,10 @@ bool AMLifeCycle::setStatus(const LifeCycleStatus status) else { - ROS_ERROR_STREAM("illegal status: " << life_cycle_mediator_.statusToString(status)); + RCLCPP_ERROR_STREAM(node_->get_logger(), "illegal status: " << life_cycle_mediator_.statusToString(status)); } + + return true; } const std::string_view& AMLifeCycle::getStatusName() diff --git a/src/super_lib/am_super_test.cpp b/src/super_lib/am_super_test.cpp new file mode 100644 index 00000000..e5b6d7d3 --- /dev/null +++ b/src/super_lib/am_super_test.cpp @@ -0,0 +1,227 @@ +#include +#include + +#include + +AMSuperTest::AMSuperTest(string target_node_name) : nh_(std::make_shared(target_node_name)) +{ + createPubsSubs(); + + target_node_name_= target_node_name; + if(target_node_name_[0] != '/') + { + target_node_name_= '/' + target_node_name_; + } + TEST_LOG("Target node name:" << target_node_name_,"AXSO"); +} + +AMSuperTest::AMSuperTest() : nh_(std::make_shared("am_super_test")) +{ + createPubsSubs(); + + am::getParam(nh_, "~target_node_name", target_node_name_, nh_->get_name()); + if(target_node_name_[0] != '/') + { + target_node_name_= '/' + target_node_name_; + } + + TEST_LOG("Target node name:" << target_node_name_, "ANNQ"); +} + +void AMSuperTest::createPubsSubs() +{ + nodeLifeCycleStateSubscription_ = nh_->create_subscription + (am_super_topics::LIFECYCLE_STATE, 1000, + std::bind(&AMSuperTest::nodeLifeCycleStateCallback, this, std::placeholders::_1)); + missionStateSubscription_ = nh_->create_subscription + (am_super_topics::SUPER_STATE, 1000, + std::bind(&AMSuperTest::missionStateCallback, this, std::placeholders::_1)); + operatorCommandPublisher_ = nh_->create_publisher + (am_super_topics::OPERATOR_COMMAND,100); + controllerStatePublisher_ = nh_->create_publisher + (am_super_topics::CONTROLLER_STATE, 100); +} + +void AMSuperTest::publishOperatorCommand(uint8_t command) +{ + brain_box_msgs::msg::OperatorCommand msg; + msg.node_name = nh_->get_name(); + msg.command = command; + operatorCommandPublisher_->publish(msg); +} + +void AMSuperTest::publishControllerState(uint8_t state) +{ + brain_box_msgs::msg::ControllerState msg; + msg.node_name = nh_->get_name(); + msg.state = state; + controllerStatePublisher_->publish(msg); +} + +/** when configured and Inactive, arm is sent to prepare all nodes for takeoff*/ +void AMSuperTest::arm() +{ + TEST_LOG("Operator sending ARM command","NQNA"); + publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::ARM); +} + +/** when armed, signals for the props to spin and takeoff */ +void AMSuperTest::launch() +{ + TEST_LOG("Operator sending LAUNCH command","ANQP"); + publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::LAUNCH); +} + +void AMSuperTest::landed() +{ + TEST_LOG("Controller sending LANDED state","BSJO"); + publishControllerState(brain_box_msgs::msg::ControllerState::COMPLETED); +} + +void AMSuperTest::cancel() +{ + TEST_LOG("Operator sending CANCEL command","LPOQ"); + publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::CANCEL); +} + +void AMSuperTest::pause() +{ + TEST_LOG("Operator sending PAUSE command","WOEB"); + publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::PAUSE); +} + +void AMSuperTest::resume() +{ + TEST_LOG("Operator sending RESUME command","ANQE"); + publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::RESUME); +} + +void AMSuperTest::abort() +{ + TEST_LOG("Operator sending ABORT command","EEEN"); + publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::ABORT); +} + +void AMSuperTest::manual() +{ + TEST_LOG("Operator sending MANUAL command","NQAY"); + publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::MANUAL); +} + +void AMSuperTest::shutdown() +{ + TEST_LOG("Operator sending SHUTDOWN command","VKSP"); + publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::SHUTDOWN); +} + +bool AMSuperTest::nodeStateReceived(string node_name,LifeCycleState state) +{ + if(node_states_.count(node_name)){ + int key = 2; + auto lower_it = node_states_.lower_bound(node_name); + auto upper_it = node_states_.upper_bound(node_name); + + while (lower_it != upper_it) + { + if (lower_it -> first == node_name) { + brain_box_msgs::msg::LifeCycleState state_msg = lower_it -> second; + if((LifeCycleState)state_msg.state == state){ + return true; + } + } + lower_it++; + } + } + return false; +} + +bool AMSuperTest::missionStateReceived(uint8_t mission_state) +{ + for (auto state : mission_states_) + { + if(state == mission_state) + { + //Erase-remove idiom for removing all occurences from list + mission_states_.erase(remove(mission_states_.begin(), mission_states_.end(), state), mission_states_.end()); + return true; + } + } + return false; +} + +void AMSuperTest::nodeLifeCycleStateCallback(const brain_box_msgs::msg::LifeCycleState& msg) +{ + node_states_.emplace(msg.node_name,msg); +} + +void AMSuperTest::missionStateCallback(const brain_box_msgs::msg::VxState& msg) +{ + mission_states_.insert(mission_states_.end(),msg.state); +} + +void AMSuperTest::waitUntilMissionState(const uint8_t mission_state, float sleep) +{ + waitUntilMissionState(mission_state,"NAWU",sleep); +} + +void AMSuperTest::waitUntilMissionState(const uint8_t mission_state, std::string error_code, float sleep) +{ + rclcpp::Rate loop_rate(sleep); + while (!missionStateReceived(mission_state) && rclcpp::ok() ) + { + rclcpp::spin_some(nh_); + loop_rate.sleep(); + TEST_LOG("[" << error_code << "] waiting to receive mission state: " << mission_state, "08JU"); + } +} + +void AMSuperTest::waitUntil(const LifeCycleState state, float sleep){ + waitUntil(state,"XS32",sleep); +} + +void AMSuperTest::waitUntil(const LifeCycleState state, const std::string log_code, float sleep){ + rclcpp::Rate loop_rate(sleep); + while (!nodeStateReceived(nh_->get_name(),state) && rclcpp::ok() ) + { + rclcpp::spin_some(nh_); + loop_rate.sleep(); + TEST_LOG("[" << log_code << "]" << " waiting to receive node state: " << (int)state,"NBDC"); + } +} + +void AMSuperTest::waitUntilStatus(const LifeCycleStatus& status, float sleep) +{ + waitUntil(status,"XWSQ",sleep); + +} +void AMSuperTest::waitUntil(const LifeCycleStatus& status, const std::string log_code, float sleep) +{ + rclcpp::Rate loop_rate(sleep); + while (!nodeStatusReceived(nh_->get_name(),status) && rclcpp::ok() ) + { + rclcpp::spin_some(nh_); + loop_rate.sleep(); + TEST_LOG("[" << log_code << "]" << " waiting to receive node status: " << (int)status, "YTNJ" ); + } +} + +bool AMSuperTest::nodeStatusReceived(string node_name, LifeCycleStatus status) +{ + if(node_states_.count(node_name)){ + int key = 2; + auto lower_it = node_states_.lower_bound(node_name); + auto upper_it = node_states_.upper_bound(node_name); + + while (lower_it != upper_it) + { + if (lower_it -> first == node_name) { + brain_box_msgs::msg::LifeCycleState state_msg = lower_it -> second; + if((LifeCycleStatus)state_msg.status == status){ + return true; + } + } + lower_it++; + } + } + return false; +} \ No newline at end of file diff --git a/super_test/abort_to_disarming/abort_to_disarming_rostest.cpp b/super_test/abort_to_disarming/abort_to_disarming_rostest.cpp new file mode 100644 index 00000000..37e5e9a4 --- /dev/null +++ b/super_test/abort_to_disarming/abort_to_disarming_rostest.cpp @@ -0,0 +1,31 @@ +#include + +class AbortToDisarming : public AMSuperTest +{ +protected: + AbortToDisarming() : AMSuperTest("abort_to_disarming") {} +}; + +TEST_F(AbortToDisarming, testState_AbortToDisarming) +{ + waitUntilMissionState(brain_box_msgs::msg::VxState::READY,"N3DJ"); + arm(); + waitUntilMissionState(brain_box_msgs::msg::VxState::ARMED,"XX3X"); + launch(); + waitUntilMissionState(brain_box_msgs::msg::VxState::AUTO,"YYUI"); + abort(); + waitUntilMissionState(brain_box_msgs::msg::VxState::ABORT,"NSKE"); + landed(); + waitUntilMissionState(brain_box_msgs::msg::VxState::DISARMING,"XXCV"); +} + +int main(int argc, char** argv) +{ + std::cout << "STARTING ROS" << std::endl; + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + std::cout << "DONE SHUTTING DOWN ROS" << std::endl; + return result; +} \ No newline at end of file diff --git a/rostest/abort_to_disarming/abort_to_disarming_rostest.test b/super_test/abort_to_disarming/abort_to_disarming_rostest.test similarity index 100% rename from rostest/abort_to_disarming/abort_to_disarming_rostest.test rename to super_test/abort_to_disarming/abort_to_disarming_rostest.test diff --git a/super_test/abort_to_disarming/launch/__pycache__/abort_to_disarming.test.cpython-38.pyc b/super_test/abort_to_disarming/launch/__pycache__/abort_to_disarming.test.cpython-38.pyc new file mode 100644 index 00000000..faae1222 Binary files /dev/null and b/super_test/abort_to_disarming/launch/__pycache__/abort_to_disarming.test.cpython-38.pyc differ diff --git a/super_test/abort_to_disarming/launch/abort_to_disarming.test.py b/super_test/abort_to_disarming/launch/abort_to_disarming.test.py new file mode 100644 index 00000000..336f9ce9 --- /dev/null +++ b/super_test/abort_to_disarming/launch/abort_to_disarming.test.py @@ -0,0 +1,52 @@ +# -*- coding: utf-8 -*- +import launch +from launch.actions import TimerAction +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +import launch_testing +import os +import sys +import unittest + + +def generate_test_description(): + + abort_to_disarming = Node( + executable=launch.substitutions.PathJoinSubstitution( + [ + launch.substitutions.LaunchConfiguration("test_binary_dir"), + "abort_to_disarming", + ] + ), + output="screen", + ) + + return launch.LaunchDescription( + [ + launch.actions.DeclareLaunchArgument( + name="test_binary_dir", + description="Binary directory of package " + "containing test executables", + ), + abort_to_disarming, + # TimerAction(period=2.0, actions=[basic_test]), + launch_testing.actions.ReadyToTest(), + ] + ), { + "abort_to_disarming": abort_to_disarming, + } + + +class TestGTestWaitForCompletion(unittest.TestCase): + # Waits for test to complete, then waits a bit to make sure result files are generated + def test_gtest_run_complete(self, abort_to_disarming): + self.proc_info.assertWaitForShutdown(abort_to_disarming, timeout=4000.0) + + +@launch_testing.post_shutdown_test() +class TestGTestProcessPostShutdown(unittest.TestCase): + # Checks if the test has been completed with acceptable exit codes + def test_gtest_pass(self, proc_info, abort_to_disarming): + launch_testing.asserts.assertExitCodes( + proc_info, process=abort_to_disarming + ) \ No newline at end of file diff --git a/rostest/abort_to_manual/abort_to_manual_rostest.cpp b/super_test/abort_to_manual/abort_to_manual_rostest.cpp similarity index 100% rename from rostest/abort_to_manual/abort_to_manual_rostest.cpp rename to super_test/abort_to_manual/abort_to_manual_rostest.cpp diff --git a/rostest/abort_to_manual/abort_to_manual_rostest.test b/super_test/abort_to_manual/abort_to_manual_rostest.test similarity index 100% rename from rostest/abort_to_manual/abort_to_manual_rostest.test rename to super_test/abort_to_manual/abort_to_manual_rostest.test diff --git a/rostest/am_super_rostest.test b/super_test/am_super_rostest.test similarity index 100% rename from rostest/am_super_rostest.test rename to super_test/am_super_rostest.test diff --git a/rostest/armed_to_ready/armed_to_ready_rostest.cpp b/super_test/armed_to_ready/armed_to_ready_rostest.cpp similarity index 100% rename from rostest/armed_to_ready/armed_to_ready_rostest.cpp rename to super_test/armed_to_ready/armed_to_ready_rostest.cpp diff --git a/rostest/armed_to_ready/armed_to_ready_rostest.test b/super_test/armed_to_ready/armed_to_ready_rostest.test similarity index 100% rename from rostest/armed_to_ready/armed_to_ready_rostest.test rename to super_test/armed_to_ready/armed_to_ready_rostest.test diff --git a/rostest/auto_to_abort/auto_to_abort_rostest.cpp b/super_test/auto_to_abort/auto_to_abort_rostest.cpp similarity index 100% rename from rostest/auto_to_abort/auto_to_abort_rostest.cpp rename to super_test/auto_to_abort/auto_to_abort_rostest.cpp diff --git a/rostest/auto_to_abort/auto_to_abort_rostest.test b/super_test/auto_to_abort/auto_to_abort_rostest.test similarity index 100% rename from rostest/auto_to_abort/auto_to_abort_rostest.test rename to super_test/auto_to_abort/auto_to_abort_rostest.test diff --git a/rostest/auto_to_manual/auto_to_manual_rostest.cpp b/super_test/auto_to_manual/auto_to_manual_rostest.cpp similarity index 100% rename from rostest/auto_to_manual/auto_to_manual_rostest.cpp rename to super_test/auto_to_manual/auto_to_manual_rostest.cpp diff --git a/rostest/auto_to_manual/auto_to_manual_rostest.test b/super_test/auto_to_manual/auto_to_manual_rostest.test similarity index 100% rename from rostest/auto_to_manual/auto_to_manual_rostest.test rename to super_test/auto_to_manual/auto_to_manual_rostest.test diff --git a/rostest/auto_to_semiauto/auto_to_semiauto_rostest.cpp b/super_test/auto_to_semiauto/auto_to_semiauto_rostest.cpp similarity index 100% rename from rostest/auto_to_semiauto/auto_to_semiauto_rostest.cpp rename to super_test/auto_to_semiauto/auto_to_semiauto_rostest.cpp diff --git a/rostest/auto_to_semiauto/auto_to_semiauto_rostest.test b/super_test/auto_to_semiauto/auto_to_semiauto_rostest.test similarity index 100% rename from rostest/auto_to_semiauto/auto_to_semiauto_rostest.test rename to super_test/auto_to_semiauto/auto_to_semiauto_rostest.test diff --git a/rostest/error_configure_tolerance/error_configure_tolerance_rostest.cpp b/super_test/error_configure_tolerance/error_configure_tolerance_rostest.cpp similarity index 100% rename from rostest/error_configure_tolerance/error_configure_tolerance_rostest.cpp rename to super_test/error_configure_tolerance/error_configure_tolerance_rostest.cpp diff --git a/rostest/error_configure_tolerance/error_configure_tolerance_rostest.test b/super_test/error_configure_tolerance/error_configure_tolerance_rostest.test similarity index 100% rename from rostest/error_configure_tolerance/error_configure_tolerance_rostest.test rename to super_test/error_configure_tolerance/error_configure_tolerance_rostest.test diff --git a/rostest/error_forced/error_forced_rostest.cpp b/super_test/error_forced/error_forced_rostest.cpp similarity index 100% rename from rostest/error_forced/error_forced_rostest.cpp rename to super_test/error_forced/error_forced_rostest.cpp diff --git a/rostest/error_forced/error_forced_rostest.test b/super_test/error_forced/error_forced_rostest.test similarity index 100% rename from rostest/error_forced/error_forced_rostest.test rename to super_test/error_forced/error_forced_rostest.test diff --git a/rostest/error_status/error_status_rostest.cpp b/super_test/error_status/error_status_rostest.cpp similarity index 100% rename from rostest/error_status/error_status_rostest.cpp rename to super_test/error_status/error_status_rostest.cpp diff --git a/rostest/error_status/error_status_rostest.test b/super_test/error_status/error_status_rostest.test similarity index 100% rename from rostest/error_status/error_status_rostest.test rename to super_test/error_status/error_status_rostest.test diff --git a/rostest/error_status_without_stats/error_status_without_stats_rostest.cpp b/super_test/error_status_without_stats/error_status_without_stats_rostest.cpp similarity index 100% rename from rostest/error_status_without_stats/error_status_without_stats_rostest.cpp rename to super_test/error_status_without_stats/error_status_without_stats_rostest.cpp diff --git a/rostest/error_status_without_stats/error_status_without_stats_rostest.test b/super_test/error_status_without_stats/error_status_without_stats_rostest.test similarity index 100% rename from rostest/error_status_without_stats/error_status_without_stats_rostest.test rename to super_test/error_status_without_stats/error_status_without_stats_rostest.test diff --git a/rostest/error_terminal_before_config/error_terminal_before_config_rostest.cpp b/super_test/error_terminal_before_config/error_terminal_before_config_rostest.cpp similarity index 100% rename from rostest/error_terminal_before_config/error_terminal_before_config_rostest.cpp rename to super_test/error_terminal_before_config/error_terminal_before_config_rostest.cpp diff --git a/rostest/error_terminal_before_config/error_terminal_before_config_rostest.test b/super_test/error_terminal_before_config/error_terminal_before_config_rostest.test similarity index 100% rename from rostest/error_terminal_before_config/error_terminal_before_config_rostest.test rename to super_test/error_terminal_before_config/error_terminal_before_config_rostest.test diff --git a/rostest/error_tolerant_before_config/error_tolerant_before_config_rostest.cpp b/super_test/error_tolerant_before_config/error_tolerant_before_config_rostest.cpp similarity index 100% rename from rostest/error_tolerant_before_config/error_tolerant_before_config_rostest.cpp rename to super_test/error_tolerant_before_config/error_tolerant_before_config_rostest.cpp diff --git a/rostest/error_tolerant_before_config/error_tolerant_before_config_rostest.test b/super_test/error_tolerant_before_config/error_tolerant_before_config_rostest.test similarity index 100% rename from rostest/error_tolerant_before_config/error_tolerant_before_config_rostest.test rename to super_test/error_tolerant_before_config/error_tolerant_before_config_rostest.test diff --git a/rostest/hz_config/hz_config_rostest.cpp b/super_test/hz_config/hz_config_rostest.cpp similarity index 100% rename from rostest/hz_config/hz_config_rostest.cpp rename to super_test/hz_config/hz_config_rostest.cpp diff --git a/rostest/hz_config/hz_config_rostest.test b/super_test/hz_config/hz_config_rostest.test similarity index 100% rename from rostest/hz_config/hz_config_rostest.test rename to super_test/hz_config/hz_config_rostest.test diff --git a/rostest/hz_config/hz_config_rostest.yaml b/super_test/hz_config/hz_config_rostest.yaml similarity index 100% rename from rostest/hz_config/hz_config_rostest.yaml rename to super_test/hz_config/hz_config_rostest.yaml diff --git a/rostest/manual_to_disarming/manual_to_disarming_rostest.cpp b/super_test/manual_to_disarming/manual_to_disarming_rostest.cpp similarity index 100% rename from rostest/manual_to_disarming/manual_to_disarming_rostest.cpp rename to super_test/manual_to_disarming/manual_to_disarming_rostest.cpp diff --git a/rostest/manual_to_disarming/manual_to_disarming_rostest.test b/super_test/manual_to_disarming/manual_to_disarming_rostest.test similarity index 100% rename from rostest/manual_to_disarming/manual_to_disarming_rostest.test rename to super_test/manual_to_disarming/manual_to_disarming_rostest.test diff --git a/rostest/param/param_rostest.cpp b/super_test/param/param_rostest.cpp similarity index 100% rename from rostest/param/param_rostest.cpp rename to super_test/param/param_rostest.cpp diff --git a/rostest/param/param_rostest.test b/super_test/param/param_rostest.test similarity index 100% rename from rostest/param/param_rostest.test rename to super_test/param/param_rostest.test diff --git a/rostest/platform_app_required_fail/platform_app_required_fail_rostest.cpp b/super_test/platform_app_required_fail/platform_app_required_fail_rostest.cpp similarity index 100% rename from rostest/platform_app_required_fail/platform_app_required_fail_rostest.cpp rename to super_test/platform_app_required_fail/platform_app_required_fail_rostest.cpp diff --git a/rostest/platform_app_required_fail/platform_app_required_fail_rostest.test b/super_test/platform_app_required_fail/platform_app_required_fail_rostest.test similarity index 100% rename from rostest/platform_app_required_fail/platform_app_required_fail_rostest.test rename to super_test/platform_app_required_fail/platform_app_required_fail_rostest.test diff --git a/rostest/platform_app_required_pass/platform_app_required_pass_rostest.cpp b/super_test/platform_app_required_pass/platform_app_required_pass_rostest.cpp similarity index 100% rename from rostest/platform_app_required_pass/platform_app_required_pass_rostest.cpp rename to super_test/platform_app_required_pass/platform_app_required_pass_rostest.cpp diff --git a/rostest/platform_app_required_pass/platform_app_required_pass_rostest.test b/super_test/platform_app_required_pass/platform_app_required_pass_rostest.test similarity index 100% rename from rostest/platform_app_required_pass/platform_app_required_pass_rostest.test rename to super_test/platform_app_required_pass/platform_app_required_pass_rostest.test diff --git a/rostest/platform_required_fail/platform_required_fail_rostest.cpp b/super_test/platform_required_fail/platform_required_fail_rostest.cpp similarity index 100% rename from rostest/platform_required_fail/platform_required_fail_rostest.cpp rename to super_test/platform_required_fail/platform_required_fail_rostest.cpp diff --git a/rostest/platform_required_fail/platform_required_fail_rostest.test b/super_test/platform_required_fail/platform_required_fail_rostest.test similarity index 100% rename from rostest/platform_required_fail/platform_required_fail_rostest.test rename to super_test/platform_required_fail/platform_required_fail_rostest.test diff --git a/rostest/platform_required_pass/platform_required_pass_rostest.cpp b/super_test/platform_required_pass/platform_required_pass_rostest.cpp similarity index 100% rename from rostest/platform_required_pass/platform_required_pass_rostest.cpp rename to super_test/platform_required_pass/platform_required_pass_rostest.cpp diff --git a/rostest/platform_required_pass/platform_required_pass_rostest.test b/super_test/platform_required_pass/platform_required_pass_rostest.test similarity index 100% rename from rostest/platform_required_pass/platform_required_pass_rostest.test rename to super_test/platform_required_pass/platform_required_pass_rostest.test diff --git a/rostest/primary/primary_rostest.cpp b/super_test/primary/primary_rostest.cpp similarity index 100% rename from rostest/primary/primary_rostest.cpp rename to super_test/primary/primary_rostest.cpp diff --git a/rostest/primary/primary_rostest.test b/super_test/primary/primary_rostest.test similarity index 100% rename from rostest/primary/primary_rostest.test rename to super_test/primary/primary_rostest.test diff --git a/rostest/ready_to_shutdown/ready_to_shutdown_rostest.cpp b/super_test/ready_to_shutdown/ready_to_shutdown_rostest.cpp similarity index 100% rename from rostest/ready_to_shutdown/ready_to_shutdown_rostest.cpp rename to super_test/ready_to_shutdown/ready_to_shutdown_rostest.cpp diff --git a/rostest/ready_to_shutdown/ready_to_shutdown_rostest.test b/super_test/ready_to_shutdown/ready_to_shutdown_rostest.test similarity index 100% rename from rostest/ready_to_shutdown/ready_to_shutdown_rostest.test rename to super_test/ready_to_shutdown/ready_to_shutdown_rostest.test diff --git a/rostest/semi_auto_to_manual/semi_auto_to_manual_rostest.cpp b/super_test/semi_auto_to_manual/semi_auto_to_manual_rostest.cpp similarity index 100% rename from rostest/semi_auto_to_manual/semi_auto_to_manual_rostest.cpp rename to super_test/semi_auto_to_manual/semi_auto_to_manual_rostest.cpp diff --git a/rostest/semi_auto_to_manual/semi_auto_to_manual_rostest.test b/super_test/semi_auto_to_manual/semi_auto_to_manual_rostest.test similarity index 100% rename from rostest/semi_auto_to_manual/semi_auto_to_manual_rostest.test rename to super_test/semi_auto_to_manual/semi_auto_to_manual_rostest.test