diff --git a/CMakeLists.txt b/CMakeLists.txt index bd1a0d96..c5334b07 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,118 +1,42 @@ -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() -#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 +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +set(dependencies am_utils - diagnostic_msgs - rosbag - roscpp - rospy + brain_box_msgs + builtin_interfaces + rclcpp + rclpy + rosbag2 + rosbag2_cpp 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} -) +# find dependencies +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${dependencies}) + find_package(${Dependency} REQUIRED) +endforeach() -file(GLOB super_lib_cpp_files - src/super_lib/*.cpp +include_directories( + include ) file(GLOB cuda_cpp_files @@ -127,119 +51,48 @@ 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}) +add_executable(am_super ${am_super_cpp_files}) +ament_target_dependencies(am_super ${dependencies}) -#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) +install(DIRECTORY include/ + DESTINATION include/ +) -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() +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) -# 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 + # 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() + + 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) + ament_target_dependencies(abort_to_disarming ${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_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..24eb4ad0 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(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(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(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(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 deleted file mode 100644 index d1fca67a..00000000 --- a/include/super_lib/am_life_cycle.h +++ /dev/null @@ -1,245 +0,0 @@ -#ifndef AM_SUPER_INCLUDE_SUPER_LIB_AM_LIFECYCLE_H_ -#define AM_SUPER_INCLUDE_SUPER_LIB_AM_LIFECYCLE_H_ - -#include - -#include - -#include - -#include -#include -#include -#include - -namespace am -{ -/** - * Parent for all nodes wishing to report their state for collective management. - * The LifeCycle is generalized to represent all nodes regardless of application. - * - * Each node reports is own state, but also receives commands requesting transition. - * - * Implementing nodes should override methods appropriate to satisfy the needs of the node. - * - * Read more about ROS2 LifeCycle and view the handy diagram. - * - * https://automodality.atlassian.net/wiki/spaces/AMROS/pages/901546330/AM+Node+LifeCycle - * - */ -class AMLifeCycle -{ - public: - static constexpr std::string_view BROADCAST_NODE_NAME = ""; - - /**Specific parts of the lifecycle where nodes have responsibilities.*/ - LifeCycleState getState() const; - - /**Simple indication of health */ - LifeCycleStatus getStatus() const; - - /** @brief string represenation of LifeCycleState*/ - const std::string_view& getStateName(); - - /** @brief string representation of LifeCycleStatus*/ - const std::string_view& getStatusName(); - - private: - /* Variables to help seperate business logic from AMLifeCycle ROS */ - AMLifeCycleMediator life_cycle_mediator_; - AMLifeCycleMediator::LifeCycleInfo life_cycle_info_; - AMLifeCycleMediator::ThrottleInfo throttle_info_; - - - /**The moment configuration is requested for this node. Used with - * max_configure_seconds_ to allow startup error tolerance.*/ - ros::Time configure_start_time_; - - void setState(const LifeCycleState state); - - /* if status is valid, then set this status to status */ - bool setStatus(const LifeCycleStatus status); - - void transition(std::string transition_name, LifeCycleState initial_state, LifeCycleState transition_state, - LifeCycleState new_state, std::function on_function); - void doTransition(std::string transition_name, bool success, LifeCycleState success_state, - LifeCycleState failure_state); - - //internal methods called to begin the transition. See on* for corresponding definitions. - void configure(); - void activate(); - void deactivate(); - void shutdown(); - void destroy(); - void cleanup(); - void sendNodeUpdate(); - void error(std::string message, std::string error_code, bool forced = false); - void configureStat(AMStat& stat, std::string name, std::string category=""); - - protected: - std::string node_name_; - - /**Maximum time errors will be ignored during configuration. */ - int configure_tolerance_s; - - diagnostic_updater::Updater updater_; - AMStatList stats_list_; - - ros::NodeHandle nh_; - ros::Timer heartbeat_timer_; - ros::Publisher state_pub_; - ros::Subscriber lifecycle_sub_; - - /** - * @brief Default constructor - */ - AMLifeCycle(); - - /** - * @brief Virtual destructor - */ - virtual ~AMLifeCycle(); - - template - - /** Exactly like ros::param, but logs INFO level showing the actual value assigned. - */ - bool param(const std::string& param_name, T& param_val, const T& default_val) const; - - //on* overriden by implementing node to do what is needed to satisfy the objective of the method - //do* is called by the implementing node when the objective attempt has completed and status is to be reported - /** - * @brief Function to be defined by the user. - * Called at the end of transition from INACTIVE to ACTIVE. - */ - virtual void onActivate(); - void doActivate(bool success); - - /** - * @brief Function to be defined by the user. - * Called at the end of transition from INACTIVE to UNCONFIGURED. - */ - virtual void onCleanup(); - void doCleanup(bool success); - - /** - * @brief Function to be defined by the user. - * Called at the end of transition from UNCONFIGURED to INACTIVE. - */ - virtual void onConfigure(); - void doConfigure(bool success); - - /** - * @brief true if configuring and within the time allowed to configure - */ - bool withinConfigureTolerance(); - - /** - * @brief Function to be defined by the user. - * Called at the end of transition from ACTIVE to INACTIVE. - */ - virtual void onDeactivate(); - void doDeactivate(bool success); - - /** - * @brief Function to be defined by the user. - * Called at the end of transition from FINALIZED to power off. - */ - virtual void onDestroy(); - void doDestroy(bool success); - - /** - * Called by all when an error has happened. Will set the status to ERROR and state to ERROR_PROCESSING - * which will eventually lead to FINALIZED. - * @param error_code provides a reference for the developer to correlate log output to the originating error. - * @param forced terminal error that will not allow any tolerance - */ - [[deprecated("use errorTolerant or errorTerminal with message")]] - void error(std::string error_code="NNLW",bool forced = false); - - /** Reports an error for immediate shutdown without any tolerance. */ - void errorTerminal(std::string message, std::string error_code); - - /** Reports an error, but may not shutdown the system if tolerance is allowed.*/ - void errorTolerant(std::string message, std::string error_code); - - /** - * @brief Function to be defined by the user. - * Called at any time and transitions to UNCONFIGURED or FINALIZED. - */ - virtual void onError(); - void doError(bool success); - - /** - * @brief Function to be defined by the user. - * Called at the end of transition from INACTIVE to FINALIZED. - */ - virtual void onShutdown(); - void doShutdown(bool success); - - /**Initialize statistics by adding to the list*/ - virtual void addStatistics(diagnostic_updater::DiagnosticStatusWrapper& dsw); - - - [[deprecated("configureHzStat with the stat's long name in the yaml")]] - AMStatReset& configureHzStats(AMStatReset& stats); - - /** Initialize the stats that reset once per second providing the equivalent of rostopic hz to ensure frequency of - * publishing. Allows for overriding values in roslaunch configurations. - * Provide the target, which is the approximate value you expect to receive. The warnings and errors will be - * provided with tolerance on both sides of the target. - * - * Configurations key use the stats long name. - * - * setting a target will also set a min/max 5% warn and 10% error - * no target allows for just min or just max or both. - * - * stats_target_sets_min_max: - * hz: - * target: 100 # sets min_error=90,min_warn=95,max_warn=105,max_error=110 - * - * stats_only_min: - * hz: - * error: - * min: 50 - * warn: - * min: 60 - * - * - * @param stats to be configured - * */ - AMStatReset& configureHzStat(AMStatReset& stats); - - /** Standard configuration of min/max or warn/error for a stat. - * - * Keyed by the stat long name in the yaml for configuration. - * - * my_stat: - * error: - * max: 85 - * warn: - * max: 70 - */ - void configureStat(AMStat& stat); - - /** Called periodically by a timer defaulting to 1 second. - * Useful for checking health regularly, but not during - * callbacks which can affect performance and be too granular - */ - virtual void heartbeatCB(const ros::TimerEvent& event); - - void lifecycleCB(const brain_box_msgs::LifeCycleCommand::ConstPtr msg); - - - double getThrottleS() const; - void setThrottleS(const double throttleS); - double getThrottle(); - - /**Providing consistency when logging the current state. */ - void logState(); - -}; // class AMLifeCycle - -}; // namespace am - -#endif diff --git a/include/super_lib/am_life_cycle_mediator.h b/include/super_lib/am_life_cycle_mediator.h deleted file mode 100644 index 4f8df45c..00000000 --- a/include/super_lib/am_life_cycle_mediator.h +++ /dev/null @@ -1,250 +0,0 @@ -#ifndef AM_LIFE_CYCLE_MEDIATOR_H_ -#define AM_LIFE_CYCLE_MEDIATOR_H_ - -#include -#include -#include - -typedef boost::bimap str_command_bimap; -typedef boost::bimap str_status_bimap; -typedef boost::bimap str_state_bimap; - -namespace am -{ - -/** Stateless methods providing function without coupling to ROS or any - * systems providing testable code. - */ -class AMLifeCycleMediator -{ - public: - static constexpr double DEFAULT_THROTTLE_S = 0.0; - - AMLifeCycleMediator(); - /** - * Holds information about AMLifeCycle - */ - struct LifeCycleInfo - { - LifeCycleStatus status; - LifeCycleState state; - }; - /** - * Holds information about throttle values for each status - */ - struct ThrottleInfo - { - double ok_throttle_s = DEFAULT_OK_THROTTLE_S; - double warn_throttle_s = DEFAULT_WARN_THROTTLE_S; - double error_throttle_s = DEFAULT_ERROR_THROTTLE_S; - }; - /** - * @brief Sets the current LifeCycleStatus in LifeCycleInfo - * - * @param status status that we want to set - * @param info mediator enum holding information about LifeCycle - * - * @returns true status is valid and mediator info.status was updated - * @returns false status is invalid and mediator info.status was not updated - */ - bool setStatus(const LifeCycleStatus& status, LifeCycleInfo& info); - - /** - * @brief Gets the current LifeCycleStatus from LifeCycleInfo - * - * @param info mediator enum holding information about LifeCycle - * - * @returns info.state - the current LifeCycleStatus in the mediator - */ - LifeCycleStatus getStatus(const LifeCycleInfo& info) const; - - /** - * @brief Sets the current LifeCycleState in LifeCycleInfo - * - * @param state state that we want to set - * @param info mediator enum holding information about LifeCycle - * - * @returns true state is valid and info.state was updated - * @returns false state is invalid and info.state was not updated - */ - bool setState(const LifeCycleState& state, LifeCycleInfo& info); - - /** - * @brief Gets the current LifeCycleState from LifeCycleInfo - * - * @param info mediator enum holding information about LifeCycle - * - * @returns info.state - the current LifeCycleState in info - */ - LifeCycleState getState(const LifeCycleInfo& info) const; - - /** - * @brief Flag for if status is error - * - * @param status current status - * - * @returns true - status is error, false otherwise - */ - bool statusError(const LifeCycleStatus& status) const; - - /** - * @brief Converts a LifeCycleCommand into its proper string representation. - * If the LifeCycleCommand is not a valid one, returns "" - * - * @param command LifeCycleCommand enum representing the command - * - * @returns The string that represents the command. "" if invalid. - */ - const std::string_view& commandToString(const LifeCycleCommand& command); - /** - * @brief Reads the string passed in and stores into 'command' the respective - * LifeCycleCommand. If the string is not a valid one, the 'command' passed in - * is unchanged - * - * @param command_str the string that is converted into a command and stored in 'command' - * @param command holds the current command - * - * @returns true if the command_str is valid and state was updated - * @returns false if the command_str is invalid and state was unchanged - */ - bool stringToCommand(const std::string& command_str, LifeCycleCommand& command); - - /** - * @brief Converts a LifeCycleStatus into its proper string representation. - * If the LifeCycleStatus is not a valid one, returns "" - * - * @param status LifeCycleStatus enum representing the status - * - * @returns The string that represents the status. "" if invalid. - */ - const std::string_view& statusToString(LifeCycleStatus status); - /** - * @brief Reads the string passed in and stores into 'status' the respective - * LifeCycleStatus. If the string is not a valid one, the 'status' passed in - * is unchanged - * - * @param status_str the string that is converted into a status and stored in 'status' - * @param status holds the current status - * - * @returns true if the status_str is valid and state was updated - * @returns false if the status_str is invalid and state was unchanged - */ - bool stringToStatus(std::string& status_str, LifeCycleStatus& status); - /** - * @brief Converts a LifeCycleState into the proper string representation. - * If the LifeCycleState is not a valid one, returns "INVALID" - * - * @param state LifeCycleState enum representing the state of LifeCycle - * - * @returns The string that represents the state. "INVALID" if invalid. - */ - const std::string_view& stateToString(LifeCycleState state); - - /** - * @brief Reads the string passed in and stores into 'state' the respective - * LifeCycleState. If the string is not a valid one, the 'state' passed in - * is unchanged - * - * @param state_str the string that is converted to a state and stored in 'state' - * @param state holds the current state - * - * @returns true if the state_str is valid and state was updated - * @returns false if the state_str is invalid and state was unchanged - */ - bool stringToState(std::string& state_str, LifeCycleState& state); - - /** - * @brief Stores all states for LifeCycleState into a vector - * @returns vector of LifeCycleStates - */ - static const std::vector getLifeCycleStates(); - - /** - * @brief Stores all commands for LifeCycleCommand into a vector - * @returns vector of LifeCycleCommands - */ - static const std::vector getLifeCycleCommands(); - - /** - * @brief Stores all status' for LifeCycleStatus into a vector - * @returns vector of LifeCycleStatus - */ - static const std::vector getLifeCycleStatuses(); - - /** - * @brief Sets the throttles to either their default or specified values - * - * @param throttleS 0.0 (default), anything else sets all throttles to number - * @param throttle struct containing throttle variables - */ - void setThrottleS(const double& throttleS, ThrottleInfo& throttle); - - /** - * @brief Grabs the throttle value based on current LifeCycle state - * - * @param info struct storing information about LifeCycle - * @param t struct storing information about throttle - * - * @returns double representing the current throttle - */ - double getThrottle(const AMLifeCycleMediator::LifeCycleInfo& info, const AMLifeCycleMediator::ThrottleInfo& t); - - bool shutdown(const AMLifeCycleMediator::LifeCycleInfo& info); - - bool redundantShutdown(const AMLifeCycleMediator::LifeCycleInfo& info); - - /** @brief return true if already in error state */ - bool redundantError(const AMLifeCycleMediator::LifeCycleInfo& info); - - /** @brief return true if in UNCONFIGURED or CONFIGURING */ - bool unconfigured(const AMLifeCycleMediator::LifeCycleInfo& info); - - bool illegalDestroy(const AMLifeCycleMediator::LifeCycleInfo& info); - - private: - static const LifeCycleStatus FIRST_STATUS = LifeCycleStatus::OK; - static const LifeCycleStatus LAST_STATUS = LifeCycleStatus::ERROR; - - static const LifeCycleState FIRST_STATE = LifeCycleState::INVALID; - static const LifeCycleState LAST_STATE = LifeCycleState::ERROR_PROCESSING; - - static const LifeCycleCommand FIRST_COMMAND = LifeCycleCommand::CREATE; - static const LifeCycleCommand LAST_COMMAND = LifeCycleCommand::DESTROY; - - str_command_bimap str_command_bimap_; - str_status_bimap str_status_bimap_; - str_state_bimap str_state_bimap_; - - /* String messages for mapping */ - static constexpr std::string_view STATE_INVALID_STRING = "INVALID"; - static constexpr std::string_view STATE_UNCONFIGURED_STRING = "UNCONFIGURED"; - static constexpr std::string_view STATE_INACTIVE_STRING = "INACTIVE"; - static constexpr std::string_view STATE_ACTIVE_STRING = "ACTIVE"; - static constexpr std::string_view STATE_FINALIZED_STRING = "FINALIZED"; - static constexpr std::string_view STATE_CONFIGURING_STRING = "CONFIGURING"; - static constexpr std::string_view STATE_CLEANING_UP_STRING = "CLEANING_UP"; - static constexpr std::string_view STATE_ACTIVATING_STRING = "ACTIVATING"; - static constexpr std::string_view STATE_DEACTIVATING_STRING = "DEACTIVATING"; - static constexpr std::string_view STATE_ERROR_PROCESSING_STRING = "ERROR_PROCESSING"; - static constexpr std::string_view STATE_SHUTTING_DOWN = "SHUTTING_DOWN"; - - static constexpr std::string_view STATUS_OK_STRING = "OK"; - static constexpr std::string_view STATUS_WARN_STRING = "WARN"; - static constexpr std::string_view STATUS_ERROR_STRING = "ERROR"; - - static constexpr std::string_view COMMAND_CREATE_STRING = "CREATE"; - static constexpr std::string_view COMMAND_CONFIGURE_STRING = "CONFIGURE"; - static constexpr std::string_view COMMAND_CLEANUP_STRING = "CLEANUP"; - static constexpr std::string_view COMMAND_ACTIVATE_STRING = "ACTIVATE"; - static constexpr std::string_view COMMAND_DEACTIVATE_STRING = "DEACTIVATE"; - static constexpr std::string_view COMMAND_SHUTDOWN_STRING = "SHUTDOWN"; - static constexpr std::string_view COMMAND_DESTROY_STRING = "DESTROY"; - - static constexpr std::string_view EMPTY_STRING = ""; - - static constexpr double DEFAULT_OK_THROTTLE_S = 10.0; - static constexpr double DEFAULT_WARN_THROTTLE_S = 2.0; - static constexpr double DEFAULT_ERROR_THROTTLE_S = 1.0; -}; -} -#endif // AM_LIFE_CYCLE_MEDIATOR_H_ \ No newline at end of file diff --git a/include/super_lib/am_life_cycle_types.h b/include/super_lib/am_life_cycle_types.h deleted file mode 100644 index a623891b..00000000 --- a/include/super_lib/am_life_cycle_types.h +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef AM_SUPER_INCLUDE_SUPER_LIB_AM_LIFECYCLE_TYPES_H_ -#define AM_SUPER_INCLUDE_SUPER_LIB_AM_LIFECYCLE_TYPES_H_ - -#include -#include -#include - -namespace am -{ -/** - * state is the lifecycle state which is about startup, shutdown, and error handling - */ -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 -}; - -/** - * status of the functionality of the node (i.e. is it operating to spec) - */ -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 -}; - -/** - * lifecycle commands to nodes to change state - */ -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, - - //FIXME: there should be no last command - LAST_COMMAND = brain_box_msgs::LifeCycleCommand::COMMAND_LAST -}; - -}; // namespace am -#endif diff --git a/include/super_lib/am_stat.h b/include/super_lib/am_stat.h deleted file mode 100644 index e1280304..00000000 --- a/include/super_lib/am_stat.h +++ /dev/null @@ -1,298 +0,0 @@ -#ifndef AM_SUPER_INCLUDE_SUPER_LIB_AM_STAT_CNT_H -#define AM_SUPER_INCLUDE_SUPER_LIB_AM_STAT_CNT_H - -#include -#include - -#include - -#include -#include - -namespace am -{ -/** - * Additive statistic looking for max thresholds exceeded during the entire Life Cycle. - * - * This is the base statistic that looks for max thresholds for warnings and errors compared to the value. - * The value is set or incremented using standard operators (=, +=, ++). - * - * See others: - * - * - AMStatReset has min/max thresholds for frequency checking. - * - AMStatAve is the average of value over count. - * - */ -class AMStat -{ -protected: - std::string short_name_ = "short"; - std::string long_name_ = "long"; - uint32_t value_ = 0; - uint32_t max_warn_ = std::numeric_limits::max(); - uint32_t max_error_ = std::numeric_limits::max(); - uint32_t min_warn_ = 0; - uint32_t min_error_ = 0; - /**indicates if min values are assigned */ - bool validate_min_ = false; - bool validate_max_ = false; - bool sample_received_ = false; - bool sample_required_ = false; -private: - AMStat(); - -public: - AMStat(const std::string& short_name, const std::string& long_name) - { - 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) - { - 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, - uint32_t max_warn, uint32_t max_error) - : AMStat(short_name, long_name,max_warn,max_error) - { - setMinError(min_error); - setMinWarn(min_warn); - } - - virtual ~AMStat() - { - } - - virtual LifeCycleStatus process(double warn_throttle_s, double error_throttle_s) - { - LifeCycleStatus status = LifeCycleStatus::OK; - - if(!sample_required_ || sample_received_) - { - if(validate_max_) - { - if (value_ > max_error_) - { - ROS_ERROR_STREAM_THROTTLE(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_ - << ") [PO9P]"); - compoundStatus(status, LifeCycleStatus::WARN); - } - } - - if(validate_min_) - { - if (value_ < min_error_) - { - ROS_ERROR_STREAM_THROTTLE(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_ - << ") [H9H8]"); - compoundStatus(status, LifeCycleStatus::WARN); - } - } - - 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]"); - } - } - else - { - //sample is required and not yet received - status = LifeCycleStatus::ERROR; - ROS_ERROR_STREAM_THROTTLE(error_throttle_s, long_name_ << " no samples received [NAQE] "); - } - return status; - } - - virtual void reset() - { - } - - virtual void addStatistics(diagnostic_updater::DiagnosticStatusWrapper& dsw) - { - dsw.add(long_name_, value_); - } - - virtual std::string getStrShort() - { - std::stringstream ss; - ss << short_name_ << ":" << value_; - return ss.str(); - } - - virtual std::string getStr() - { - std::stringstream ss; - ss << long_name_ << ": " << value_; - return ss.str(); - } - - virtual void add(uint32_t adder) - { - value_ += adder; - sample_received_ = true; - } - - AMStat& operator++(int) - { - value_++; - sample_received_ = true; - return *this; - } - - AMStat& operator+=(int adder) - { - value_ += adder; - sample_received_ = true; - return *this; - } - - AMStat& operator=(uint32_t assignment) - { - value_ = assignment; - sample_received_ = true; - return *this; - } - - /** Confusing name for return the value. Use getValue() instead*/ - [[deprecated]] - uint32_t getCount() const - { - return value_; - } - - /**The current value for the stat */ - uint32_t getValue() const - { - return value_; - } - - const std::string& getLongName() const - { - return long_name_; - } - - void setWarnError(uint32_t max_warn, uint32_t max_error) - { - setMaxWarn(max_warn); - setMaxError(max_error); - } - - uint32_t getMaxError() const - { - return max_error_; - } - - void setMaxError(uint32_t max_error) - { - max_error_ = max_error; - validate_max_=true; - } - - uint32_t getMaxWarn() const - { - return max_warn_; - } - - void setMaxWarn(uint32_t max_warn) - { - validate_max_=true; - max_warn_ = max_warn; - } - - void setWarnError(uint32_t min_error, uint32_t min_warn, uint32_t max_warn, uint32_t max_error) - { - setMinError(min_error); - setMinWarn(min_warn); - setWarnError(max_warn,max_error); - } - - uint32_t getMinError() const - { - return min_error_; - } - - void setMinError(uint32_t min_error) - { - validate_min_=true; - min_error_ = min_error; - } - - uint32_t getMinWarn() const - { - return min_warn_; - } - - void setMinWarn(uint32_t min_warn) - { - validate_min_=true; - min_warn_ = min_warn; - } - - const std::string& getShortName() const - { - return short_name_; - } - - - /**indicates if max values are assigned */ - const bool isValidatingMax() const - { - return validate_max_; - } - - /**indicates if min values are assigned */ - const bool isValidatingMin() const - { - return validate_min_; - } - - /**indicates if this stat has received a value since constructed.*/ - const bool isSampleReceived() const - { - return sample_received_; - } - - /**indicates if no samples received results in an error*/ - const bool isSampleRequired() const - { - return sample_required_; - } - - void setSampleRequired(bool sample_required) - { - sample_required_ = sample_required; - } - - static void compoundStatus(LifeCycleStatus& status, const LifeCycleStatus new_status) - { - if (new_status == LifeCycleStatus::ERROR) - { - status = LifeCycleStatus::ERROR; - } - else if (new_status == LifeCycleStatus::WARN && status == LifeCycleStatus::OK) - { - status = LifeCycleStatus::WARN; - } - } -}; - -}; // namespace am - -#endif // AM_OUSTER_OUSTER_STATS_H diff --git a/include/super_lib/am_stat_ave.h b/include/super_lib/am_stat_ave.h deleted file mode 100644 index f978940b..00000000 --- a/include/super_lib/am_stat_ave.h +++ /dev/null @@ -1,234 +0,0 @@ -#ifndef AM_SUPER_INCLUDE_SUPER_LIB_AM_STAT_AVE_H -#define AM_SUPER_INCLUDE_SUPER_LIB_AM_STAT_AVE_H - -#include - -namespace am -{ - -/** Calculates the recent average, min and max of a value, over the past second since it - * is reset upon the Life Cycle heartbeat. - */ -class AMStatAve : public AMStatReset -{ -protected: - uint64_t total_ = 0; - uint32_t max_ = 0; - uint32_t min_ = std::numeric_limits::max(); - uint32_t min_min_warn_ = 0; - uint32_t min_min_error_ = 0; - uint32_t max_max_warn_ = std::numeric_limits::max(); - uint32_t max_max_error_ = std::numeric_limits::max(); - -private: - AMStatAve(); - -public: - AMStatAve(const std::string& short_name, const std::string& long_name) : AMStatReset(short_name, long_name) - { - } - - AMStatAve(const std::string& short_name, const std::string& long_name, uint32_t max_warn, uint32_t max_error) - : AMStatReset(short_name, long_name, max_warn, max_error) - { - } - - AMStatAve(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) - : AMStatReset(short_name, long_name, min_error, min_warn, max_warn, max_error) - { - } - - AMStatAve(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, uint32_t min_min_error = 0, uint32_t min_min_warn = 0, - uint32_t max_max_warn = std::numeric_limits::max(), - uint32_t max_max_error = std::numeric_limits::max()) - : AMStatReset(short_name, long_name, min_error, min_warn, max_warn, max_error) - { - min_min_error_ = min_min_error; - min_min_warn_ = min_min_warn; - max_max_warn_ = max_max_warn; - max_max_error_ = max_max_error; - } - - LifeCycleStatus process(double warn_throttle_s, double error_throttle_s) override - { - uint32_t ave = getAve(); - LifeCycleStatus status = LifeCycleStatus::OK; - - if (ave > max_error_) - { - ROS_ERROR_STREAM_THROTTLE(error_throttle_s, long_name_ << " ave exceeding max_error: " << ave - << " (max:" << max_error_ << ")"); - compoundStatus(status, LifeCycleStatus::ERROR); - } - else if (ave > max_warn_) - { - ROS_WARN_STREAM_THROTTLE(warn_throttle_s, long_name_ << " ave exceeding max_warn: " << ave - << " (max:" << max_warn_ << ")"); - compoundStatus(status, LifeCycleStatus::WARN); - } - - if (ave < min_error_) - { - ROS_ERROR_STREAM_THROTTLE(error_throttle_s, long_name_ << " ave exceeding min_error: " << ave - << " (min:" << min_error_ << ")"); - compoundStatus(status, LifeCycleStatus::ERROR); - } - else if (ave < min_warn_) - { - ROS_WARN_STREAM_THROTTLE(warn_throttle_s, long_name_ << " ave exceeding min_warn: " << ave - << " (min:" << min_warn_ << ")"); - compoundStatus(status, LifeCycleStatus::WARN); - } - - if (min_ < min_min_error_) - { - ROS_ERROR_STREAM_THROTTLE(error_throttle_s, long_name_ << " min exceeding min_min_error: " << min_ - << " (min:" << min_min_error_ << ")"); - compoundStatus(status, LifeCycleStatus::ERROR); - } - else if (min_ < min_min_warn_) - { - ROS_WARN_STREAM_THROTTLE(warn_throttle_s, long_name_ << " min exceeding min_min_warn: " << min_ - << " (min:" << min_min_warn_ << ")"); - compoundStatus(status, LifeCycleStatus::WARN); - } - - if (max_ > max_max_error_) - { - ROS_ERROR_STREAM_THROTTLE(error_throttle_s, long_name_ << " max exceeding max_max_error: " << max_ - << " (max:" << max_max_error_ << ")"); - compoundStatus(status, LifeCycleStatus::ERROR); - } - else if (max_ > max_max_warn_) - { - ROS_WARN_STREAM_THROTTLE(warn_throttle_s, long_name_ << " max exceeding max_max_warn: " << max_ - << " (max:" << max_max_warn_ << ")"); - compoundStatus(status, LifeCycleStatus::WARN); - } - - return status; - } - - void reset() override - { - value_ = 0; - total_ = 0; - max_ = 0; - min_ = std::numeric_limits::max(); - } - - void add(uint32_t value) override - { - total_ += value; - value_++; - if (value < min_) - { - min_ = value; - } - if (value > max_) - { - max_ = value; - } - } - - uint32_t getAve() - { - uint64_t ave_64 = ((float)total_ / (float)value_ + 0.5); - uint32_t ave_32 = ave_64 > std::numeric_limits::max() ? std::numeric_limits::max() : ave_64; - return ave_32; - } - - void addStatistics(diagnostic_updater::DiagnosticStatusWrapper& dsw) override - { - dsw.add(long_name_ + " Ave", getAve()); - dsw.add(long_name_ + " Max", getMax()); - dsw.add(long_name_ + " Min", getMin()); - } - - std::string getStrShort() override - { - std::stringstream ss; - ss << short_name_ << "-av:" << getAve() << "," << short_name_ << "-mx:" << getMax() << "," << short_name_ - << "-mn:" << getMin(); - return ss.str(); - } - - std::string getStr() override - { - std::stringstream ss; - ss << long_name_ << " Ave: " << getAve() << "," << long_name_ << " Max: " << getMax() << "," << long_name_ - << " Min: " << getMin(); - return ss.str(); - } - - uint32_t getMax() const - { - return max_; - } - - uint32_t getMaxMaxError() const - { - return max_max_error_; - } - - uint32_t getMaxMaxWarn() const - { - return max_max_warn_; - } - - uint32_t getMin() const - { - return min_; - } - - uint32_t getMinMinError() const - { - return min_min_error_; - } - - uint32_t getMinMinWarn() const - { - return min_min_warn_; - } - - uint64_t getTotal() const - { - return total_; - } - - void setMax(uint32_t max = 0) - { - max_ = max; - } - - void setMaxMaxError(uint32_t maxMaxError = std::numeric_limits::max()) - { - max_max_error_ = maxMaxError; - } - - void setMaxMaxWarn(uint32_t maxMaxWarn = std::numeric_limits::max()) - { - max_max_warn_ = maxMaxWarn; - } - - void setMin(uint32_t min = std::numeric_limits::max()) - { - min_ = min; - } - - void setMinMinError(uint32_t minMinError = 0) - { - min_min_error_ = minMinError; - } - - void setMinMinWarn(uint32_t minMinWarn = 0) - { - min_min_warn_ = minMinWarn; - } -}; - -}; // namespace am - -#endif // AM_SUPER_INCLUDE_SUPER_LIB_AM_STAT_AVE_H diff --git a/include/super_lib/am_stat_list.h b/include/super_lib/am_stat_list.h deleted file mode 100644 index 97e69585..00000000 --- a/include/super_lib/am_stat_list.h +++ /dev/null @@ -1,85 +0,0 @@ -#ifndef AM_SUPER_INCLUDE_SUPER_LIB_AM_STAT_LIST_H -#define AM_SUPER_INCLUDE_SUPER_LIB_AM_STAT_LIST_H - -#include - -#include -#include - -namespace am -{ -/** - * Specialized collection assisting with managing multiple stats simultaneously. - */ -class AMStatList -{ -protected: - std::vector stat_list_; - -public: - AMStatList() - { - } - - void add(AMStat* stat) - { - stat_list_.push_back(stat); - } - - LifeCycleStatus process(double warn_throttle_s, double error_throttle_s) - { - LifeCycleStatus status = LifeCycleStatus::OK; - - for (AMStat* stat : stat_list_) - { - AMStat::compoundStatus(status, stat->process(warn_throttle_s, error_throttle_s)); - } - - return status; - } - - void reset() - { - for (AMStat* stat : stat_list_) - { - stat->reset(); - } - } - - void addStatistics(diagnostic_updater::DiagnosticStatusWrapper& dsw) - { - for (AMStat* stat : stat_list_) - { - stat->addStatistics(dsw); - } - } - - std::string getStatsStrShort() - { - std::stringstream ss; - for (AMStat* stat : stat_list_) - { - ss << stat->getStrShort() << ","; - } - return ss.str(); - } - - std::string getStatsStr() - { - std::stringstream ss; - for (AMStat* stat : stat_list_) - { - ss << stat->getStr() << ", "; - } - return ss.str(); - } - /** simple method to indicate if we are using stats for a node */ - bool hasStats() const - { - return !stat_list_.empty(); - } -}; - -}; // namespace am - -#endif // AM_SUPER_INCLUDE_SUPER_LIB_AM_STAT__LIST_H diff --git a/include/super_lib/am_stat_reset.h b/include/super_lib/am_stat_reset.h deleted file mode 100644 index 08097cd6..00000000 --- a/include/super_lib/am_stat_reset.h +++ /dev/null @@ -1,60 +0,0 @@ -#ifndef AM_SUPER_INCLUDE_SUPER_LIB_AM_STAT_CNT_RESET_H -#define AM_SUPER_INCLUDE_SUPER_LIB_AM_STAT_CNT_RESET_H - -#include - -namespace am -{ - -/** - * Additive statics reporting minimum/maximum thresholds where the value is reset - * upon every Life Cycle heartbeat of 1 second. This is used for frequency validation - * like rostopic hz shows. - * - */ -class AMStatReset : public AMStat -{ - -private: - AMStatReset(); - void init() - { - sample_required_ = true; - } -public: - AMStatReset(const std::string& short_name, const std::string& long_name) : AMStat(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) - { - init(); - } - - AMStatReset(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) - { - init(); - } - - void reset() override - { - value_ = 0; - } - - AMStatReset& operator=(uint32_t assignment) - { - sample_received_ = true; - value_ = assignment; - return *this; - } - -}; - - -}; // namespace am - -#endif // AM_SUPER_INCLUDE_SUPER_LIB_AM_STAT_CNT_H diff --git a/include/super_lib/am_super_topics.h b/include/super_lib/am_super_topics.h deleted file mode 100644 index 7852fe46..00000000 --- a/include/super_lib/am_super_topics.h +++ /dev/null @@ -1,31 +0,0 @@ -#ifndef AM_SUPER_LIB_TOPICS_H -#define AM_SUPER_LIB_TOPICS_H - -#include - -namespace am -{ - -class am_super_topics -{ -public: - /** @brief Operator interacting with the system */ - static constexpr char OPERATOR_COMMAND[] = "/operator/command"; - - /** Controller of the mission sends State to advance through lifecycle */ - static constexpr char CONTROLLER_STATE[] = "/controller/state"; - - /** State of super as reported by super */ - static constexpr char SUPER_STATE[] = "/vstate/summary"; - - static constexpr char LIFECYCLE_STATE[] = "/node_state"; - - static constexpr char SUPER_STATUS[] = "/super/status"; - - static constexpr char NODE_LIFECYCLE[] = "/node_lifecycle"; - -}; - -} - -#endif diff --git a/launch/am_super.yaml b/launch/am_super.yaml index e4502276..255fa3c5 100644 --- a/launch/am_super.yaml +++ b/launch/am_super.yaml @@ -1,3 +1,4 @@ - -platform: - actual: $(arg platform) \ No newline at end of file +am_super: + ros__parameters: + platform: + actual: farm-ng_amiga_mockup 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..be82e0f0 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,7 @@ #include #include #include + #if CUDA_FLAG #include #endif @@ -48,37 +50,35 @@ namespace am * * uses BabySitter instances to generate state and health for nodes that don't publish brain_box_msgs::LifeCycleState */ -class AMSuper : AMLifeCycle +class AMSuper { + friend class AMSuperNode; + private: + shared_ptr life_cycle_node_; + /** * heartbeat log output period */ const int LOG_THROTTLE_S = 10; - /** - * the ros node handle - */ - ros::NodeHandle 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::Subscription::SharedPtr log_control_sub_; BagLogger::BagLoggerLevel log_level_; /** manage logic for SuperState transitions */ @@ -108,13 +108,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 +124,14 @@ class AMSuper : AMLifeCycle #endif public: - AMSuper() : nh_("~"), node_mediator_(SuperNodeMediator::nodeNameStripped(ros::this_node::getName())) + AMSuper() : node_mediator_(am::Node::node, SuperNodeMediator::nodeNameStripped(am::Node::node->get_name())) { - ROS_INFO_STREAM(NODE_FUNC); + ROS_INFO_STREAM( am::Node::node->get_name()); - ros::param::param("~node_timeout_s", node_timeout_s_, 2.0); - ROS_INFO_STREAM("node_timeout_s = " << node_timeout_s_); + life_cycle_node_ = std::static_pointer_cast(am::Node::node); + + am::getParam("node_timeout_s", node_timeout_s_, 2.0); + ROS_INFO_STREAM( "node_timeout_s = " << node_timeout_s_); /* * create initial node list from manifest and create babysitters as needed @@ -137,7 +139,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("manifest", manifest_param, ""); node_mediator_.parseManifest(supervisor_, manifest_param); @@ -147,13 +149,13 @@ class AMSuper : AMLifeCycle // if a manifest has been specified if (!supervisor_.manifest.empty()) { - ROS_INFO_STREAM("configuring nodes from manifest: " << manifest_param); + ROS_INFO_STREAM( "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); + ROS_INFO_STREAM( " " << name); // create babysitters based on hard coded node names if (!name.compare(NODE_BS_ALTIMETER)) @@ -161,20 +163,20 @@ class AMSuper : AMLifeCycle int altimeter_warn_ms, altimeter_error_ms; calcBSTiming(ALTIMETER_HZ, altimeter_warn_ms, altimeter_error_ms); altimeter_bs_ = new am::BabySitter( - nh_, BagLogger::instance(), name, ALTIMETER_BS_TOPIC, altimeter_warn_ms, altimeter_error_ms); + am::Node::node, BagLogger::instance(), name, ALTIMETER_BS_TOPIC, altimeter_warn_ms, altimeter_error_ms); } else if (!name.compare(NODE_BS_DJI)) { int dji_warn_ms, dji_error_ms; calcBSTiming(DJI_HZ, dji_warn_ms, dji_error_ms); - dji_bs_ = new am::BabySitter(nh_, BagLogger::instance(), name, DJI_BS_TOPIC, dji_warn_ms, + dji_bs_ = new am::BabySitter(am::Node::node, BagLogger::instance(), name, DJI_BS_TOPIC, dji_warn_ms, dji_error_ms); } } } else { - ROS_WARN_STREAM("Manifest is empty. No nodes will be monitored."); + RCLCPP_WARN_STREAM(am::Node::node->get_logger(), "Manifest is empty. No nodes will be monitored."); } reportSystemState(); @@ -187,22 +189,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_ = am::Node::node->create_publisher(am_super_topics::SUPER_STATE, 1000); + system_state_pub_ = am::Node::node->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_ = am::Node::node->create_publisher(am_super_topics::NODE_LIFECYCLE, 100); /** * led control pub */ - led_pub_ = nh_.advertise(am::am_topics::LED_BLINK, 1000); + led_pub_ = am::Node::node->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_ = am::Node::node->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_ = am::Node::node->create_publisher(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, 1000); supervisor_.system_state = SuperState::BOOTING; supervisor_.flt_ctrl_state = SuperNodeMediator::SuperFltCtrlState::INIT; @@ -210,10 +212,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_ = am::Node::node->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); + ROS_INFO_STREAM( "start logging to ST, level " << SU_LOG_LEVEL); BagLogger::instance()->startLogging("ST", SU_LOG_LEVEL); log_level_ = intToLoggerLevel (SU_LOG_LEVEL); @@ -222,21 +225,24 @@ class AMSuper : AMLifeCycle /** * node status via LifeCycle */ - node_state_sub_ = nh_.subscribe(am_super_topics::LIFECYCLE_STATE, 100, &AMSuper::nodeStateCB, this); + node_state_sub_ = am::Node::node->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); - - controller_state_sub = nh_.subscribe(am_super_topics::CONTROLLER_STATE, 100, &AMSuper::controllerStateCB, this); + operator_command_sub_ = am::Node::node->create_subscription(am_super_topics::OPERATOR_COMMAND, 100, + std::bind(&AMSuper::operatorCommandCB, this, std::placeholders::_1)); - diagnostics_sub = nh_.subscribe("/diagnostics", 100, &AMSuper::diagnosticsCB, this); + controller_state_sub = am::Node::node->create_subscription(am_super_topics::CONTROLLER_STATE, 100, + std::bind(&AMSuper::controllerStateCB, this, std::placeholders::_1)); - current_enu_sub = nh_.subscribe(am_topics::CTRL_VX_VEHICLE_CURRENTENU, 100, &AMSuper::currentENUCB, this); + diagnostics_sub = am::Node::node->create_subscription("/diagnostics", 100, + std::bind(&AMSuper::diagnosticsCB, this, std::placeholders::_1)); - heartbeat_timer_ = nh_.createTimer(ros::Duration(1.0), &AMSuper::heartbeatCB, this); - } + current_enu_sub = am::Node::node->create_subscription(am_topics::CTRL_VX_VEHICLE_CURRENTENU, 100, + std::bind(&AMSuper::currentENUCB, this, std::placeholders::_1)); + } ~AMSuper() { @@ -253,9 +259,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 +272,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(am::Node::node->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(am::Node::node->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 +310,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 +325,25 @@ class AMSuper : AMLifeCycle SuperNodeMediator::SuperNodeInfo& nr = it->second; if (!nr.online) { - ROS_INFO_STREAM("manifested node '" << node_name << "' came online [PGPG]"); + ROS_INFO_STREAM( "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]"); + ROS_INFO_STREAM( 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]"); + ROS_INFO_STREAM( 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]"); + ROS_INFO_STREAM( "Manifested node " << nr.name << " changed status to ERROR. Shutting down nodes... [JHRE]"); stopFlightPlan(); } } @@ -343,11 +352,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]"); + ROS_INFO_STREAM( node_name << " process is alive [UIRE]"); } else { - ROS_WARN_STREAM(node_name << " changed process id from: " << nr.pid << " to: " << pid << " [WNEW]"); + ROS_INFO_STREAM( node_name << " changed process id from: " << nr.pid << " to: " << pid << " [WNEW]"); } nr.pid = pid; nodes_changed = true; @@ -357,7 +366,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(am::Node::node->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,110 +402,13 @@ class AMSuper : AMLifeCycle } if (flt_ctrl_state_changed) { - ROS_INFO_STREAM_THROTTLE(1.0, "flight status: " << value); + RCLCPP_INFO_STREAM_THROTTLE(am::Node::node->get_logger(), *am::Node::node->get_clock(), 1.0, "flight status: " << value); checkForSystemStateTransition(); } } } - /** - * called once per second. - * - * 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 - { -#if CUDA_FLAG - gpu_info_->display(); -#endif - - //publish deprecated topic - { - brain_box_msgs::VxState state_msg; - state_msg.state = (uint8_t)supervisor_.system_state; - vstate_summary_pub_.publish(state_msg); - } - - //publish the system state - { - brain_box_msgs::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); - } - - // cycle thru all the nodes in the list to look for a timeout - ros::Time now = ros::Time().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_); - if (time_since_contact > timeout_dur) - { - nr.online = false; - ROS_ERROR_STREAM("node timed out:" << nr.name); - reportSystemState(); - } - } - } - - // check for state transition due to timeouts or anything else that changed since last heartbeat - checkForSystemStateTransition(); - - int num_manifest_nodes_online = node_mediator_.manifestedNodesOnlineCount(supervisor_); - // publish and bag log super status message - brain_box_msgs::Super2Status status_msg; - status_msg.man = supervisor_.manifest.size(); - status_msg.man_run = num_manifest_nodes_online; - status_msg.run = node_mediator_.nodesOnlineCount(supervisor_); - - for (it = supervisor_.nodes.begin(); it != supervisor_.nodes.end(); it++) - { - SuperNodeMediator::SuperNodeInfo& nr = (*it).second; - status_msg.nodes.push_back(nr.name); - } - LOG_MSG("/status/super", status_msg, 1); - if (super_status_pub_.getNumSubscribers() > 0) - { - super_status_pub_.publish(status_msg); - } - - // report current status to trace log - std::stringstream ss; - genSystemState(ss); - - 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_)); - } - else - { - // if all manifested nodes are running, report as info - ROS_INFO_STREAM_THROTTLE(LOG_THROTTLE_S, ss.str()); - } - - // log stats - fstream newfile; - newfile.open("/sys/bus/i2c/devices/7-0040/iio_device/in_power0_input",ios::in); //open a file to perform read operation using file object - if (newfile.is_open()) - { //checking whether the file is open - string tp; - getline(newfile, tp); - std_msgs::Int16 msg; - msg.data = std::stoi(tp); - LOG_MSG("/watts", msg, SU_LOG_LEVEL); - newfile.close(); //close the file object. - } - - AMLifeCycle::heartbeatCB(event); - } - + /** * update stream with system state and status */ @@ -529,10 +441,10 @@ 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; + 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); } /** @@ -564,10 +476,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); + ROS_ERROR_STREAM( "Sending flight plan kill command."); } /** @@ -578,7 +490,7 @@ class AMSuper : AMLifeCycle */ void checkForSystemStateTransition() { - if(getState() == LifeCycleState::INACTIVE && supervisor_.system_state == SuperState::READY) //if super lifecycle is currently inactive + if(life_cycle_node_->getState() == LifeCycleState::INACTIVE && supervisor_.system_state == SuperState::READY) //if super lifecycle is currently inactive { sendLifeCycleCommand(node_mediator_.getNodeName(), LifeCycleCommand::ACTIVATE); } @@ -638,39 +550,12 @@ 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); } } - /** - * Verify the basic requirements are being met: - * - platform required matches actual platform - */ - void onConfigure() - { - - 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); - if(!node_mediator_.isCorrectPlatform(required_platform,actual_platform)) - { - std::stringstream message; - message << "Platform required: `" - << node_mediator_.platformVariantToConfig(required_platform) - << "` actual: `" - << node_mediator_.platformVariantToConfig(actual_platform) - ; - errorTerminal(message.str(),"NSK2"); //force failure since this is not recoverable - } - else - { - AMLifeCycle::onConfigure(); - } - } /** load the platform configurations from the launch file and populate the variants provided. */ @@ -680,10 +565,10 @@ class AMSuper : AMLifeCycle //actual platform is required or we fail std::string not_provided = "none"; std::string actual_platform_param; - param("platform/actual",actual_platform_param,not_provided); + life_cycle_node_->param("platform.actual",actual_platform_param,not_provided); if(actual_platform_param == not_provided) { - errorTerminal("param `/am_super/platform/actual` must provide the platform running","NNS9"); + life_cycle_node_->errorTerminal("param `am_super.platform.actual` must provide the platform running","NNS9"); return; } node_mediator_.platformConfigToVariant(actual_platform_param,actual_platform); @@ -691,8 +576,8 @@ class AMSuper : AMLifeCycle //compare actual platform to required platform, if provided std::string required_platform_param; std::string platform_app_required_param; - param("platform/required",required_platform_param,not_provided); - param("platform/app/required",platform_app_required_param,not_provided); + life_cycle_node_->param("platform/required",required_platform_param,not_provided); + life_cycle_node_->param("platform/app/required",platform_app_required_param,not_provided); if(required_platform_param != not_provided) { node_mediator_.platformConfigToVariant(required_platform_param,required_platform); @@ -712,12 +597,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 +703,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,7 +735,7 @@ 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) { @@ -864,18 +749,188 @@ class AMSuper : AMLifeCycle } }; + +class AMSuperNode : public AMLifeCycle +{ +private: + shared_ptr am_super_; + +public: + AMSuperNode(const std::string & node_name) : AMLifeCycle(node_name) + { + } + + ~AMSuperNode() + { + } + + void setAMSuper(shared_ptr am_super) + { + am_super_= am_super; + } + +/** + * Verify the basic requirements are being met: + * - platform required matches actual platform + */ + void onConfigure() override + { + if(am_super_ == nullptr) + { + AMLifeCycle::onConfigure(); + return; + } + + SuperNodeMediator::PlatformVariant required_platform; + SuperNodeMediator::PlatformVariant actual_platform; + am_super_->configurePlatformRequirements(required_platform, actual_platform); + ROS_WARN_STREAM("required" << required_platform.maker); + ROS_WARN_STREAM("actual" << actual_platform.maker); + if(!am_super_->node_mediator_.isCorrectPlatform(required_platform,actual_platform)) + { + std::stringstream message; + message << "Platform required: `" + << am_super_->node_mediator_.platformVariantToConfig(required_platform) + << "` actual: `" + << am_super_->node_mediator_.platformVariantToConfig(actual_platform) + ; + errorTerminal(message.str(),"NSK2"); //force failure since this is not recoverable + } + else + { + AMLifeCycle::onConfigure(); + } + } + + /** + * called once per second. + * + * times out nodes that haven't been heard from recently. reports on status to bag and trace logs. + */ + void heartbeatCB() override + { + if(am_super_ == nullptr) + { + AMLifeCycle::heartbeatCB(); + return; + } + +#if CUDA_FLAG + gpu_info_->display(); +#endif + + //publish deprecated topic + { + brain_box_msgs::msg::VxState state_msg; + state_msg.state = (uint8_t)am_super_->supervisor_.system_state; + am_super_->vstate_summary_pub_->publish(state_msg); + } + + //publish the system state + { + brain_box_msgs::msg::SystemState system_state_msg; + system_state_msg.state = (uint8_t)am_super_->supervisor_.system_state; + system_state_msg.state_string = am_super_->state_mediator_.stateToString(am_super_->supervisor_.system_state); + am_super_->system_state_pub_->publish(system_state_msg); + } + + // cycle thru all the nodes in the list to look for a timeout + rclcpp::Time now = am::Node::node->now(); + map::iterator it; + for (it = am_super_->supervisor_.nodes.begin(); it != am_super_->supervisor_.nodes.end(); it++) + { + SuperNodeMediator::SuperNodeInfo& nr = (*it).second; + if (nr.online) + { + rclcpp::Duration time_since_contact = (now - nr.last_contact); + rclcpp::Duration timeout_dur(am::toDuration(am_super_->node_timeout_s_)); + if (time_since_contact > timeout_dur) + { + nr.online = false; + ROS_ERROR_STREAM("node timed out:" << nr.name); + am_super_->reportSystemState(); + } + } + } + + // check for state transition due to timeouts or anything else that changed since last heartbeat + am_super_->checkForSystemStateTransition(); + + int num_manifest_nodes_online = am_super_->node_mediator_.manifestedNodesOnlineCount(am_super_->supervisor_); + // publish and bag log super status message + brain_box_msgs::msg::Super2Status status_msg; + status_msg.man = am_super_->supervisor_.manifest.size(); + status_msg.man_run = num_manifest_nodes_online; + status_msg.run = am_super_->node_mediator_.nodesOnlineCount(am_super_->supervisor_); + + for (it = am_super_->supervisor_.nodes.begin(); it != am_super_->supervisor_.nodes.end(); it++) + { + SuperNodeMediator::SuperNodeInfo& nr = (*it).second; + status_msg.nodes.push_back(nr.name); + } + LOG_MSG("/status/super", status_msg, 1); + if (am_super_->super_status_pub_->get_subscription_count() > 0) + { + am_super_->super_status_pub_->publish(status_msg); + } + + // report current status to trace log + std::stringstream ss; + am_super_->genSystemState(ss); + + if (am_super_->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: " << am_super_->node_mediator_.manifestedNodesNotOnlineNamesList(am_super_->supervisor_)); + } + else + { + // if all manifested nodes are running, report as info + ROS_INFO_STREAM_THROTTLE(am_super_->LOG_THROTTLE_S, ss.str()); + } + + // log stats + fstream newfile; + newfile.open("/sys/bus/i2c/devices/7-0040/iio_device/in_power0_input",ios::in); //open a file to perform read operation using file object + if (newfile.is_open()) + { //checking whether the file is open + string tp; + getline(newfile, tp); + std_msgs::msg::Int16 msg; + msg.data = std::stoi(tp); + LOG_MSG("/watts", msg, am_super_->SU_LOG_LEVEL); + newfile.close(); //close the file object. + } + + AMLifeCycle::heartbeatCB(); + } + }; -#ifdef TESTING -#else +}; // namespace + +// #ifdef TESTING +// #else + +shared_ptr am::Node::node; + int main(int argc, char** argv) { - ros::init(argc, argv, ros::this_node::getName()); + rclcpp::init(argc, argv); + + shared_ptr am_super_node = make_shared("am_super"); + am::Node::node = am_super_node; - am::AMSuper node; + std::shared_ptr am_super = make_shared(); + am_super_node->setAMSuper(am_super); - ROS_INFO_STREAM(ros::this_node::getName() << ": running..."); + ROS_INFO_STREAM(am::Node::node->get_name() << ": running..."); - ros::spin(); + rclcpp::spin(am::Node::node); + + rclcpp::shutdown(); + + return 0; } -#endif +// #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 deleted file mode 100644 index 9b157b9f..00000000 --- a/src/super_lib/am_life_cycle.cpp +++ /dev/null @@ -1,559 +0,0 @@ -#include -#include -#include -#include - - - - -namespace am -{ - - -// static constexpr std::string AMLifeCycle::STATE_INVALID_STRING; -// static constexpr std::string AMLifeCycle::STATE_UNCONFIGURED_STRING; - -AMLifeCycle::AMLifeCycle() : nh_("~") -{ - std::string init_state_str; - //FIXME: This string should come from the enum - // always prefix with life_cycle in the parent to avoid namespace collisions with child and clarity in definition - // param would be: `/am_child_node/life_cycle/some_param` so no conflict with `/am_child_node/some_param` - std::string param_prefix="life_cycle/"; - - //deprecated - prefix with life_cycle instead - std::string default_state = "UNCONFIGURED"; - param("init_state", init_state_str, default_state); - param(param_prefix + "init_state", init_state_str, default_state); - - //deprecated - prefix with life_cycle instead - param("configure_tolerance_s", configure_tolerance_s, 10); - param(param_prefix + "configure_tolerance_s", configure_tolerance_s, configure_tolerance_s); - - LifeCycleState init_state; - if (life_cycle_mediator_.stringToState(init_state_str, init_state)) - { - life_cycle_info_.state = init_state; - } - else - { - life_cycle_info_.state = LifeCycleState::ACTIVE; - } - life_cycle_info_.status = LifeCycleStatus::OK; - state_pub_ = nh_.advertise("/node_state", 100); - - updater_.setHardwareID("none"); - updater_.broadcast(0, "Initializing node"); - updater_.add("diagnostics", this, &AMLifeCycle::addStatistics); - updater_.force_update(); - - // 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) == '/') - { - node_name_ = ros::this_node::getName().substr(1); - } - else - { - node_name_ = ros::this_node::getName(); - } - - - - // subs should always come at the end - /** - * node status via LifeCycle - */ - lifecycle_sub_ = nh_.subscribe("/node_lifecycle", 100, &AMLifeCycle::lifecycleCB, this); - - heartbeat_timer_ = nh_.createTimer(ros::Duration(1.0), &AMLifeCycle::heartbeatCB, this); - -} - -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); - return result; -} - -void AMLifeCycle::lifecycleCB(const brain_box_msgs::LifeCycleCommand::ConstPtr msg) -{ - ROS_DEBUG_STREAM_THROTTLE(1.0, life_cycle_mediator_.commandToString((LifeCycleCommand)msg->command)); - - if (!msg->node_name.compare(AMLifeCycle::BROADCAST_NODE_NAME) || !msg->node_name.compare(node_name_)) - { - // if this message is for us - switch ((LifeCycleCommand)msg->command) - { - case LifeCycleCommand::ACTIVATE: - transition("activate", LifeCycleState::INACTIVE, LifeCycleState::ACTIVATING, LifeCycleState::ACTIVE, - std::bind(&AMLifeCycle::onActivate, this)); - break; - case LifeCycleCommand::CLEANUP: - transition("cleanup", LifeCycleState::INACTIVE, LifeCycleState::CLEANING_UP, LifeCycleState::UNCONFIGURED, - std::bind(&AMLifeCycle::onCleanup, this)); - break; - case LifeCycleCommand::CONFIGURE: - configure(); - break; - case LifeCycleCommand::CREATE: - ROS_WARN_STREAM("illegal command " << life_cycle_mediator_.commandToString(LifeCycleCommand::CREATE) << " [7YT8]"); - break; - case LifeCycleCommand::DEACTIVATE: - transition("deactivate", LifeCycleState::ACTIVE, LifeCycleState::DEACTIVATING, LifeCycleState::INACTIVE, - std::bind(&AMLifeCycle::onDeactivate, this)); - break; - case LifeCycleCommand::DESTROY: - destroy(); - break; - case LifeCycleCommand::SHUTDOWN: - shutdown(); - break; - } - } -} - -void AMLifeCycle::transition(std::string transition_name, LifeCycleState initial_state, LifeCycleState transition_state, - LifeCycleState final_state, std::function on_function) -{ - if (life_cycle_info_.state == initial_state) - { - ROS_INFO_STREAM(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]"); - } - else - { - ROS_WARN_STREAM("received illegal " << transition_name << " in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [JGV5]"); - } -} - -void AMLifeCycle::doTransition(std::string transition_name, bool success, LifeCycleState success_state, - LifeCycleState failure_state) -{ - logState(); - if (success) - { - ROS_INFO_STREAM(transition_name << " succeeded"); - setState(success_state); - } - else - { - ROS_INFO_STREAM(transition_name << " failed"); - setState(failure_state); - } -} - -void AMLifeCycle::onActivate() -{ - logState(); - doActivate(true); -} - -void AMLifeCycle::doActivate(bool success) -{ - doTransition("activation", success, LifeCycleState::ACTIVE, LifeCycleState::INACTIVE); -} - -void AMLifeCycle::onCleanup() -{ - logState(); - doCleanup(true); -} - -void AMLifeCycle::doCleanup(bool success) -{ - doTransition("cleanup", success, LifeCycleState::UNCONFIGURED, LifeCycleState::INACTIVE); - logState(); -} - - -void AMLifeCycle::onConfigure() -{ - ROS_INFO("onConfigure called [POMH]"); - if(stats_list_.hasStats()) - { - LifeCycleStatus status = stats_list_.process(throttle_info_.warn_throttle_s, throttle_info_.error_throttle_s); - if(status != LifeCycleStatus::ERROR) - { - doConfigure(true); - } - else if (!withinConfigureTolerance()) - { - ROS_WARN_STREAM_THROTTLE(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 - else - { - doConfigure(true); - } -} - -void AMLifeCycle::doConfigure(bool success) -{ - - doTransition("configuration", success, LifeCycleState::INACTIVE, LifeCycleState::UNCONFIGURED); -} - -void AMLifeCycle::onDeactivate() -{ - logState(); - doDeactivate(true); -} - -void AMLifeCycle::logState() -{ - ROS_INFO_STREAM("LifeCycle: " << life_cycle_mediator_.stateToString(life_cycle_info_.state)); -} - -void AMLifeCycle::doDeactivate(bool success) -{ - doTransition("deactivation", success, LifeCycleState::INACTIVE, LifeCycleState::ACTIVE); -} - -void AMLifeCycle::configure() -{ - //mark the configuration start time once - if(getState() != LifeCycleState::CONFIGURING) - { - configure_start_time_=ros::Time().now(); - } - transition("configure", LifeCycleState::UNCONFIGURED, LifeCycleState::CONFIGURING, LifeCycleState::INACTIVE, - std::bind(&AMLifeCycle::onConfigure, this)); -} - -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]"); - } - /* 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]"); - onDestroy(); - } -} - -void AMLifeCycle::onDestroy() -{ - logState(); - doDestroy(true); -} - -void AMLifeCycle::doDestroy(bool success) -{ - logState(); - // TODO: how do we call node destructor and exit main()? raise some type of ROS error? -} - -bool AMLifeCycle::withinConfigureTolerance() -{ - bool tolerated = false; - //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) ) - { - tolerated = true; - } - } - return tolerated; -} - -void AMLifeCycle::error(std::string error_code, bool forced) -{ - error("[GSHY]",error_code,forced); -} - -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]"); - } - else - { - std::string forced_prefix = forced?"Terminal ":""; - std::string repeat_prefix = ""; - //only change the state if - if(!life_cycle_mediator_.redundantError(life_cycle_info_)) - { - setState(LifeCycleState::ERROR_PROCESSING); - setStatus(LifeCycleStatus::ERROR); - onError(); - } - else - { - repeat_prefix = "Repeated "; - } - std::string error_explanation=forced_prefix + repeat_prefix + error_code_message; - ROS_ERROR_STREAM(message << " -> " << error_explanation << " [R45Y]" ); - } -} - - -void AMLifeCycle::errorTerminal(std::string message, std::string error_code) -{ - error(message,error_code,true); -} - -void AMLifeCycle::errorTolerant(std::string message, std::string error_code) -{ - error(message,error_code,false); -} - -void AMLifeCycle::onError() -{ - logState(); - doError(false); -} - -void AMLifeCycle::doError(bool success) -{ - logState(); - if (success) - { - life_cycle_info_.state = LifeCycleState::UNCONFIGURED; - } - else - { - life_cycle_info_.state = LifeCycleState::FINALIZED; - } - sendNodeUpdate(); -} - -void AMLifeCycle::shutdown() -{ - if (life_cycle_mediator_.shutdown(life_cycle_info_)) - { - ROS_INFO_STREAM("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"); - } - else - { - ROS_INFO_STREAM("received illegal activate in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state)); - } -} - -void AMLifeCycle::onShutdown() -{ - logState(); - doShutdown(true); -} - -void AMLifeCycle::doShutdown(bool success) -{ - logState(); - setState(LifeCycleState::FINALIZED); -} - -void AMLifeCycle::addStatistics(diagnostic_updater::DiagnosticStatusWrapper& dsw) -{ - stats_list_.addStatistics(dsw); - LifeCycleStatus status = stats_list_.process(throttle_info_.warn_throttle_s, throttle_info_.error_throttle_s); - if(life_cycle_mediator_.statusError(status)) - { - errorTolerant("Stats reporting error","PQAE"); - } - else - { - //only report status if not already errored - if(!life_cycle_mediator_.redundantError(life_cycle_info_)) - { - setStatus(status); - } - - //configuring is a special case where tolerance for errors is allowed - if(getState() == LifeCycleState::CONFIGURING) - { - onConfigure(); - } - } - dsw.summary((uint8_t)status, "update"); -} - -void AMLifeCycle::configureStat(AMStat& stat, std::string name, std::string category) -{ - int unassigned=UINT_MAX; - int target = unassigned; - int min_error =unassigned; - int min_warn=unassigned; - int max_warn=unassigned; - int max_error=unassigned; - std::string prefix = name + "/"; - - if(!category.empty()) - { - prefix = prefix + category + "/"; - } - - const std::string target_key =prefix + "target"; - const std::string error_min_key=prefix + "error/min"; - const std::string warn_min_key =prefix + "warn/min"; - const std::string warn_max_key =prefix + "warn/max"; - const std::string error_max_key=prefix + "error/max"; - - //set all if target is provided - if(param(target_key, target, 0)) - { - //give 5% tolerance in either direction for warning, 10% for error. Override default values as desired - const int warning_offset = std::ceil(target * 0.05); - const int error_offset = 2 * warning_offset; - //don't go below zero because that doesn't make any sense for hz. - min_error=std::max(0,target - error_offset); - min_warn=std::max(0,target - warning_offset); - max_warn=target + warning_offset; - max_error=target + error_offset; - stat.setWarnError(min_error, min_warn, max_warn, max_error); - } - //override individual boundary configs if provided - if(param(error_min_key, min_error, min_error)) - { - stat.setMinError(min_error); - } - if(param(warn_min_key, min_warn, min_warn)) - { - stat.setMinWarn(min_warn); - } - if(param(warn_max_key, max_warn, max_warn)) - { - stat.setMaxWarn(max_warn); - } - if(param(error_max_key, max_error, max_error)) - { - stat.setMaxError(max_error); - } -} - -void AMLifeCycle::configureStat(AMStat& stat) -{ - configureStat(stat,stat.getLongName()); -} - -AMStatReset& AMLifeCycle::configureHzStat(AMStatReset& stat) -{ - configureStat(stat, stat.getLongName(), "hz"); -} - -AMStatReset& AMLifeCycle::configureHzStats(AMStatReset& stats) -{ - configureStat(stats, stats.getShortName(), "hz"); -} - -void AMLifeCycle::sendNodeUpdate() -{ - brain_box_msgs::LifeCycleState msg; - msg.node_name = ros::this_node::getName(); - 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); -} - - -void AMLifeCycle::heartbeatCB(const ros::TimerEvent& event) -{ - updater_.force_update(); - - std::stringstream ss; - ss << life_cycle_mediator_.stateToString(life_cycle_info_.state) << "," << life_cycle_mediator_.statusToString(life_cycle_info_.status) << "," - << stats_list_.getStatsStrShort(); - - double throttle_s = getThrottle(); - ROS_INFO_STREAM_THROTTLE(throttle_s, "LifeCycle heartbeat: " << ss.str()); - - stats_list_.reset(); - - sendNodeUpdate(); -} - -LifeCycleState AMLifeCycle::getState() const -{ - return life_cycle_mediator_.getState(life_cycle_info_); -} - -const std::string_view& AMLifeCycle::getStateName() -{ - return life_cycle_mediator_.stateToString(getState()); -} - -void AMLifeCycle::setState(const LifeCycleState state) -{ - LifeCycleState initial_state = life_cycle_info_.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)); - sendNodeUpdate(); - } - else - { - ROS_ERROR_STREAM("illegal state: " << (int)state); - } -} - -LifeCycleStatus AMLifeCycle::getStatus() const -{ - return life_cycle_mediator_.getStatus(life_cycle_info_); -} - -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]"); - } - else if (life_cycle_mediator_.setStatus(status, life_cycle_info_)) - { - sendNodeUpdate(); - } - - else - { - ROS_ERROR_STREAM("illegal status: " << life_cycle_mediator_.statusToString(status)); - } -} - -const std::string_view& AMLifeCycle::getStatusName() -{ - return life_cycle_mediator_.statusToString(getStatus()); -} - - -//Is this being used? -void AMLifeCycle::setThrottleS(const double throttleS) -{ - return life_cycle_mediator_.setThrottleS(throttleS, throttle_info_); -} - -double AMLifeCycle::getThrottle() -{ - return life_cycle_mediator_.getThrottle(life_cycle_info_, throttle_info_); -} - - -}; - diff --git a/src/super_lib/am_life_cycle_mediator.cpp b/src/super_lib/am_life_cycle_mediator.cpp deleted file mode 100644 index 7b56ae80..00000000 --- a/src/super_lib/am_life_cycle_mediator.cpp +++ /dev/null @@ -1,237 +0,0 @@ -#include -#include - - - - -namespace am -{ - -AMLifeCycleMediator::AMLifeCycleMediator() -{ - str_command_bimap_ = boost::assign::list_of< str_command_bimap::relation > - (COMMAND_ACTIVATE_STRING, LifeCycleCommand::ACTIVATE) - (COMMAND_CLEANUP_STRING, LifeCycleCommand::CLEANUP) - (COMMAND_CONFIGURE_STRING, LifeCycleCommand::CONFIGURE) - (COMMAND_CREATE_STRING, LifeCycleCommand::CREATE) - (COMMAND_DEACTIVATE_STRING, LifeCycleCommand::DEACTIVATE) - (COMMAND_DESTROY_STRING, LifeCycleCommand::DESTROY) - (COMMAND_SHUTDOWN_STRING, LifeCycleCommand::SHUTDOWN); - - str_status_bimap_ = boost::assign::list_of< str_status_bimap::relation > - (STATUS_OK_STRING, LifeCycleStatus::OK) - (STATUS_WARN_STRING, LifeCycleStatus::WARN) - (STATUS_ERROR_STRING, LifeCycleStatus::ERROR); - - str_state_bimap_ = boost::assign::list_of< str_state_bimap::relation > - (STATE_INVALID_STRING, LifeCycleState::INVALID) - (STATE_UNCONFIGURED_STRING, LifeCycleState::UNCONFIGURED) - (STATE_INACTIVE_STRING, LifeCycleState::INACTIVE) - (STATE_ACTIVE_STRING, LifeCycleState::ACTIVE) - (STATE_FINALIZED_STRING, LifeCycleState::FINALIZED) - (STATE_CONFIGURING_STRING, LifeCycleState::CONFIGURING) - (STATE_CLEANING_UP_STRING, LifeCycleState::CLEANING_UP) - (STATE_ACTIVATING_STRING, LifeCycleState::ACTIVATING) - (STATE_DEACTIVATING_STRING, LifeCycleState::DEACTIVATING) - (STATE_ERROR_PROCESSING_STRING, LifeCycleState::ERROR_PROCESSING) - (STATE_SHUTTING_DOWN, LifeCycleState::SHUTTING_DOWN); -} - - -const std::string_view& AMLifeCycleMediator::commandToString(const LifeCycleCommand& command) -{ - if(str_command_bimap_.right.count(command)) - { - return str_command_bimap_.right.at(command); - } - return EMPTY_STRING; -} - -bool AMLifeCycleMediator::stringToCommand(const std::string& command_str, LifeCycleCommand& command) -{ - if(str_command_bimap_.left.count(command_str)) - { - command = str_command_bimap_.left.at(command_str); - return true; - } - return false; -} - -const std::string_view& AMLifeCycleMediator::statusToString(LifeCycleStatus status) -{ - if(str_status_bimap_.right.count(status)) - { - return str_status_bimap_.right.at(status); - } - return EMPTY_STRING; -} - -bool AMLifeCycleMediator::stringToStatus(std::string& status_str, LifeCycleStatus& status) -{ - if(str_status_bimap_.left.count(status_str)) - { - status = str_status_bimap_.left.at(status_str); - return true; - } - return false; -} - - -const std::string_view& AMLifeCycleMediator::stateToString(LifeCycleState state) -{ - if(str_state_bimap_.right.count(state)) - { - return str_state_bimap_.right.at(state); - } - return STATE_INVALID_STRING; -} - -bool AMLifeCycleMediator::stringToState(std::string& state_str, LifeCycleState& state) -{ - if(str_state_bimap_.left.count(state_str)) - { - state = str_state_bimap_.left.at(state_str); - return true; - } - return false; -} - -bool AMLifeCycleMediator::setStatus(const LifeCycleStatus& status, LifeCycleInfo& info) -{ - if (status < AMLifeCycleMediator::FIRST_STATUS || status > AMLifeCycleMediator::LAST_STATUS) - return false; - - if(info.status != LifeCycleStatus::ERROR) - { - info.status = status; - } - return true; -} -LifeCycleStatus AMLifeCycleMediator::getStatus(const LifeCycleInfo& info) const -{ - return info.status; -} - -bool AMLifeCycleMediator::setState(const LifeCycleState& state, LifeCycleInfo& info) -{ - if (state < AMLifeCycleMediator::FIRST_STATE || state > AMLifeCycleMediator::LAST_STATE) - return false; - - info.state = state; - return true; -} - -LifeCycleState AMLifeCycleMediator::getState(const LifeCycleInfo& info) const -{ - return info.state; -} - -bool AMLifeCycleMediator::statusError(const LifeCycleStatus& status) const -{ - return status == LifeCycleStatus::ERROR; -} - -const std::vector AMLifeCycleMediator::getLifeCycleCommands() -{ - std::vector all; - for (int enumIndex = (int)FIRST_COMMAND; enumIndex <= (int)LAST_COMMAND; enumIndex++) - { - LifeCycleCommand command = static_cast(enumIndex); - all.push_back(command); - } - return all; -} - -const std::vector AMLifeCycleMediator::getLifeCycleStates() -{ - std::vector all; - for (int enumIndex = (int)FIRST_STATE; enumIndex <= (int)LAST_STATE; enumIndex++) - { - LifeCycleState state = static_cast(enumIndex); - all.push_back(state); - } - return all; -} - -const std::vector AMLifeCycleMediator::getLifeCycleStatuses() -{ - std::vector all; - for (int enumIndex = (int)FIRST_STATUS; enumIndex <= (int)LAST_STATUS; enumIndex++) - { - LifeCycleStatus Status = static_cast(enumIndex); - all.push_back(Status); - } - return all; -} - -void AMLifeCycleMediator::setThrottleS(const double& throttleS, ThrottleInfo& throttle) -{ - if (throttleS == DEFAULT_THROTTLE_S) - { - throttle.ok_throttle_s = DEFAULT_OK_THROTTLE_S; - throttle.warn_throttle_s = DEFAULT_WARN_THROTTLE_S; - throttle.error_throttle_s = DEFAULT_ERROR_THROTTLE_S; - } - else - { - throttle.ok_throttle_s = throttleS; - throttle.warn_throttle_s = throttleS; - throttle.error_throttle_s = throttleS; - } -} - -double AMLifeCycleMediator::getThrottle(const AMLifeCycleMediator::LifeCycleInfo& info, const AMLifeCycleMediator::ThrottleInfo& t) -{ - double throttle; - switch (info.status) - { - case LifeCycleStatus::OK: - throttle = t.ok_throttle_s; - break; - case LifeCycleStatus::WARN: - throttle = t.warn_throttle_s; - break; - case LifeCycleStatus::ERROR: - throttle = t.error_throttle_s; - break; - } - return throttle; -} - -bool AMLifeCycleMediator::shutdown(const AMLifeCycleMediator::LifeCycleInfo& info) -{ - return info.state == LifeCycleState::UNCONFIGURED || - info.state == LifeCycleState::INACTIVE || - info.state == LifeCycleState::ACTIVE; -} - -bool AMLifeCycleMediator::unconfigured(const AMLifeCycleMediator::LifeCycleInfo& info) -{ - switch (info.state) - { - case LifeCycleState::UNCONFIGURED: - case LifeCycleState::CONFIGURING: - return true; - default: - return false; - } -} - - -bool AMLifeCycleMediator::redundantShutdown(const AMLifeCycleMediator::LifeCycleInfo& info) -{ - return info.state == LifeCycleState::SHUTTING_DOWN || - info.state == LifeCycleState::FINALIZED; -} - -bool AMLifeCycleMediator::redundantError(const AMLifeCycleMediator::LifeCycleInfo& info) -{ - return info.state == LifeCycleState::ERROR_PROCESSING || info.state == LifeCycleState::FINALIZED; -} - -bool AMLifeCycleMediator::illegalDestroy(const AMLifeCycleMediator::LifeCycleInfo& info) -{ - return info.state != LifeCycleState::FINALIZED; -} - -} //namespace am \ 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