diff --git a/CMakeLists.txt b/CMakeLists.txt index bd1a0d96..1d88066a 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,55 @@ 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}) +target_link_libraries(am_super) +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) +add_executable(resource_monitor src/resource_monitor/resource_status_class.cpp + src/resource_monitor/resource_monitor_main.cpp + src/resource_monitor/resource_monitor_node.cpp) +ament_target_dependencies(resource_monitor ${dependencies}) -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(DIRECTORY include/ + DESTINATION include/ +) + +install(TARGETS + am_super + resource_monitor + DESTINATION lib/${PROJECT_NAME} +) -file(GLOB TEST_FILES - test/*_tests.cpp +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ ) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + find_package(ros_testing REQUIRED) + + # the following lines skip linters + set(ament_cmake_cppcheck_FOUND TRUE) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + set(ament_cmake_flake8_FOUND TRUE) + set(ament_cmake_uncrustify_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + # Run all lint tests in package.xml except those listed above + ament_lint_auto_find_test_dependencies() -# code coverage setup: https://github.com/mikeferguson/code_coverage -if (CATKIN_ENABLE_TESTING) - - ## Add gtest based cpp test target and link libraries - - catkin_add_gtest(${PROJECT_NAME}_test ${TEST_FILES} ${SUPER_MEDIATOR_FILES}) - - if(TARGET ${PROJECT_NAME}_test) - target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES} super_lib) - endif() - - # For ros testing - find_package(rostest REQUIRED) - - #follow test naming convention of files in a folder and the cpp and launch have the test name with _rostest - set(TEST_NAMES abort_to_disarming - abort_to_manual - armed_to_ready - auto_to_abort - auto_to_manual - auto_to_semiauto - error_configure_tolerance - error_forced - error_status - error_status_without_stats - error_terminal_before_config - error_tolerant_before_config - hz_config - manual_to_disarming - param - platform_app_required_fail - platform_app_required_pass - platform_required_fail - platform_required_pass - primary - ready_to_shutdown - semi_auto_to_manual + ament_add_gtest_executable(abort_to_disarming + super_test/abort_to_disarming/abort_to_disarming_rostest.cpp ) - foreach(TEST_NAME ${TEST_NAMES}) - set(PROJECT_TEST_NAME ${TEST_NAME}_${PROJECT_NAME}_rostest) - add_rostest_gtest(${PROJECT_TEST_NAME} rostest/${TEST_NAME}/${TEST_NAME}_rostest.test rostest/${TEST_NAME}/${TEST_NAME}_rostest.cpp) - target_link_libraries(${PROJECT_TEST_NAME} ${catkin_LIBRARIES} super_lib) - endforeach() -endif() \ No newline at end of file + + target_link_libraries(abort_to_disarming) + 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/LICENSE b/LICENSE new file mode 100644 index 00000000..463d8a02 --- /dev/null +++ b/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2021 AutoModality, Inc. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the “Software”), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. 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..ad51abc4 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 }; /** @@ -72,6 +73,10 @@ class SuperNodeMediator /** manifest node (generated from manifest param) */ std::vector manifest; + /** Errored out nodes*/ + // The second part of the pair does not matter + std::unordered_map errored_nodes_; + /** * Overall state of the system, cumulative of the nodes * and super together. @@ -92,6 +97,10 @@ class SuperNodeMediator /** Signals if any of the manifested nodes status errored */ bool status_error = false; + + /** Allows supervisor to start the FP automatically when ready */ + bool start_fp_from_super_ = false; + }; /**Encapsulates properties and methods that relate to the transition of states @@ -100,6 +109,7 @@ class SuperNodeMediator */ struct StateTransition { + // TODO: casting a -1 as an enum of type uint8_t is problematic. would be better to just add the NO_* elements to the enums. static const SuperState NO_SUPER_STATE = (SuperState)-1; static const LifeCycleCommand NO_LIFECYCLE_COMMAND = (LifeCycleCommand)-1; static const OperatorCommand NO_OPERATOR_COMMAND = (OperatorCommand)-1; @@ -153,13 +163,16 @@ class SuperNodeMediator /** if True, then command should be sent. if ready_for_transition=true, then this is false*/ bool resend_life_cycle_command; + /** If this is an error transition, then we want to handle their lifecycles accordingly */ + bool error_transition; + /** The command that notifies nodes to continue processing so the state can transition*/ LifeCycleCommand life_cycle_command; /** List of node names that should receive the life_cycle_command */ std::vector failed_nodes; /** List of reasons nodes aren't transitioning */ - std::vector failed_nodes_reasons; + std::vector> failed_nodes_reasons; }; /** Returns the name of the node that is using the mediator */ @@ -197,7 +210,7 @@ class SuperNodeMediator * @param supervisor is in charge of knowing the state of the system * @return pair with the boolean indicating transition is ready and the optional state if ready. */ - SuperNodeMediator::TransitionInstructions transitionReady(Supervisor supervisor); + SuperNodeMediator::TransitionInstructions transitionReady(Supervisor& supervisor); /** * FIXME: currently public for unit testing @@ -241,6 +254,14 @@ class SuperNodeMediator * @return true if Lifecyle state equals ShuttingDown or Finalized */ static bool checkNodesShuttingDownOrFinalized(SuperNodeMediator::SuperNodeInfo& nr, SuperNodeMediator& node_mediator); + /** + * @return true if the LifeCycleState is out of ACTIVE (ideally in UNCONFIGURED or INACTIVE) */ + static bool checkErrorTransition(SuperNodeMediator::SuperNodeInfo& nr, SuperNodeMediator& node_mediator); + + /** + * @return true if the super node is in LifeCycleStatus::ERROR, and in either BOOTING or AUTO for LifeCycleState */ + static bool checkSuperError(SuperNodeMediator::SuperNodeInfo& nr, SuperNodeMediator& node_mediator); + /** Reads the given manifest string, typically provided by a ROS param, * converts it to a vector or node names which will be assigned to the given @@ -265,7 +286,7 @@ class SuperNodeMediator * @return a pair with overall success and a map containing any erroneous node names with message explaining why * */ - pair> allManifestedNodesCheck(Supervisor& supervisor, + pair>> allManifestedNodesCheck(Supervisor& supervisor, function check); /**@return the number of nodes where online=true*/ @@ -331,6 +352,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/resource_monitor/resource_monitor_node.h b/include/resource_monitor/resource_monitor_node.h new file mode 100644 index 00000000..7dcf914f --- /dev/null +++ b/include/resource_monitor/resource_monitor_node.h @@ -0,0 +1,31 @@ +#ifndef AM_SUPER_INCLUDE_RESOURCE_MONITOR_RESOURCE_MONITOR_NODE_H_ +#define AM_SUPER_INCLUDE_RESOURCE_MONITOR_RESOURCE_MONITOR_NODE_H_ + + +#include + +namespace am +{ +class ResourceMonitorNode : public AMLifeCycle +{ +public: + ResourceMonitorNode(const std::string &node_name); + + ~ResourceMonitorNode(); + + std::shared_ptr resource_status_ = nullptr; + + std::shared_ptr getAMClass(); + + void setAMClass(std::shared_ptr am_class); + + bool configured_ = false; + + // AMLifeCycle overrides + void heartbeatCB() override; + bool onCleanup() override; + bool onConfigure() override; +}; +} + +#endif /*AM_SUPER_INCLUDE_RESOURCE_MONITOR_RESOURCE_MONITOR_NODE_H_*/ \ No newline at end of file diff --git a/include/resource_monitor/resource_monitor_stats.h b/include/resource_monitor/resource_monitor_stats.h new file mode 100644 index 00000000..45885f6c --- /dev/null +++ b/include/resource_monitor/resource_monitor_stats.h @@ -0,0 +1,50 @@ +#ifndef AM_LIDAR_BS_AM_LIDAR_BS_STATS_H_ +#define AM_LIDAR_BS_AM_LIDAR_BS_STATS_H_ + +#include +#include +#include +#include +#include + +namespace am +{ + +class ResourceMonitorStats +{ +public: + AMStatStatus statStatus = AMStatStatus("ss", "AMStatStatus"); + + + AMStat tf_stats = AMStat("tf_s", "Transform Stats", 1, 2, 80, 99); + AMStat node_stats = AMStat("n_s", "Nodes Stats", 1, 2, 80, 99); + AMStat cpu_stats = AMStat("cpu_s", "CPU Stats", 1, 2, 80, 99); + AMStat gpu_stats = AMStat("gpu_s", "GPU Stats", 1, 2, 80, 99); + AMStat ram_stats = AMStat("ram_s", "RAM Stats", 1, 2, 80, 99); + AMStat drive_stats = AMStat("drive_s", "Drive Stats", 1, 2, 80, 99); + AMStat lidar_ip = AMStat("lidar_ip_s", "Lidar IP Stats", 1, 2, 80, 99); + AMStat fl_ip = AMStat("fl_s", "FL IP Stats", 1, 2, 80, 99); + AMStat fr_ip = AMStat("fr_s", "FR IP Stats", 1, 2, 80, 99); + AMStat rl_ip = AMStat("rl_s", "RL IP Stats", 1, 2, 80, 99); + AMStat rr_ip = AMStat("rr_s", "RR IP Stats", 1, 2, 80, 99); + + ResourceMonitorStats(AMStatList &stat_list) + { + stat_list.add(&statStatus); + stat_list.add(&tf_stats); + stat_list.add(&node_stats); + stat_list.add(&gpu_stats); + stat_list.add(&cpu_stats); + stat_list.add(&ram_stats); + stat_list.add(&drive_stats); + stat_list.add(&lidar_ip); + stat_list.add(&fl_ip); + stat_list.add(&fr_ip); + stat_list.add(&rl_ip); + stat_list.add(&rr_ip); + } +}; + +}; // namespace + +#endif /* AM_LIDAR_BS_AM_LIDAR_BS_STATS_H_ */ diff --git a/include/resource_monitor/resource_status_class.h b/include/resource_monitor/resource_status_class.h new file mode 100644 index 00000000..0f24eb66 --- /dev/null +++ b/include/resource_monitor/resource_status_class.h @@ -0,0 +1,159 @@ +#ifndef AM_SUPER_INCLUDE_RESOURCE_MONITOR_RESOURCE_STATUS_CLASS_H_ +#define AM_SUPER_INCLUDE_RESOURCE_MONITOR_RESOURCE_STATUS_CLASS_H_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include // For statvfs +#include // For std::setprecision + + +namespace am +{ + +struct MemoryInfo +{ + unsigned long total; + unsigned long free; + unsigned long used; + unsigned long available; + int used_percent; +}; + +struct GpuInfo +{ + std::string gpu_name; + int temp; + int load_percent; +}; + +struct CpuInfo +{ + unsigned long long user; + unsigned long long nice; + unsigned long long system; + unsigned long long idle; + unsigned long long iowait; + unsigned long long irq; + unsigned long long softirq; + unsigned long long steal; + unsigned long long total; +}; + +struct DiskInfo { + unsigned long long totalSpace; // Total space in bytes + unsigned long long availableSpace; // Available space in bytes (matches `df`) + unsigned long long usedSpace; // Used space in bytes + double percentUsed; // Percentage used +}; + +class ResourceStatus +{ +public: + ResourceStatus(std::shared_ptr stats); + + ~ResourceStatus(); + + am::MemoryInfo& getMemoryInfo(); + + am::CpuInfo getCPUInfo(); + + am::GpuInfo getGPUInfo(); + + void getCPUInfo(std::vector &infos); + + DiskInfo getDiskInfo(const std::string& path = "/"); + + double calculateCpuLoad(const am::CpuInfo &ci, const am::CpuInfo &ci_old); + + double getUpTime(); + + void updateInfos(); + + void print(); + + bool isReachable(const std::string &ipAddress); + + void getParams(); + + std::unordered_set getActiveIPs(const std::string& subnet = "192.168.1.0/24"); + + std::shared_ptr getStats(); + + std::vector getInetAddresses(); + + // AMLifeCycle passthrus + bool onConfigure(); + + bool onCleanup(); + + void heartbeatCB(); + +private: + + std::shared_ptr stats_; + + rclcpp::Subscription::SharedPtr status_sub_; + + rclcpp::Subscription::SharedPtr stat_sub_; + + void statusCB(const std_msgs::msg::Int32::SharedPtr msg); + + void statCB(const std_msgs::msg::Int32::SharedPtr msg); + + int getCPUCoresCount(); + + std::string readFile(const std::string& path); + + am::CpuInfo parseCpuLine(const std::string &line); + + int cpu_cnt_= -1; + + double cpu_usage_; + + double uptime_seconds_; + + bool ip_check_ {false}; + + bool is_first_time_ {true}; + + std::vector cpu_loads_; + + am::MemoryInfo mi; + + std::vector cpu_infos_; + + std::vector cpu_infos_old_; + + am::GpuInfo gpu_info_; + + std::vector sub_nets_add_; + + std::map ip_addresses_; //IPAddress, Name + + /*ROS Infrastructure Checking tools*/ + std::shared_ptr transformer_; + + std::vector> transform_list_; + + rclcpp::TimerBase::SharedPtr timer_; + + void timerCB(); + + void checkNodeNames(); + + void checkTransforms(); + + void checkSensorIPs(); +}; +} + +#endif /*AM_SUPER_INCLUDE_RESOURCE_MONITOR_RESOURCE_STATUS_CLASS_H_*/ \ No newline at end of file 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..b4984717 100644 --- a/launch/am_super.yaml +++ b/launch/am_super.yaml @@ -1,3 +1,5 @@ - -platform: - actual: $(arg platform) \ No newline at end of file +am_super: + ros__parameters: + platform: + actual: farm-ng_amiga_mockup + manifest: am_node_template 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..e63d0558 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -1,28 +1,31 @@ #include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include "std_msgs/msg/int32_multi_array.hpp" #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 @@ -30,55 +33,187 @@ #include #include #include +#include #include #include #include #include + +#include + #if CUDA_FLAG #include #endif +#ifdef WIN32 + #define COLOR_NORMAL "" + #define COLOR_RED "" + #define COLOR_GREEN "" + #define COLOR_YELLOW "" +#else + #define COLOR_NORMAL "\033[0m" + #define COLOR_RED "\033[31m" + #define COLOR_BLUE "\033[34m" + #define COLOR_GREEN "\033[32m" + #define COLOR_YELLOW "\033[33m" +#endif + +#undef ROS_INFO_STREAM +#undef ROS_INFO_STREAM_THROTTLE + +// Differentiate system state messages from others by blue console logs +#define ROS_INFO_STREAM(stream) RCLCPP_INFO_STREAM(am::Node::node->get_logger(), COLOR_BLUE << stream << COLOR_NORMAL) +#define ROS_INFO_STREAM_THROTTLE(duration, stream) RCLCPP_INFO_STREAM_THROTTLE(am::Node::node->get_logger(), *am::Node::node->get_clock(), (long)((duration) * 1000.0), COLOR_BLUE << stream << COLOR_NORMAL) + + +std::ostream &operator << ( std::ostream& strm, SuperState ss ) +{ + switch (ss) + { + case SuperState::OFF: + return strm << "OFF"; + case SuperState::BOOTING: + return strm << "BOOTING"; + case SuperState::READY: + return strm << "READY"; + case SuperState::ARMING: + return strm << "ARMING"; + case SuperState::ARMED: + return strm << "ARMED"; + case SuperState::AUTO: + return strm << "AUTO"; + case SuperState::DISARMING: + return strm << "DISARMING"; + case SuperState::SEMI_AUTO: + return strm << "SEMI_AUTO"; + case SuperState::HOLD: + return strm << "HOLD"; + case SuperState::ABORT: + return strm << "ABORT"; + case SuperState::MANUAL: + return strm << "MANUAL"; + case SuperState::SHUTDOWN: + return strm << "SHUTDOWN"; + } + return strm << "Unclear SuperState. Check ostream override."; +} + using namespace std; namespace am { + +std::ostream &operator << ( std::ostream& strm, LifeCycleState lcstate ) +{ + switch (lcstate) + { + case LifeCycleState::INVALID: + return strm << "INVALID"; + case LifeCycleState::UNCONFIGURED: + return strm << "UNCONFIGURED"; + case LifeCycleState::INACTIVE: + return strm << "INACTIVE"; + case LifeCycleState::ACTIVE: + return strm << "ACTIVE"; + case LifeCycleState::FINALIZED: + return strm << "FINALIZED"; + case LifeCycleState::CONFIGURING: + return strm << "CONFIGURING"; + case LifeCycleState::CLEANING_UP: + return strm << "CLEANING_UP"; + case LifeCycleState::SHUTTING_DOWN: + return strm << "SHUTTING_DOWN"; + case LifeCycleState::ACTIVATING: + return strm << "ACTIVATING"; + case LifeCycleState::DEACTIVATING: + return strm << "DEACTIVATING"; + case LifeCycleState::ERROR_PROCESSING: + return strm << "ERROR_PROCESSING"; + } + return strm << "Unclear LifeCycleState. Check ostream override."; +} + + +std::ostream &operator << ( std::ostream& strm, LifeCycleStatus lcstatus ) +{ + switch (lcstatus) + { + case LifeCycleStatus::OK: + return strm << "OK"; + case LifeCycleStatus::WARN: + return strm << "WARN"; + case LifeCycleStatus::ERROR: + return strm << "ERROR"; + } + return strm << "Unclear LifeCycleStatus. Check ostream override."; +} + + +std::ostream &operator << ( std::ostream& strm, LifeCycleCommand lccommand ) +{ + switch (lccommand) + { + case LifeCycleCommand::CREATE: + return strm << "CREATE"; + case LifeCycleCommand::CONFIGURE: + return strm << "CONFIGURE"; + case LifeCycleCommand::CLEANUP: + return strm << "CLEANUP"; + case LifeCycleCommand::ACTIVATE: + return strm << "ACTIVATE"; + case LifeCycleCommand::DEACTIVATE: + return strm << "DEACTIVATE"; + case LifeCycleCommand::SHUTDOWN: + return strm << "SHUTDOWN"; + case LifeCycleCommand::DESTROY: + return strm << "DESTROY"; + // case LifeCycleCommand::LAST_COMMAND: + // return strm << "LAST_COMMAND"; + } + return strm << "Unclear LifeCycleCommand. Check ostream override."; +} + + /** * AM supervisor class. aggregates system state and system health and manages node lifecycle. * * 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 super_errored_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::Publisher::SharedPtr set_gpio_pin_pub_; + rclcpp::Subscription::SharedPtr node_state_sub_; + rclcpp::Subscription::SharedPtr operator_command_sub_; + rclcpp::Subscription::SharedPtr fake_operator_command_sub_; + rclcpp::Subscription::SharedPtr ui_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 */ @@ -97,47 +232,49 @@ class AMSuper : AMLifeCycle * amount of time in seconds without hearing from a node that will cause it to go offline */ double node_timeout_s_; + bool offline_timer_running_ = false; + rclcpp::Time offline_start_time_; // start_timer from here + const rclcpp::Duration offline_allowance_ = am::toDuration(20.0); + bool reset_timer_running_ = false; + rclcpp::Time reset_start_time_; // start_timer from here + const rclcpp::Duration reset_allowance_ = am::toDuration(5.0); // How much time we give am_locator to lock in. + bool reset_required_ = false; + bool restart_required_ = false; + bool offline_restart_required_ = false; + bool flight_plan_running_ = false; + bool first_time_booted_ = false; + rclcpp::Time booting_start_time_ = am::Node::node->now(); + const rclcpp::Duration booting_allowance_ = am::toDuration(20.0); /** * baglogger level */ const int SU_LOG_LEVEL = 1; - // - // babysitters - // - const std::string NODE_BS_ALTIMETER = "can_node"; // TODO: replace with system global const - - typedef brain_box_msgs::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; - am::BabySitter* dji_bs_; - const std::string DJI_BS_TOPIC = "/dji_sdk/rc"; // TODO: replace with system global const - const int DJI_HZ = 50; - #if CUDA_FLAG std::shared_ptr gpu_info_; #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_, 3.1); + ROS_INFO_STREAM( "node_timeout_s = " << node_timeout_s_); + + am::getParam("start_fp_from_super", supervisor_.start_fp_from_super_, false); + ROS_INFO_STREAM("start_fp_from_super = " << supervisor_.start_fp_from_super_); /* - * create initial node list from manifest and create babysitters as needed + * create initial node list from manifest */ 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,34 +284,19 @@ 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"); 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)) - { - 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); - } - 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_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(); @@ -184,59 +306,78 @@ class AMSuper : AMLifeCycle gpu_info_ = std::make_shared(nh_); #endif - /** - * system status pub - */ - vstate_summary_pub_ = nh_.advertise(am_super_topics::SUPER_STATE, 1000); - system_state_pub_ = nh_.advertise(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); - /** - * led control pub - */ - led_pub_ = nh_.advertise(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); + // vehicle state: position, velocity, etc. + vstate_summary_pub_ = am::Node::node->create_publisher(am_super_topics::SUPER_STATE, am::getSensorQoS(10)); + + // system state: BOOTING, READY, AUTO, etc. ("/system/state") + system_state_pub_ = am::Node::node->create_publisher(am_topics::SYSTEM_STATE, am::getSensorQoS(10)); + + // lifecycle command: CONFIGURE, ACTIVATE, etc. - sent to all nodes to control lifecycle + lifecycle_pub_ = am::Node::node->create_publisher(am_super_topics::NODE_LIFECYCLE, am::getSensorQoS(1)); + + // led blink rate + // TODO: remove since no longer supported? + led_pub_ = am::Node::node->create_publisher(am::am_topics::LED_BLINK, am::getSensorQoS(1)); + + // super state: manifest info + super_status_pub_ = am::Node::node->create_publisher(am_super_topics::SUPER_STATUS, am::getSensorQoS(10)); + + // super state: errored_nodes info + super_errored_pub_ = am::Node::node->create_publisher("/super/errored_nodes", am::getSensorQoS(10)); + + + // ??? + rclcpp::QoS qos_profile(1); + qos_profile.reliable(); + flight_plan_deactivation_pub_ = am::Node::node->create_publisher(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, qos_profile); - flight_plan_deactivation_pub_ = nh_.advertise(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, 1000); + // Tractor Light + set_gpio_pin_pub_ = am::Node::node->create_publisher("/set_gpio_pin", 1); supervisor_.system_state = SuperState::BOOTING; supervisor_.flt_ctrl_state = SuperNodeMediator::SuperFltCtrlState::INIT; - /** - * amros log control - */ - log_control_sub_ = nh_.subscribe(am::am_topics::CTRL_LOG_CONTROL, 10, &AMSuper::logControlCB, this); - // 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); // subs should always come at the end - /** - * node status via LifeCycle - */ - node_state_sub_ = nh_.subscribe(am_super_topics::LIFECYCLE_STATE, 100, &AMSuper::nodeStateCB, this); + // amros log control + log_control_sub_ = am::Node::node->create_subscription(am::am_topics::CTRL_LOG_CONTROL, 10, + std::bind(&AMSuper::logControlCB, this, std::placeholders::_1)); - /** - * commands from operator - */ - operator_command_sub_ = nh_.subscribe(am_super_topics::OPERATOR_COMMAND, 100, &AMSuper::operatorCommandCB, this); + // lifecycle state: UNCONFIGURED, INACTIVE, etc. - sent by all lifecycle nodes periodically + node_state_sub_ = am::Node::node->create_subscription(am_super_topics::LIFECYCLE_STATE, am::getSensorQoS(1), + std::bind(&AMSuper::nodeStateCB, this, std::placeholders::_1)); - controller_state_sub = nh_.subscribe(am_super_topics::CONTROLLER_STATE, 100, &AMSuper::controllerStateCB, this); + // operator commands: ARM, DISARM, etc. - send by operator node + 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); + fake_operator_command_sub_ = am::Node::node->create_subscription(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, 100, + std::bind(&AMSuper::fakeOperatorCommandCB, this, std::placeholders::_1)); - current_enu_sub = nh_.subscribe(am_topics::CTRL_VX_VEHICLE_CURRENTENU, 100, &AMSuper::currentENUCB, this); + ui_operator_command_sub_ = am::Node::node->create_subscription("/ui_operator_cmd", 100, + std::bind(&AMSuper::uiOperatorCommandCB, this, std::placeholders::_1)); - heartbeat_timer_ = nh_.createTimer(ros::Duration(1.0), &AMSuper::heartbeatCB, this); - } + + // controller state: ??? + controller_state_sub = am::Node::node->create_subscription(am_super_topics::CONTROLLER_STATE, 100, + std::bind(&AMSuper::controllerStateCB, this, std::placeholders::_1)); + + // dignostics: these are logged by am_super + diagnostics_sub = am::Node::node->create_subscription("/diagnostics", 100, + std::bind(&AMSuper::diagnosticsCB, this, std::placeholders::_1)); + + // currentENU: vehicle location from am_locator + current_enu_sub = am::Node::node->create_subscription(am_topics::CTRL_VX_VEHICLE_CURRENTENU, am::getSensorQoS(1), + std::bind(&AMSuper::currentENUCB, this, std::placeholders::_1)); + + + + } ~AMSuper() { @@ -247,15 +388,18 @@ class AMSuper : AMLifeCycle } private: + + /** * process LifeCycleState messages from nodes * * 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,32 +409,78 @@ 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 ); + + // HARDIK:: Commenting this out as we are focused on the fake commands. + // 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); + } + + void fakeOperatorCommandCB(const std_msgs::msg::Bool::SharedPtr msg) + { + // This is listening to see if the flight plan is running or not + flight_plan_running_ = msg->data; + + if (supervisor_.start_fp_from_super_) + { + return; + } + + RCLCPP_INFO(am::Node::node->get_logger(), "Received FAKE Operator Command (actually flight_controller command): %i", msg->data); + if (msg->data == true && !supervisor_.start_fp_from_super_) // We don't want this to be receieved if we are starting the fp elsewhere + { + node_mediator_.setOperatorCommand(supervisor_, OperatorCommand::LAUNCH); + } + else + { + node_mediator_.setOperatorCommand(supervisor_, OperatorCommand::CANCEL); + } - 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(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, *msg, SU_LOG_LEVEL); } + + void uiOperatorCommandCB(const std_msgs::msg::Bool::SharedPtr msg) + { + if (!supervisor_.start_fp_from_super_) + { + return; + } + + RCLCPP_INFO(am::Node::node->get_logger(), "Received UI Operator Command: %i", msg->data); + if (msg->data == true) + { + node_mediator_.setOperatorCommand(supervisor_, OperatorCommand::LAUNCH); + } + else + { + node_mediator_.setOperatorCommand(supervisor_, OperatorCommand::CANCEL); + stopFlightPlan(); + } + } + /** - * process state + * process state from lifecycle nodes * @param node_name_in * @param state * @param status @@ -301,7 +491,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); @@ -309,55 +499,92 @@ class AMSuper : AMLifeCycle // search for the node in the list bool nodes_changed = false; map::iterator it; - it = supervisor_.nodes.find(node_name); + it = supervisor_.nodes.find(node_name); // This specifically only reacts to the specific node experiencing the callback if (it != supervisor_.nodes.end()) { // if we get here, the node is already in our list 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]"); + if (nr.manifested && nr.status == LifeCycleStatus::ERROR) + { + // This means we have now Come OUT of error + if (supervisor_.errored_nodes_.find(nr.name) == supervisor_.errored_nodes_.end()) + { + ROS_ERROR_STREAM("We should never be here. Node " << nr.name << " was in the errored_nodes_ list incorrectly"); + } + // Remove the node from the errored_nodes list + supervisor_.errored_nodes_.erase(nr.name); + if (nr.name == "am_locator") + { + reset_timer_running_ = false; + } + } + nr.status = status; nodes_changed = true; - if(nr.manifested && nr.status == LifeCycleStatus::ERROR) + // if(nr.manifested && nr.status == LifeCycleStatus::ERROR && supervisor_.system_state != SuperState::BOOTING) + if(nr.manifested && status == LifeCycleStatus::ERROR) { + if (supervisor_.errored_nodes_.find(nr.name) == supervisor_.errored_nodes_.end()) + { + // The node is not in the errored_nodes_ list, so it should be added + supervisor_.errored_nodes_[nr.name] = true; + } + if (nr.name == "am_locator") + { + if (!reset_timer_running_) + { + reset_start_time_ = am::Node::node->now(); + reset_timer_running_ = true; + } + } 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. Stopping Flight Plan... [JHRE]"); + // TODO: put this back in somehow - need to rethink how am_super influsenes control stopFlightPlan(); + node_mediator_.setOperatorCommand(supervisor_, OperatorCommand::CANCEL); } + + + // if (nr.manifested && nr.status == LifeCycleStatus::ACTIVE) } + // TODO: need to test the pid stuff - not sure if it is working if (nr.pid != pid) { //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; } + + nr.last_contact = last_contact; } 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; @@ -374,129 +601,34 @@ class AMSuper : AMLifeCycle if (nodes_changed) { reportSystemState(); + ROS_INFO("processSTate"); checkForSystemStateTransition(); } - // cache flight controller state and check for state transition - if (!node_name.compare("flight_controller") && !subsystem.compare("FLIGHT_CONTROL")) - { - bool flt_ctrl_state_changed = false; - if (!value.compare("AUTO") && supervisor_.flt_ctrl_state != SuperNodeMediator::SuperFltCtrlState::AUTO) - { - supervisor_.flt_ctrl_state = SuperNodeMediator::SuperFltCtrlState::AUTO; - flt_ctrl_state_changed = true; - } - else if (!value.compare("HOLD") && supervisor_.flt_ctrl_state != SuperNodeMediator::SuperFltCtrlState::HOLD) - { - supervisor_.flt_ctrl_state = SuperNodeMediator::SuperFltCtrlState::HOLD; - flt_ctrl_state_changed = true; - } - if (flt_ctrl_state_changed) - { - ROS_INFO_STREAM_THROTTLE(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); + // TODO: not sure if/how this should go back in + // // cache flight controller state and check for state transition + // if (!node_name.compare("flight_controller") && !subsystem.compare("FLIGHT_CONTROL")) + // { + // bool flt_ctrl_state_changed = false; + // if (!value.compare("AUTO") && supervisor_.flt_ctrl_state != SuperNodeMediator::SuperFltCtrlState::AUTO) + // { + // supervisor_.flt_ctrl_state = SuperNodeMediator::SuperFltCtrlState::AUTO; + // flt_ctrl_state_changed = true; + // } + // else if (!value.compare("HOLD") && supervisor_.flt_ctrl_state != SuperNodeMediator::SuperFltCtrlState::HOLD) + // { + // supervisor_.flt_ctrl_state = SuperNodeMediator::SuperFltCtrlState::HOLD; + // flt_ctrl_state_changed = true; + // } + // if (flt_ctrl_state_changed) + // { + // RCLCPP_INFO_STREAM_THROTTLE(am::Node::node->get_logger(), *am::Node::node->get_clock(), 1.0, "flight status: " << value); + // checkForSystemStateTransition(); + // } + // } } + /** * update stream with system state and status */ @@ -528,46 +660,57 @@ 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; + ROS_INFO_STREAM("sending command: " << life_cycle_mediator_.commandToString(command) << " to " << node_name << " lifecycle"); + brain_box_msgs::msg::LifeCycleCommand msg; + msg.header.stamp = am::ClockNow(); 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); } - /** - * check if all manifested nodes are ready for configuration - * @param state - * @param status - * @return true if all manifested nodes are ready to become active - * - * This means: - * - all are online - * - all states are UNCONFIGURED or INACTIVE or ACTIVE - * - all statuses are not error - */ - bool allManifestedNodesCheck(std::function check) - { - pair> result = node_mediator_.allManifestedNodesCheck(supervisor_, check); - bool success = result.first; - if (!success) - { - for (const auto & [ node_name, error_message ] : result.second) - { - ROS_WARN_STREAM(error_message); - } - } - return success; - } + // /** + // * check if all manifested nodes are ready for configuration + // * @param state + // * @param status + // * @return true if all manifested nodes are ready to become active + // * + // * This means: + // * - all are online + // * - all states are UNCONFIGURED or INACTIVE or ACTIVE + // * - all statuses are not error + // */ + // bool allManifestedNodesCheck(std::function check) + // { + // pair> result = node_mediator_.allManifestedNodesCheck(supervisor_, check); + // bool success = result.first; + // if (!success) + // { + // for (const auto & [ node_name, error_message ] : result.second) + // { + // ROS_WARN_STREAM(error_message); + // } + // } + // return success; + // } /** 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."); + } + + /** Send signal to flight controller that flight can be started. */ + void startFlightPlan() + { + std_msgs::msg::Bool msg; + msg.data = true; //false means deactivate + flight_plan_deactivation_pub_->publish(msg); + ROS_ERROR_STREAM( "Sending flight plan start command."); + } /** @@ -578,15 +721,22 @@ class AMSuper : AMLifeCycle */ void checkForSystemStateTransition() { - if(getState() == LifeCycleState::INACTIVE && supervisor_.system_state == SuperState::READY) //if super lifecycle is currently inactive - { - sendLifeCycleCommand(node_mediator_.getNodeName(), LifeCycleCommand::ACTIVATE); - } - else - { + // TODO: put this back in maybe when we enable ACTIVE + // if(life_cycle_node_->getState() == LifeCycleState::INACTIVE && supervisor_.system_state == SuperState::READY) //if super lifecycle is currently inactive + // { + // ROS_INFO_STREAM("Automatically activating am_super"); + // sendLifeCycleCommand(node_mediator_.getNodeName(), LifeCycleCommand::ACTIVATE); + // } + // else + // { SuperNodeMediator::TransitionInstructions transition_instructions = node_mediator_.transitionReady(supervisor_); + ROS_WARN_STREAM("Transition Instructions: ready_for_transition=" << transition_instructions.ready_for_transition << + " new_state=" << transition_instructions.new_state << + " resend_life_cycle_command=" << transition_instructions.resend_life_cycle_command << + " life_cycle_command=" << transition_instructions.life_cycle_command); + if (transition_instructions.ready_for_transition) { setSystemState(transition_instructions.new_state); @@ -595,17 +745,47 @@ class AMSuper : AMLifeCycle { LifeCycleCommand command = transition_instructions.life_cycle_command; std::string failed_nodes_string = boost::algorithm::join(transition_instructions.failed_nodes, ", "); - std::string failed_nodes_reasons_string = boost::algorithm::join(transition_instructions.failed_nodes_reasons, ", "); + auto printThePair = [](const auto& pair) { return pair.second + " (need_lifecycle_resend:" + std::to_string(pair.first) + ")"; }; + std::string failed_nodes_reasons_string = boost::algorithm::join(transition_instructions.failed_nodes_reasons | boost::adaptors::transformed(printThePair), ", "); ROS_INFO_STREAM_THROTTLE(5,state_mediator_.stateToString(supervisor_.system_state) << ": sending " << life_cycle_mediator_.commandToString(command) << " to " << failed_nodes_string << " because " << failed_nodes_reasons_string); + int i = 0; for(string failed_node_name : transition_instructions.failed_nodes) { - sendLifeCycleCommand(failed_node_name, command); + // Only resend the LifeCycleCommand if we need it to be resent + // This is done to allow am_super to only care about the manifested nodes + if (transition_instructions.failed_nodes_reasons[i].first) + { + sendLifeCycleCommand(failed_node_name, command); + } + i++; } - } - } + } + if (transition_instructions.error_transition) + { + ROS_WARN_STREAM("ERROR TRANSITION LIFECYCLE COMMANDS"); + // For each node in the manifest, we must bring it back down to INACTIVE + // If it was in error, we want to bring it down, clean it up, and then bring it back up to INACTIVE + map::iterator it; + for (it = supervisor_.nodes.begin(); it != supervisor_.nodes.end(); it++) + { + SuperNodeMediator::SuperNodeInfo& nr = (*it).second; + + if (nr.state == LifeCycleState::ACTIVE || nr.state == LifeCycleState::DEACTIVATING) + { + sendLifeCycleCommand(nr.name, LifeCycleCommand::DEACTIVATE); + } + else if (nr.state == LifeCycleState::UNCONFIGURED || nr.state == LifeCycleState::CONFIGURING) + { + sendLifeCycleCommand(nr.name, LifeCycleCommand::CONFIGURE); + } + + + } + + } } /** @@ -636,42 +816,25 @@ class AMSuper : AMLifeCycle reportSystemState(); - sendLEDMessage(); + // sendLEDMessage(); + sendTractorLightMessage(); - 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(); + if (state == SuperState::AUTO && supervisor_.start_fp_from_super_) + { + startFlightPlan(); + } + if (state == SuperState::READY && !first_time_booted_) + { + first_time_booted_ = true; + } } } + /** load the platform configurations from the launch file and populate the variants provided. */ void configurePlatformRequirements(SuperNodeMediator::PlatformVariant &required_platform, @@ -680,10 +843,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 +854,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 +875,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; @@ -806,6 +969,29 @@ class AMSuper : AMLifeCycle sendLEDMessage(r, g, b, rate); } + // Subject to State + void sendTractorLightMessage() + { + // TODO: This should only be if we are on tractor, where the light is connected to the orin + // Otherwise we risk changing values on the pin unecessarily. + std_msgs::msg::Int32MultiArray msg; + + int32_t gpio_pin_number = 13; + int32_t light_value = 0; // 0 is off, 1 is on + + if (supervisor_.system_state == SuperState::AUTO) + { + light_value = 1; + } + else + { + light_value = 0; + } + + msg.data = {gpio_pin_number, light_value}; + set_gpio_pin_pub_->publish(msg); + } + /** * calculate babysitter timing params * @param hz expected frequency in hz @@ -818,14 +1004,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 +1036,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 +1050,439 @@ class AMSuper : AMLifeCycle } }; + + + +class AMSuperNodeStats +{ + friend class AmSuperNode; + + +public: + AMStatStatus statStatus = AMStatStatus("ss", "AMStatStatus"); + AMSuperNodeStats(AMStatList &stat_list) + { + stat_list.add(&statStatus); + } }; -#ifdef TESTING -#else + +class AMSuperNode : public AMLifeCycle +{ +private: + shared_ptr am_super_; + std::shared_ptr stats_; + +public: + AMSuperNode(const std::string & node_name) : AMLifeCycle(node_name), stats_(std::make_shared(stats_list_)) + { + } + + ~AMSuperNode() + { + } + + void setAMSuper(shared_ptr am_super) + { + am_super_= am_super; + } + +/** + * Verify the basic requirements are being met: + * - platform required matches actual platform + */ + bool onConfigure() override + { + if(am_super_ == nullptr) + { + return AMLifeCycle::onConfigure(); + } + + 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 + return false; + + } + return 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); + bool stuck_in_booting_too_long = (((am::Node::node->now() - am_super_->booting_start_time_) > am_super_->booting_allowance_) && (am_super_->supervisor_.errored_nodes_.size() > 0)); + if (am_super_->supervisor_.system_state == SuperState::BOOTING) + { + if (!am_super_->first_time_booted_) + { + if (am_super_->offline_restart_required_) + { + system_state_msg.state_string = "RESTART_OFFLINE"; + // system_state_msg.state_string = "RESTART"; + } + else if (stuck_in_booting_too_long) + { + system_state_msg.state_string = "RESTART"; + } + else + { + if (am_super_->flight_plan_running_) + { + system_state_msg.state_string = "RESET"; + } + else // flight plan is NOT running + { + system_state_msg.state_string = "BOOTING"; + } + } + } + + // Check if we need a reset or a restart, and overwrite accordingly. + // if (am_super_->offline_restart_required_ || am_super_->restart_required_) + else if (am_super_->restart_required_) + { + system_state_msg.state_string = "RESTART"; + } + else if (am_super_->offline_restart_required_) + { + system_state_msg.state_string = "RESTART_OFFLINE"; + // system_state_msg.state_string = "RESTART"; + } + else if (am_super_->reset_required_) + { + system_state_msg.state_string = "RESET"; + } + } + 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); + if (nr.state == LifeCycleState::ACTIVE) + { + am_super_->sendLifeCycleCommand(nr.name, LifeCycleCommand::DEACTIVATE); + } + if (nr.state == LifeCycleState::INACTIVE) + { + am_super_->sendLifeCycleCommand(nr.name, LifeCycleCommand::CLEANUP); + } + 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.all.push_back(nr.name); + + if (nr.manifested) + { + if (nr.online){ + status_msg.man_onl.push_back(nr.name); + } + else{ + status_msg.man_offl.push_back(nr.name); + } + } + else{ + if (nr.online){ + status_msg.unman_onl.push_back(nr.name); + } + else{ + status_msg.unman_offl.push_back(nr.name); + } + } + } + LOG_MSG("/super/status", 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_)); + // if && supervisor_.system_state != SuperState::BOOTING + + // if (am_super_->supervisor_.system_state != SuperState::BOOTING) + if (true) // Seeing if we need this if statement at all. + { + if (!am_super_->offline_timer_running_) + { + am_super_->offline_start_time_ = am::Node::node->now(); + am_super_->offline_timer_running_ = true; + } + stats_->statStatus = 2; + am_super_->node_mediator_.setOperatorCommand(am_super_->supervisor_, OperatorCommand::CANCEL); + am_super_->stopFlightPlan(); // This will keep sending until the all nodes are back, and that is ok for now + } + + } + else + { + // if all manifested nodes are running, report as info + ROS_INFO_STREAM_THROTTLE(am_super_->LOG_THROTTLE_S, ss.str()); + + // This should be in any state, if all nodes come back online + if (am_super_->offline_timer_running_) + { + am_super_->offline_timer_running_ = false; + } + + if (am_super_->supervisor_.system_state == SuperState::BOOTING) + { + stats_->statStatus=0; + + } + } + + // Call for an restart if you can't see a node up at this time. + if (am_super_->offline_timer_running_) + { + // Check if we need to restart things + rclcpp::Time time_now = am::Node::node->now(); + if ((time_now - am_super_->offline_start_time_) > am_super_->offline_allowance_) + { + ROS_WARN_STREAM("Offline timer is: " << (time_now - am_super_->offline_start_time_).seconds()); + // ROS_WARN_STREAM((time_now - am_super_->offline_start_time_).seconds()); + am_super_->offline_restart_required_ = true; + // This should already have been done when the offline first happened (about 30 lines above) + // am_super_->node_mediator_.setOperatorCommand(am_super_->supervisor_, OperatorCommand::CANCEL); + // am_super_->stopFlightPlan(); + } + else + { + am_super_->offline_restart_required_ = false; + } + } + else + { + am_super_->offline_restart_required_ = false; + } + + // std_msgs/Header header + // uint8 num_manifested_nodes + // uint8 num_errored_nodes + // string[] reset_nodes + // string[] restart_nodes + brain_box_msgs::msg::Super2ErrorNodes super_errored_msg; + super_errored_msg.num_manifested_nodes = am_super_->supervisor_.manifest.size(); + super_errored_msg.num_errored_nodes = am_super_->supervisor_.errored_nodes_.size(); + if (am_super_->supervisor_.manifest.size() != am_super_->node_mediator_.manifestedNodesOnlineCount(am_super_->supervisor_)) + { + super_errored_msg.offline_nodes.push_back(am_super_->node_mediator_.manifestedNodesNotOnlineNamesList(am_super_->supervisor_)); + } + + // Let's look at the errored_nodes_ and see if we need to call for a reset or a restart from there. + // Reset nodes: am_locator, am_pilot + // Restart nodes: anything else + if (am_super_->supervisor_.errored_nodes_.size()>0) + { + // If either of reset nodes are in there, you have to ask for a reset + // if(am_super_->supervisor_.errored_nodes_.find("am_locator") != am_super_->supervisor_.errored_nodes_.end() || + // am_super_->supervisor_.errored_nodes_.find("am_pilot") != am_super_->supervisor_.errored_nodes_.end()) + // { + // am_super_->reset_required_ = true; + // } + // // Remove the reset request if they are BOTH out of error, which is the negation of the above statement + + + /* Approach 2*/ + // if(am_super_->supervisor_.errored_nodes_.find("am_locator") != am_super_->supervisor_.errored_nodes_.end()) + // { + // am_super_->reset_required_ = true; + // } + // else if (am_super_->supervisor_.errored_nodes_.find("am_pilot") != am_super_->supervisor_.errored_nodes_.end()) + // { + // am_super_->reset_required_ = true; + // } + // else + // { + // am_super_->reset_required_ = false; + + // // But if we are here then there is something ELSE in the error condition, which means we need a restart + // am_super_->restart_required_ = true; + // } + + + + /* Approach 3 */ + am_super_->reset_required_ = false; + am_super_->restart_required_ = false; + for (auto node : am_super_->supervisor_.errored_nodes_) + { + // if (node.first == "am_locator" || node.first == "am_pilot") + if (node.first == "am_locator") + { + // if (!am_super_->flight_plan_running_ && ((am::Node::node->now() - am_super_->reset_start_time_) < am_super_->reset_allowance_)) + // { + // am_super_->reset_required_ = false; + // // super_errored_msg.reset_nodes.push_back(node.first); + // } + // else + // { + + // If it has been more than 5 seconds of trying, then remove the command to automatically try and launch the flight plan. + if (!am_super_->flight_plan_running_) + { + if ((am::Node::node->now() - am_super_->reset_start_time_) > am_super_->reset_allowance_) // We have exceeded our allowance + { + // Turn off the activate command + am_super_->node_mediator_.setOperatorCommand(am_super_->supervisor_, OperatorCommand::CANCEL); + am_super_->reset_required_ = true; + } + // else: you have 5 seconds to get it running + } + else + { + // So the flight plan IS running + am_super_->reset_required_ = true; + super_errored_msg.reset_nodes.push_back(node.first); + am_super_->node_mediator_.setOperatorCommand(am_super_->supervisor_, OperatorCommand::CANCEL); + am_super_->stopFlightPlan(); + } + + + + + // } + } + + else + { + am_super_->restart_required_ = true; + super_errored_msg.restart_nodes.push_back(node.first); + am_super_->node_mediator_.setOperatorCommand(am_super_->supervisor_, OperatorCommand::CANCEL); + am_super_->stopFlightPlan(); + } + } + + } + else + { + // If there are no errored nodes you are good to go + am_super_->reset_required_ = false; + am_super_->restart_required_ = false; + } + + LOG_MSG("/super/errored_nodes", super_errored_msg, 1); + if (am_super_->super_errored_pub_->get_subscription_count() > 0) + { + am_super_->super_errored_pub_->publish(super_errored_msg); + } + // rclcpp::Publisher::SharedPtr super_errored_pub_; + + + + + + // 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(); + } + +}; + +}; // 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..57f56af5 100644 --- a/src/am_super/super_node_mediator.cpp +++ b/src/am_super/super_node_mediator.cpp @@ -9,42 +9,166 @@ namespace am /** * The state of the system as the supervisor sees it.*/ -SuperNodeMediator::SuperNodeMediator(const std::string& node_name): - SUPER_NODE_NAME(node_name), + +std::string printLifeCycleState(LifeCycleState lcstate ) +{ + switch (lcstate) + { + case LifeCycleState::INVALID: + return "INVALID"; + case LifeCycleState::UNCONFIGURED: + return "UNCONFIGURED"; + case LifeCycleState::INACTIVE: + return "INACTIVE"; + case LifeCycleState::ACTIVE: + return "ACTIVE"; + case LifeCycleState::FINALIZED: + return "FINALIZED"; + case LifeCycleState::CONFIGURING: + return "CONFIGURING"; + case LifeCycleState::CLEANING_UP: + return "CLEANING_UP"; + case LifeCycleState::SHUTTING_DOWN: + return "SHUTTING_DOWN"; + case LifeCycleState::ACTIVATING: + return "ACTIVATING"; + case LifeCycleState::DEACTIVATING: + return "DEACTIVATING"; + case LifeCycleState::ERROR_PROCESSING: + return "ERROR_PROCESSING"; + } + return "Unclear LifeCycleState."; +} + + +std::string printLifeCycleStatus(LifeCycleStatus lcstatus ) +{ + switch (lcstatus) + { + case LifeCycleStatus::OK: + return "OK"; + case LifeCycleStatus::WARN: + return "WARN"; + case LifeCycleStatus::ERROR: + return "ERROR"; + } + return "Unclear LifeCycleStatus."; +} + +std::string printOperatorCommand(OperatorCommand opcmd ) +{ + switch (opcmd) + { + case OperatorCommand::ARM: + return "ARM"; + case OperatorCommand::CANCEL: + return "CANCEL"; + case OperatorCommand::LAUNCH: + return "LAUNCH"; + case OperatorCommand::PAUSE: + return "PAUSE"; + case OperatorCommand::RESUME: + return "RESUME"; + case OperatorCommand::MANUAL: + return "MANUAL"; + case OperatorCommand::LANDED: + return "LANDED"; + case OperatorCommand::ABORT: + return "ABORT"; + case OperatorCommand::SHUTDOWN: + return "SHUTDOWN"; + } + return "Unclear OperatorCommand."; +} + + + + +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}}}}, - { SuperState::READY, { - {SuperState::ARMING, {SuperState::ARMING, SuperNodeMediator::checkReadyToArm, StateTransition::NO_LIFECYCLE_COMMAND, OperatorCommand::ARM}}, - {SuperState::SHUTDOWN, {SuperState::SHUTDOWN, SuperNodeMediator::checkNodesShuttingDownOrFinalized, LifeCycleCommand::SHUTDOWN, OperatorCommand::SHUTDOWN}} - }}, - {SuperState::ARMING, { - {SuperState::ARMED, {SuperState::ARMED, SuperNodeMediator::checkArmed, LifeCycleCommand::ACTIVATE}}}}, - {SuperState::ARMED, { - {SuperState::AUTO, {SuperState::AUTO, SuperNodeMediator::checkArmed, StateTransition::NO_LIFECYCLE_COMMAND, OperatorCommand::LAUNCH}}, - {SuperState::DISARMING, {SuperState::DISARMING, SuperNodeMediator::checkArmed, StateTransition::NO_LIFECYCLE_COMMAND, OperatorCommand::CANCEL}} - }}, - {SuperState::AUTO, { - {SuperState::DISARMING, {SuperState::DISARMING, SuperNodeMediator::checkArmed, StateTransition::NO_LIFECYCLE_COMMAND, StateTransition::NO_OPERATOR_COMMAND, ControllerState::COMPLETED}}, - {SuperState::MANUAL, {SuperState::MANUAL, SuperNodeMediator::checkReadyToArm, LifeCycleCommand::DEACTIVATE, OperatorCommand::MANUAL}}, - {SuperState::SEMI_AUTO, {SuperState::SEMI_AUTO, SuperNodeMediator::checkArmed, StateTransition::NO_LIFECYCLE_COMMAND, OperatorCommand::PAUSE}}, - {SuperState::ABORT, {SuperState::ABORT, SuperNodeMediator::checkArmed, StateTransition::NO_LIFECYCLE_COMMAND, OperatorCommand::ABORT}} - }}, - {SuperState::SEMI_AUTO, { - {SuperState::AUTO, {SuperState::AUTO, SuperNodeMediator::checkArmed, StateTransition::NO_LIFECYCLE_COMMAND, OperatorCommand::RESUME}}, - {SuperState::MANUAL, {SuperState::MANUAL, SuperNodeMediator::checkReadyToArm, LifeCycleCommand::DEACTIVATE, OperatorCommand::MANUAL}} - }}, - {SuperState::DISARMING, { - {SuperState::READY, {SuperState::READY, SuperNodeMediator::checkReadyToArm, LifeCycleCommand::DEACTIVATE}} - }}, - {SuperState::ABORT, { - {SuperState::MANUAL, {SuperState::MANUAL, SuperNodeMediator::checkReadyToArm, LifeCycleCommand::DEACTIVATE, OperatorCommand::MANUAL}}, - {SuperState::DISARMING, {SuperState::DISARMING, SuperNodeMediator::checkArmed, StateTransition::NO_LIFECYCLE_COMMAND, StateTransition::NO_OPERATOR_COMMAND, ControllerState::COMPLETED}}, - }}, - {SuperState::MANUAL, { - {SuperState::DISARMING, {SuperState::DISARMING, SuperNodeMediator::checkNodesActiveOrInactive, StateTransition::NO_LIFECYCLE_COMMAND, StateTransition::NO_OPERATOR_COMMAND, ControllerState::COMPLETED}}, - }} -}) + // const std::map> state_transitions_ ; + // StateTransition(SuperState _to_state = NO_SUPER_STATE, std::function _check = NULL, + // LifeCycleCommand _life_cycle_command = NO_LIFECYCLE_COMMAND, OperatorCommand _operator_command = NO_OPERATOR_COMMAND, + // ControllerState _controller_state = NO_CONTROLLER_STATE) + + { SuperState::BOOTING, { // from state + {SuperState::READY, {SuperState::READY, SuperNodeMediator::checkReadyToArm, LifeCycleCommand::CONFIGURE}} // to state + }}, + + + // FROM HARDIK SIDE: + + { SuperState::READY, { // from state + {SuperState::AUTO, {SuperState::AUTO, SuperNodeMediator::checkArmed, LifeCycleCommand::ACTIVATE, OperatorCommand::LAUNCH}} + // {SuperState::BOOTING, {SuperState::BOOTING, SuperNodeMediator::checkSuperError}}, + }}, + + { SuperState::AUTO, { + // {SuperState::DISARMING, {SuperState::DISARMING, SuperNodeMediator::checkArmed, StateTransition::NO_LIFECYCLE_COMMAND, StateTransition::NO_OPERATOR_COMMAND, ControllerState::COMPLETED}}, + // {SuperState::READY, {SuperState::READY, SuperNodeMediator::checkArmed, LifeCycleCommand::DEACTIVATE, OperatorCommand::CANCEL}} + // There is also always the error transition of going back to BOOTING + // {SuperState::DISARMING, {SuperState::DISARMING, SuperNodeMediator::checkArmed, StateTransition::NO_LIFECYCLE_COMMAND, StateTransition::NO_OPERATOR_COMMAND, ControllerState::COMPLETED}}, + + {SuperState::READY, {SuperState::READY, SuperNodeMediator::checkReadyToArm, LifeCycleCommand::DEACTIVATE, OperatorCommand::CANCEL}} + }}, + + // { SuperState::DISARMING, { + // {SuperState::READY, {SuperState::READY, SuperNodeMediator::checkReadyToArm, LifeCycleCommand::DEACTIVATE}} + // }}, + + + + + + + + + // { SuperState:} + + + // FROM BEFORE SIDE: + + // { SuperState::READY, { + // {SuperState::ARMING, {SuperState::ARMING, SuperNodeMediator::checkReadyToArm, StateTransition:: + // , OperatorCommand::ARM}}, + // {SuperState::SHUTDOWN, {SuperState::SHUTDOWN, SuperNodeMediator::checkNodesShuttingDownOrFinalized, LifeCycleCommand::SHUTDOWN, OperatorCommand::SHUTDOWN}} + + // }}, + // { SuperState::ARMING, { + // {SuperState::ARMED, {SuperState::ARMED, SuperNodeMediator::checkArmed, LifeCycleCommand::ACTIVATE}} + // }}, + // + // { SuperState::ARMED, { + // {SuperState::AUTO, {SuperState::AUTO, SuperNodeMediator::checkArmed, StateTransition::NO_LIFECYCLE_COMMAND, OperatorCommand::LAUNCH}}, + // {SuperState::DISARMING, {SuperState::DISARMING, SuperNodeMediator::checkArmed, StateTransition::NO_LIFECYCLE_COMMAND, OperatorCommand::CANCEL}} + // }}, + // + // { SuperState::AUTO, { + // {SuperState::DISARMING, {SuperState::DISARMING, SuperNodeMediator::checkArmed, StateTransition::NO_LIFECYCLE_COMMAND, StateTransition::NO_OPERATOR_COMMAND, ControllerState::COMPLETED}}, + // {SuperState::MANUAL, {SuperState::MANUAL, SuperNodeMediator::checkReadyToArm, LifeCycleCommand::DEACTIVATE, OperatorCommand::MANUAL}}, + // {SuperState::SEMI_AUTO, {SuperState::SEMI_AUTO, SuperNodeMediator::checkArmed, StateTransition::NO_LIFECYCLE_COMMAND, OperatorCommand::PAUSE}}, + // {SuperState::ABORT, {SuperState::ABORT, SuperNodeMediator::checkArmed, StateTransition::NO_LIFECYCLE_COMMAND, OperatorCommand::ABORT}} + // }}, + // + // { SuperState::SEMI_AUTO, { + // {SuperState::AUTO, {SuperState::AUTO, SuperNodeMediator::checkArmed, StateTransition::NO_LIFECYCLE_COMMAND, OperatorCommand::RESUME}}, + // {SuperState::MANUAL, {SuperState::MANUAL, SuperNodeMediator::checkReadyToArm, LifeCycleCommand::DEACTIVATE, OperatorCommand::MANUAL}} + // }}, + // + // { SuperState::DISARMING, { + // {SuperState::READY, {SuperState::READY, SuperNodeMediator::checkReadyToArm, LifeCycleCommand::DEACTIVATE}} + // }}, + // + // { SuperState::ABORT, { + // {SuperState::MANUAL, {SuperState::MANUAL, SuperNodeMediator::checkReadyToArm, LifeCycleCommand::DEACTIVATE, OperatorCommand::MANUAL}}, + // {SuperState::DISARMING, {SuperState::DISARMING, SuperNodeMediator::checkArmed, StateTransition::NO_LIFECYCLE_COMMAND, StateTransition::NO_OPERATOR_COMMAND, ControllerState::COMPLETED}}, + // }}, + // + // {SuperState::MANUAL, { + // {SuperState::DISARMING, {SuperState::DISARMING, SuperNodeMediator::checkNodesActiveOrInactive, StateTransition::NO_LIFECYCLE_COMMAND, StateTransition::NO_OPERATOR_COMMAND, ControllerState::COMPLETED}}, + // }} + }) { } @@ -73,6 +197,7 @@ std::string_view SuperNodeMediator::getNodeName() void SuperNodeMediator::addSuperToManifest(SuperNodeMediator::Supervisor& supervisor) { supervisor.manifest.push_back(SUPER_NODE_NAME); + // supervisor.manifest.insert(supervisor.manifest.begin(), SUPER_NODE_NAME); } SuperNodeMediator::SuperNodeInfo SuperNodeMediator::initializeManifestedNode(std::string node_name) @@ -81,7 +206,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; @@ -98,7 +223,7 @@ SuperNodeMediator::StateTransition SuperNodeMediator::getStateTransition(const S std::map transitions(state_transitions_.at(supervisor.system_state)); StateTransition attempt_transition; - + ROS_ERROR_STREAM("GetStateTransition...." << printOperatorCommand(supervisor.last_op_command_received)); for (auto const& [state, transition] : transitions) { //if this transition has an operator command associated with it and super received it @@ -106,27 +231,44 @@ SuperNodeMediator::StateTransition SuperNodeMediator::getStateTransition(const S { if(supervisor.last_op_command_received == transition.operator_command) { + ROS_ERROR_STREAM("GetStateTransition: Processing an operator command | " << printOperatorCommand(supervisor.last_op_command_received) << " vs " << printOperatorCommand(transition.operator_command)); return transition; } - } + } + // else if (transition.operator_command == OperatorCommand::LAUNCH && supervisor.start_fp_from_super_) + // { + // // Allows for the mission to be started automatically if the anove parameter is set to true. + // ROS_ERROR_STREAM("GetStateTransition: Allowing automatic launch command without operator when blue light is achieved."); + // return transition; + // } else if(transitionHasControllerState(transition)) { if(supervisor.last_controller_state_received == transition.controller_state) { + ROS_ERROR_STREAM("GetStateTransition: Processing a controller state transition"); return transition; } } else { + ROS_ERROR_STREAM("GetStateTransition: Return blank transition"); return transition; } } + ROS_ERROR_STREAM("GetStateTransition: INVALID transition"); + return invalidTransition(); } SuperNodeMediator::StateTransition SuperNodeMediator::getErrorTransition() { - return {SuperState::SHUTDOWN, SuperNodeMediator::checkNodesShuttingDownOrFinalized, LifeCycleCommand::SHUTDOWN}; + // OLD + // return {SuperState::SHUTDOWN, SuperNodeMediator::checkNodesShuttingDownOrFinalized, LifeCycleCommand::SHUTDOWN}; + + // NEW + return {SuperState::BOOTING, SuperNodeMediator::checkErrorTransition, LifeCycleCommand::DEACTIVATE}; + // It's possible deactivate could be unsuitable if this is an error when you are in READY...? + // If a node times out, then you need to error out, but the node that comes back online is ACTIVE.... then what. } SuperNodeMediator::StateTransition SuperNodeMediator::invalidTransition() @@ -139,26 +281,135 @@ bool SuperNodeMediator::transitionIsValid(const StateTransition& transition) return transition.to_state != StateTransition::NO_SUPER_STATE; } -SuperNodeMediator::TransitionInstructions SuperNodeMediator::transitionReady(Supervisor supervisor) +SuperNodeMediator::TransitionInstructions SuperNodeMediator::transitionReady(Supervisor& supervisor) { // required default state is junk and should not be consulted since not ready TransitionInstructions transition_instructions; transition_instructions.ready_for_transition = false; transition_instructions.resend_life_cycle_command = false; + transition_instructions.error_transition = false; + + // Hardik: shortcircuit if we have am_super in error + // if (supervisor.nodes.at(SUPER_NODE_NAME).status == LifeCycleStatus::ERROR) + // { + // transition_instructions.ready_for_transition = true; + // transition_instructions.new_state = getErrorTransition().to_state; + // return transition_instructions; + // } + + // // Come out of error when looking for the next transition if nothing is in error + // if (supervisor.status_error && supervisor.errored_nodes_.size() == 0) + // { + // ROS_WARN_STREAM("Removing the status_error_ !!"); + // supervisor.status_error = false; + // } + + + // pair>> check_results = allManifestedNodesCheck(supervisor, transition.check); + // Simplified logic that shortcuts getting any other transition. + ROS_WARN_STREAM("For some reason status_error is true here? : " << supervisor.status_error); + if (supervisor.status_error) + { + + if (supervisor.errored_nodes_.size()==0 && allManifestedNodesCheck(supervisor, getErrorTransition().check).first) + { + // TransitionInstructions pseudo_transition; + // pseudo_transition.ready_for_transition = false; + // pesudo_transition.resend_life_cycle_command = false; + // pseudo_transition.error_transition = false; + // pseudo_transition.new_state = getErrorTransition().to_state; + // if () + ROS_WARN_STREAM("Removing the status_error_ !!"); + supervisor.status_error = false; + } + else + { + // assert(supervisor.errored_nodes_.size() > 0); + transition_instructions.ready_for_transition = true; + transition_instructions.new_state = getErrorTransition().to_state; + transition_instructions.error_transition = true; + return transition_instructions; + } + } + + + // // bool are_in_error = false; + // for (pair nodePair : supervisor.nodes) + // { + // SuperNodeInfo node = nodePair.second; + // if (node.status == LifeCycleStatus::ERROR) + // { + // transition_instructions.ready_for_transition = true; + // // are_in_error = true; + // supervisor.status_error = true; + // transition_instructions.new_state = getErrorTransition().to_state; + // transition_instructions.resend_life_cycle_command = true; + // // if (supervisor.system_state == SuperState::READY) + // // { + // // transition_instructions.life_cycle_command = LifeCycleCommand::CLEANUP; + // // } + // // else if (supervisor.system_state == SuperState::AUTO) + // // { + // // transition_instructions.life_cycle_command = LifeCycleCommand::DEACTIVATE; + // // } + // // return transition_instructions; + // break; + // } + // } + + // if (are_in_error) + // { + // bool active_true = false; + // bool inactive_true = false; + + // for (pair nodePair : supervisor.nodes) + // { + // SuperNodeInfo node = nodePair.second; + // // If things are active, make sure they come down inactive + // // If things are inactive, then make sure they come down to Unconfingured + + // if (node.state == LifeCycleState::ACTIVE) + // { + // active_true = true; + // } + // if (node.state == LifeCycleState::INACTIVE) + // { + // inactive_true = true; + // } + + // } + + // if (active_true) + // { + // transition_instructions.life_cycle_command = LifeCycleCommand::DEACTIVATE; + // } + // else if (inactive_true) + // { + // transition_instructions.life_cycle_command = LifeCycleCommand::CLEANUP; + // } + // else + // { + // // This technically should not happen....? + // transition_instructions.life_cycle_command = LifeCycleCommand::CONFIGURE; + // } + // return transition_instructions; + + // } + + // only check those states registered with state_transitions if (state_transitions_.count(supervisor.system_state)) { StateTransition transition; - if(supervisor.status_error) - { - transition = getErrorTransition(); - } - else + // TODO: put htis back in when we figure out how errors are going to work + // if(supervisor.status_error) + // else { transition = getStateTransition(supervisor); } + // each state has a check method providing the logic that should cause transition (based on manifest nodes // lifecycle) // some transitions happen only when check fails (mostly to abort) @@ -166,7 +417,7 @@ SuperNodeMediator::TransitionInstructions SuperNodeMediator::transitionReady(Sup //if there was no statetransition as indicated by the to_state equalling the current state, then don't transition if(transitionIsValid(transition)) { - pair> check_results = allManifestedNodesCheck(supervisor, transition.check); + pair>> check_results = allManifestedNodesCheck(supervisor, transition.check); //transition to new state if checks passed or forced bool checks_passed = check_results.first; @@ -180,7 +431,8 @@ SuperNodeMediator::TransitionInstructions SuperNodeMediator::transitionReady(Sup if (!checks_passed) { vector failed_nodes; - vector failed_nodes_reasons; + // vector failed_nodes_reasons; + vector> failed_nodes_reasons; boost::copy(check_results.second | boost::adaptors::map_keys, std::back_inserter(failed_nodes)); boost::copy(check_results.second | boost::adaptors::map_values, std::back_inserter(failed_nodes_reasons)); transition_instructions.failed_nodes = failed_nodes; @@ -211,12 +463,15 @@ bool SuperNodeMediator::lifeCycleNotYetImplemented(string node_name) bool SuperNodeMediator::checkReadyToArm(SuperNodeMediator::SuperNodeInfo& nr, SuperNodeMediator& node_mediator) { - return nr.state == LifeCycleState::INACTIVE || (nr.state == LifeCycleState::ACTIVE && node_mediator.nodeNameIsSuper(nr.name)); + ROS_INFO_STREAM("checkReadyToArm: " << nr.name << " is " << printLifeCycleState(nr.state)); + // return nr.state == LifeCycleState::INACTIVE || (nr.state == LifeCycleState::ACTIVE && node_mediator.nodeNameIsSuper(nr.name)); + return nr.state == LifeCycleState::INACTIVE && nr.status != LifeCycleStatus::ERROR; } bool SuperNodeMediator::checkArmed(SuperNodeMediator::SuperNodeInfo& nr, SuperNodeMediator& node_mediator) { - return nr.state == LifeCycleState::ACTIVE; + ROS_INFO_STREAM("checkArmed: " << nr.name << " is " << printLifeCycleState(nr.state)); + return nr.state == LifeCycleState::ACTIVE && nr.status != LifeCycleStatus::ERROR; } bool SuperNodeMediator::checkNodesActiveOrInactive(SuperNodeMediator::SuperNodeInfo& nr, SuperNodeMediator& node_mediator) @@ -229,54 +484,198 @@ bool SuperNodeMediator::checkNodesShuttingDownOrFinalized(SuperNodeMediator::Sup return nr.state == LifeCycleState::SHUTTING_DOWN || nr.state == LifeCycleState::FINALIZED; } +bool SuperNodeMediator::checkErrorTransition(SuperNodeMediator::SuperNodeInfo& nr, SuperNodeMediator& node_mediator) +{ + return nr.state == LifeCycleState::INACTIVE; + // return nr.state != LifeCycleState::ACTIVE; + // return nr.state == LifeCycleState::UNCONFIGURED | nr.state == LifeCycleState::INACTIVE; +} + +// If we use Solution 1, this function should probably be renamed. +bool SuperNodeMediator::checkSuperError(SuperNodeMediator::SuperNodeInfo& nr, SuperNodeMediator& node_mediator) +{ + + // Solution 1: + // If any node goes into an error or out of inactive/active, then bring the system back to booting + // return !(nr.state == LifeCycleState::INACTIVE || nr.state == LifeCycleState::ACTIVE) || nr.status == LifeCycleStatus::ERROR; + + + // Solution 2: + // Ignore all nodes except am_super, and let am_super go into error if it encounters an issue in the manifest. + // This requires the AMSuperNodeStats to update. + + // return (node_mediator.nodeNameIsSuper(nr.name) && (nr.state == LifeCycleState::INACTIVE || nr.state == LifeCycleState::ACTIVE) && nr.status == LifeCycleStatus::ERROR ); + // bool normal_node = (!node_mediator.nodeNameIsSuper(nr.name) && (nr.state == LifeCycleState::INACTIVE || nr.state == LifeCycleState::ACTIVE)); + bool normal_node = (!node_mediator.nodeNameIsSuper(nr.name)); + // bool super_node = (node_mediator.nodeNameIsSuper(nr.name) && (nr.state == LifeCycleState::INACTIVE || nr.state == LifeCycleState::ACTIVE) && nr.status == LifeCycleStatus::ERROR ); + bool super_node = (node_mediator.nodeNameIsSuper(nr.name) && (nr.state == LifeCycleState::INACTIVE || nr.state == LifeCycleState::ACTIVE) && nr.status == LifeCycleStatus::ERROR ); + + ROS_WARN_STREAM("HARDIK: CHECKSUPERERROR retval:" << (normal_node || super_node) + << " - name:(" << nr.name << "," << node_mediator.nodeNameIsSuper(nr.name) << ") - state:" << am::printLifeCycleState(nr.state) << " - status:" << printLifeCycleStatus(nr.status)); + // return (normal_node || super_node); + + + if (node_mediator.nodeNameIsSuper(nr.name)) + { + // The node is am_super + if ((nr.state == LifeCycleState::INACTIVE || nr.state == LifeCycleState::ACTIVE) && nr.status == LifeCycleStatus::ERROR) + { + return true; // super is in error + } + else + { + return false; // super is not in error + } + } + + // If the node is not am_super, we can ignore it. + return true; + + +} + + +// bool SuperNodeMediator::allManifestedBackToInactiveCheck(Supervisor& supervisor) +// { +// for (auto nodePair : supervisor.nodes) +// { +// SuperNodeInfo node = nodePair.second; + +// if (node.manifested) +// { -pair> SuperNodeMediator::allManifestedNodesCheck( +// } +// } +// } + +// This does the check function on all manifested nodes. +pair>> SuperNodeMediator::allManifestedNodesCheck( Supervisor& supervisor, std::function check) { - map failed_nodes; + // The format of this map is: > + map> failed_nodes; bool success = true; for (pair nodePair : supervisor.nodes) { SuperNodeInfo node = nodePair.second; std::string error_message; + bool need_lifecycle_resend; // only check manifested nodes, ignore others if (node.manifested) { + ROS_WARN_STREAM("hardik-allManifestedNodesCheck: 1"); if (!node.online) { + ROS_WARN_STREAM("hardik-allManifestedNodesCheck: 1.1"); error_message = "[U5JB] check failed: node not online: " + node.name; success = false; + need_lifecycle_resend = true; } else if (lifeCycleNotYetImplemented(node.name)) { + ROS_WARN_STREAM("hardik-allManifestedNodesCheck: 1.2"); error_message = "[WCK2] check skipped: node LifeCycle not yet implemented: " + node.name; //not a failure to allow temporary transition until implemented + need_lifecycle_resend = false; } else if (!check(node, *this)) { + ROS_WARN_STREAM("hardik-allManifestedNodesCheck: 1.3"); string_view node_state = life_cycle_mediator.stateToString(node.state); error_message = "[2OQ0] check failed: node in wrong state " + node.name + ": " + string(node_state); + ROS_WARN_STREAM(error_message); success = false; + need_lifecycle_resend = true; } else if (node.status == LifeCycleStatus::ERROR) { + ROS_WARN_STREAM("hardik-allManifestedNodesCheck: 1.4"); error_message = "[AA0A] check failed: node status is ERROR: " + node.name; //if check method passes and we are in error, we want to pass + // success = false; + need_lifecycle_resend = false; // TODO from Hardik: Should this be flipped? } + // else if (node.status == LifeCycleState::ACTIVE) } else { error_message = "[BJIL] check skipped: not manifested: " + node.name; + need_lifecycle_resend = false; } if (!error_message.empty()) { - failed_nodes.insert(pair(node.name, error_message)); + failed_nodes.insert(pair>(node.name, pair(need_lifecycle_resend, error_message))); } + ROS_WARN_STREAM("hardik-allManifestedNodesCheck: success:" << success << " latest_node:" << node.name); } // for each node - return pair(success, failed_nodes); + return std::pair(success, failed_nodes); } +// // This does the check function on all manifested nodes. +// pair>> SuperNodeMediator::allManifestedNodesCheck( +// Supervisor& supervisor, std::function check) +// { +// // The format of this map is: > +// map> failed_nodes; +// bool success = true; + +// for (pair nodePair : supervisor.nodes) +// { +// SuperNodeInfo node = nodePair.second; +// std::string error_message; +// bool need_lifecycle_resend; +// // only check manifested nodes, ignore others +// if (node.manifested) +// { +// ROS_WARN_STREAM("hardik-allManifestedNodesCheck: 1"); +// if (!node.online) +// { +// ROS_WARN_STREAM("hardik-allManifestedNodesCheck: 1.1"); +// error_message = "[U5JB] check failed: node not online: " + node.name; +// // success = false; +// need_lifecycle_resend = true; +// } +// if (lifeCycleNotYetImplemented(node.name)) +// { +// ROS_WARN_STREAM("hardik-allManifestedNodesCheck: 1.2"); +// error_message = "[WCK2] check skipped: node LifeCycle not yet implemented: " + node.name; +// //not a failure to allow temporary transition until implemented +// need_lifecycle_resend = false; +// } +// else if (!check(node, *this)) +// { +// ROS_WARN_STREAM("hardik-allManifestedNodesCheck: 1.3"); +// string_view node_state = life_cycle_mediator.stateToString(node.state); +// error_message = "[2OQ0] check failed: node in wrong state " + node.name + ": " + string(node_state); +// success = false; +// need_lifecycle_resend = true; +// } +// else if (node.status == LifeCycleStatus::ERROR && this->nodeNameIsSuper(node.name)) // IN ready, this will catch all errors EXCEPT the am_super one. +// { +// ROS_WARN_STREAM("hardik-allManifestedNodesCheck: 1.4"); +// error_message = "[AA0A] check failed: node status is ERROR: " + node.name; +// //if check method passes and we are in error, we want to pass +// success = false; +// need_lifecycle_resend = false; // TODO from Hardik: Should this be flipped? +// } +// } +// else +// { +// error_message = "[BJIL] check skipped: not manifested: " + node.name; +// need_lifecycle_resend = false; +// } +// if (!error_message.empty()) +// { +// failed_nodes.insert(pair>(node.name, pair(need_lifecycle_resend, error_message))); +// } +// ROS_WARN_STREAM("hardik-allManifestedNodesCheck: success:" << success << " latest_node:" << node.name); + +// } // for each node +// return std::pair(success, failed_nodes); +// } + + void SuperNodeMediator::parseManifest(Supervisor& supervisor, string manifest) { boost::erase_all(manifest, " "); @@ -405,4 +804,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..b8b5bed7 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. @@ -28,19 +28,24 @@ struct SuperStateInfo /**Registered constants for states mapping to values. */ const std::map state_info_ = { { SuperState::OFF, { "OFF", { SuperState::BOOTING } } }, - { SuperState::BOOTING, { "BOOTING", { SuperState::READY, SuperState::SHUTDOWN } } }, - { SuperState::READY, { "READY", { SuperState::ARMING, SuperState::SHUTDOWN } } }, - { SuperState::ARMING, { "ARMING", { SuperState::ARMED, SuperState::READY } } }, - { SuperState::ARMED, { "ARMED", { SuperState::AUTO, SuperState::DISARMING } } }, + { SuperState::BOOTING, { "BOOTING", { SuperState::BOOTING, SuperState::READY, SuperState::SHUTDOWN } } }, + { SuperState::READY, { "READY", { SuperState::AUTO, SuperState::BOOTING} } }, { SuperState::AUTO, - { "AUTO", { SuperState::DISARMING, SuperState::SEMI_AUTO, SuperState::HOLD, SuperState::ABORT, SuperState::MANUAL } } }, - { SuperState::DISARMING, {"DISARMING", { SuperState::READY } } }, - { SuperState::SEMI_AUTO, - { "SEMI_AUTO", { SuperState::AUTO, SuperState::HOLD, SuperState::ABORT, SuperState::MANUAL } } }, - { SuperState::HOLD, { "HOLD", { SuperState::ABORT, SuperState::MANUAL } } }, - { SuperState::ABORT, { "ABORT", { SuperState::DISARMING, SuperState::MANUAL } } }, - { SuperState::MANUAL, { "MANUAL", { SuperState::DISARMING } } }, - { SuperState::SHUTDOWN, { "SHUTDOWN", { SuperState::OFF } } }, + { "AUTO", { SuperState::READY, SuperState::BOOTING } } }, + + // OLD ONES + // // { SuperState::READY, { "READY", { SuperState::ARMING, SuperState::SHUTDOWN } } }, + // { SuperState::ARMING, { "ARMING", { SuperState::ARMED, SuperState::READY } } }, + // { SuperState::ARMED, { "ARMED", { SuperState::AUTO, SuperState::DISARMING } } }, + // // { SuperState::AUTO, + // // { "AUTO", { SuperState::DISARMING, SuperState::SEMI_AUTO, SuperState::HOLD, SuperState::ABORT, SuperState::MANUAL } } }, + // { SuperState::DISARMING, {"DISARMING", { SuperState::READY } } }, + // { SuperState::SEMI_AUTO, + // { "SEMI_AUTO", { SuperState::AUTO, SuperState::HOLD, SuperState::ABORT, SuperState::MANUAL } } }, + // { SuperState::HOLD, { "HOLD", { SuperState::ABORT, SuperState::MANUAL } } }, + // { SuperState::ABORT, { "ABORT", { SuperState::DISARMING, SuperState::MANUAL } } }, + // { SuperState::MANUAL, { "MANUAL", { SuperState::DISARMING } } }, + // { SuperState::SHUTDOWN, { "SHUTDOWN", { SuperState::OFF } } }, }; SuperStateMediator::SuperStateMediator() @@ -86,4 +91,4 @@ std::string_view SuperStateMediator::stateToString(SuperState state) return info(state).name_; } -} \ No newline at end of file +} diff --git a/src/resource_monitor/resource_monitor_main.cpp b/src/resource_monitor/resource_monitor_main.cpp new file mode 100644 index 00000000..81a4f729 --- /dev/null +++ b/src/resource_monitor/resource_monitor_main.cpp @@ -0,0 +1,28 @@ +#include + +#include +#include + +std::shared_ptr am::Node::node = nullptr; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + // create the AMLifeCycle object with stats and assign it to the AMNode singleton + std::shared_ptr am_node = std::make_shared("resource_monitor"); + std::shared_ptr stats = std::make_shared(am_node->stats_list_); + am::Node::node = am_node; + + // create the buisness logic object and give the AMLifecycle class a pointer to it + std::shared_ptr am_class = std::make_shared(stats); + am_node->setAMClass(am_class); + + ROS_INFO_STREAM(am::Node::node->get_name() << ": running..."); + + rclcpp::spin(am::Node::node); + + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file diff --git a/src/resource_monitor/resource_monitor_node.cpp b/src/resource_monitor/resource_monitor_node.cpp new file mode 100644 index 00000000..b56f100f --- /dev/null +++ b/src/resource_monitor/resource_monitor_node.cpp @@ -0,0 +1,65 @@ +#include +#include +#include +#include + +namespace am +{ + +ResourceMonitorNode::ResourceMonitorNode(const std::string & node_name) : AMLifeCycle(node_name) +{ +} + +ResourceMonitorNode::~ResourceMonitorNode() +{ + +} + +void ResourceMonitorNode::setAMClass(std::shared_ptr am_class) +{ + resource_status_= am_class; +} + +std::shared_ptr ResourceMonitorNode::getAMClass() +{ + return resource_status_; +} + +bool ResourceMonitorNode::onConfigure() +{ + if(configured_) + { + return AMLifeCycle::onConfigure(); + } + + ROS_INFO("onConfigure"); + + if(!resource_status_->onConfigure()) + { + ROS_WARN("am_class_->onConfigure() failed"); + resource_status_->onCleanup(); + return false; + } + else + { + configured_ = true; + return AMLifeCycle::onConfigure(); + } +} + +bool ResourceMonitorNode::onCleanup() +{ + ROS_INFO("onCleanup"); + + resource_status_->onCleanup(); + return AMLifeCycle::onCleanup(); +} + +void ResourceMonitorNode::heartbeatCB() +{ + resource_status_->heartbeatCB(); + AMLifeCycle::heartbeatCB(); +} + + +} // namespace \ No newline at end of file diff --git a/src/resource_monitor/resource_status_class.cpp b/src/resource_monitor/resource_status_class.cpp new file mode 100644 index 00000000..2e94962d --- /dev/null +++ b/src/resource_monitor/resource_status_class.cpp @@ -0,0 +1,681 @@ + + +#include +#include +#include // For popen and fgets +#include // For std::unique_ptr +#include // For std::regex +#include + +namespace am +{ +ResourceStatus::ResourceStatus(std::shared_ptr stats) : stats_(stats) +{ + transformer_ = std::make_shared(); + + sub_nets_add_ = getInetAddresses(); + + for(const std::string &ip : sub_nets_add_) + { + //ROS_INFO("subnet: %s", ip.c_str()); + if(ip == "192.168.1.1") + { + ip_check_ = true; + ROS_INFO("Resource Monitor: looking for sensors on 192.168.1.1"); + } + } + + getParams(); + + timer_ = am::Node::node->create_wall_timer(am::toDuration(1.0), std::bind(&ResourceStatus::timerCB, this)); + + cpu_cnt_ = getCPUCoresCount(); +} + +ResourceStatus::~ResourceStatus() +{ + +} + +void ResourceStatus::getParams() +{ + + //getting the ip sensor parameters + int counter = 0; + am::getParam("ip_sensor_cnt", counter, counter); + for(int i = 0; i < counter; i++) + { + std::string ip_check_str = "ip_sensor_" + std::to_string(i); + std::string ip_address = ""; + std::string sensor_name = ""; + am::getParam(ip_check_str + std::string(".ip_address") , ip_address, ip_address); + am::getParam(ip_check_str + std::string(".name") , sensor_name, sensor_name); + if(ip_address == "" || sensor_name == "") + { + ROS_ERROR("ip sensor %d has configuration issues: ip: %s and name: %s", i, ip_address.c_str(), sensor_name.c_str()); + continue; + } + + ip_addresses_[ip_address] = sensor_name; + ROS_INFO(GREEN "IP Sensor[%s] is configured as %s" COLOR_RESET, ip_address.c_str(), sensor_name.c_str()); + } + + //getting the transform list + counter = 0; + am::getParam("transform_cnt", counter, counter); + for(int i = 0; i < counter; i++) + { + std::string transform_str = "transform_" + std::to_string(i); + std::string src = ""; + std::string target = ""; + am::getParam(transform_str + std::string(".source") , src, src); + am::getParam(transform_str + std::string(".target") , target, target); + + if(src == "" || target == "") + { + ROS_ERROR("transform %d has configuration issues: source: %s and target: %s", i, src.c_str(), target.c_str()); + continue; + } + transform_list_.push_back(std::make_pair(src, target)); + ROS_INFO(GREEN "Transform check is set between source %s and target %s" COLOR_RESET, src.c_str(), target.c_str()); + } +} + +std::shared_ptr ResourceStatus::getStats() +{ + return stats_; +} + +bool ResourceStatus::onConfigure() +{ + status_sub_ = am::Node::node->create_subscription(std::string(am::Node::node->get_name()) + "/status", 100, std::bind(&ResourceStatus::statusCB, this, std::placeholders::_1)); + + stat_sub_ = am::Node::node->create_subscription(std::string(am::Node::node->get_name()) + "/stat", 100, std::bind(&ResourceStatus::statCB, this, std::placeholders::_1)); + + return true; +} + +bool ResourceStatus::onCleanup() +{ + status_sub_.reset(); + stat_sub_.reset(); + return true; +} + +void ResourceStatus::statusCB(const std_msgs::msg::Int32::SharedPtr msg) +{ + stats_->statStatus = msg->data; +} + +void ResourceStatus::statCB(const std_msgs::msg::Int32::SharedPtr msg) +{ + +} + +void ResourceStatus::heartbeatCB() +{ +} + + +int ResourceStatus::getCPUCoresCount() +{ + std::ifstream file("/proc/stat"); + if (!file.is_open()) { + ROS_ERROR("Unable to open /proc/stat"); + } + + int coreCount = 0; + std::string line; + + while (std::getline(file, line)) + { + if (line.compare(0, 3, "cpu") == 0 && line[3] >= '0' && line[3] <= '9') + { + coreCount++; + } + } + + file.close(); + return coreCount; +} + +am::CpuInfo ResourceStatus::parseCpuLine(const std::string& line) +{ + am::CpuInfo info = {0, 0, 0, 0, 0, 0, 0, 0, 0}; + std::istringstream iss(line); + std::string cpuLabel; // e.g., "cpu0", "cpu1", etc. + iss >> cpuLabel >> info.user >> info.nice >> info.system >> info.idle + >> info.iowait >> info.irq >> info.softirq >> info.steal; + + info.total = info.user + info.nice + info.system + info.idle + + info.iowait + info.irq + info.softirq + info.steal; + return info; +} + +void ResourceStatus::updateInfos() +{ + getMemoryInfo(); + if(cpu_cnt_ < 0) + { + cpu_cnt_ = getCPUCoresCount(); + if(cpu_cnt_ < 0) + { + ROS_ERROR("Cannot get CPU count"); + return; + } + } + + cpu_infos_.resize(cpu_cnt_); + cpu_infos_old_.resize(cpu_cnt_); + cpu_loads_.resize(cpu_cnt_); + + getCPUInfo(cpu_infos_); + + if(is_first_time_) + { + getCPUInfo(cpu_infos_old_); + is_first_time_ = false; + } + double avg_load = 0.0; + for(int i = 0; i < cpu_infos_.size(); i++) + { + double load = calculateCpuLoad(cpu_infos_[i], cpu_infos_old_[i]); + avg_load+=load; + cpu_loads_[i] = load; + } + + if(cpu_cnt_ > 0) + { + avg_load = avg_load/cpu_cnt_; + stats_->cpu_stats = (avg_load > 80.0?100:50); + } + + + uptime_seconds_ = getUpTime(); + + cpu_infos_old_ = cpu_infos_; + + gpu_info_ = getGPUInfo(); + stats_->gpu_stats = 50; + //ROS_INFO("GPU Load Percent: %d , Temp: %d", gpu_info_.load_percent, gpu_info_.temp); + if(gpu_info_.load_percent > 90) + { + stats_->gpu_stats = 100; + ROS_ERROR("GPU Issues: LOAD Percent: %d, Temp: %d", gpu_info_.load_percent, gpu_info_.temp); + } + + + //check the drive stats + stats_->drive_stats = 50; + am::DiskInfo disk_info = getDiskInfo(); + if(disk_info.percentUsed > 98.0) + { + stats_->drive_stats = 100; + int coef = 1024*1024; + ROS_ERROR("Disk total: %lld MB, available: %lld MB, used: %lld MB, percentage: %f", (disk_info.totalSpace/coef), (disk_info.availableSpace/coef), (disk_info.usedSpace/coef), disk_info.percentUsed); + } + +} + + +am::MemoryInfo& ResourceStatus::getMemoryInfo() +{ + mi = {0, 0, 0}; + std::ifstream file("/proc/meminfo"); + if (!file.is_open()) { + ROS_ERROR("Error: Unable to open /proc/meminfo"); + return mi; + } + + std::string line; + while (std::getline(file, line)) { + std::istringstream iss(line); + std::string key; + unsigned long value; + std::string unit; + + iss >> key >> value >> unit; + + if (key == "MemTotal:") { + mi.total = value; // in kB + } else if (key == "MemFree:") { + mi.free = value; // in kB + } else if (key == "Buffers:" || key == "Cached:") { + mi.used += value; // Add buffers and cached to used + }else if(key == "MemAvailable:"){ + mi.available = value; + } + + } + + // Calculate used memory + mi.used = mi.total - mi.free; + mi.used_percent = (mi.used / mi.total) * 100; + stats_->ram_stats = (mi.used_percent > 80?100:50); + file.close(); + return mi; +} + +// Function to read the content of a file +std::string ResourceStatus::readFile(const std::string& path) +{ + std::ifstream file(path); + if (!file.is_open()) { + throw std::runtime_error("Error: Unable to open file " + path); + } + + std::string content; + std::getline(file, content); + file.close(); + return content; +} + +am::GpuInfo ResourceStatus::getGPUInfo() +{ + am::GpuInfo gpu_info; + + // "/sys/devices/gpu.0/load" exists only in Jetpack + //in contrast, nvidia-smi only exists in amd64 architure + const std::string loadPath = "/sys/devices/gpu.0/load"; + if (boost::filesystem::exists(loadPath)) + { + std::string loadStr = readFile(loadPath); + gpu_info.load_percent = std::stoi(loadStr) / 1000; // Convert to percentage + + const std::string baseThermalPath = "/sys/class/thermal/"; + const std::string typeSuffix = "/type"; + const std::string tempSuffix = "/temp"; + + bool found_gpu_file = false; + for (int i = 0; i < 10; ++i) + { // Check up to 10 thermal zones + try { + std::string typePath = baseThermalPath + "thermal_zone" + std::to_string(i) + typeSuffix; + //ROS_INFO("Type file: %s", typePath.c_str()); + if(!boost::filesystem::exists(typePath)) + { + continue; + } + std::string type = readFile(typePath); + if (type.find("GPU") != std::string::npos) + { // Look for the GPU thermal zone + found_gpu_file = true; + std::string tempPath = baseThermalPath + "thermal_zone" + std::to_string(i) + tempSuffix; + std::string tempStr = readFile(tempPath); + gpu_info.temp = std::stoi(tempStr) / 1000; // Convert millidegrees to degrees Celsius + break; + } + } catch (...) + { + // Ignore errors and continue checking other zones + } + } + + return gpu_info; + } + + + // Execute the nvidia-smi command and read the output directly + const std::string command = "nvidia-smi --query-gpu=name,utilization.gpu,temperature.gpu,memory.used,memory.free --format=csv,nounits,noheader"; + FILE* pipe = popen(command.c_str(), "r"); + if (!pipe) + { + ROS_ERROR("Error: Unable to execute nvidia-smi. Ensure it's installed and available in PATH."); + return gpu_info; + } + + char buffer[128]; + std::ostringstream result; + while (fgets(buffer, sizeof(buffer), pipe) != nullptr) + { + result << buffer; + } + pclose(pipe); + + // Parse the command output + std::istringstream iss(result.str()); + std::string line; + while (std::getline(iss, line)) + { + //ROS_INFO(GREEN "%s" COLOR_RESET, line.c_str()); + std::istringstream lineStream(line); + // Parse memory used and free values + std::string gpuName; + int gpuUtilization, gpuTemperature, memoryUsed, memoryFree; + + // Using ',' to split the values + std::getline(lineStream, gpuName, ','); // Get the GPU name + + // Extracting the other values (removing leading/trailing spaces) + lineStream >> gpuUtilization; + lineStream.ignore(); // Ignore the space after the utilization + lineStream >> gpuTemperature; + lineStream.ignore(); // Ignore the space after the temperature + lineStream >> memoryUsed; + lineStream.ignore(); // Ignore the space after the memory used + lineStream >> memoryFree; + + gpu_info.gpu_name = gpuName; + gpu_info.load_percent = gpuUtilization; + gpu_info.temp = gpuTemperature; + + return gpu_info; + } + + + + return gpu_info; +} + + +void ResourceStatus::getCPUInfo(std::vector &infos) +{ + std::ifstream file("/proc/stat"); + if (!file.is_open()) { + ROS_ERROR("Error: Unable to open /proc/stat"); + return; + } + + std::string line; + int cnt = 0; + while (std::getline(file, line)) + { + if (line.find("cpu") == 0 && line.find("cpu ") != 0) + { + // Only process lines like "cpu0", "cpu1", etc. + infos[cnt] = parseCpuLine(line); + cnt++; + } + } + + file.close(); +} + +bool ResourceStatus::isReachable(const std::string &ip_address) +{ + std::string command = std::string("ping -c 1 -W 0.2 ") + ip_address + std::string(" >/dev/null 2>&1"); + + int result = std::system(command.c_str()); + + return result == 0; +} + +am::CpuInfo ResourceStatus::getCPUInfo() +{ + if(cpu_cnt_ < 0) + { + ROS_ERROR("Cannot get CPU Core Count: %d", cpu_cnt_); + } + + am::CpuInfo cpu_info = {0, 0, 0, 0, 0, 0, 0, 0, 0}; + std::ifstream file("/proc/stat"); + if (!file.is_open()) { + ROS_ERROR("Error: Unable to open /proc/stat"); + return cpu_info; + } + + std::string line; + if (std::getline(file, line)) { + std::istringstream iss(line); + std::string cpu; + iss >> cpu >> cpu_info.user >> cpu_info.nice >> cpu_info.system + >> cpu_info.idle >> cpu_info.iowait >> cpu_info.irq + >> cpu_info.softirq >> cpu_info.steal; + + // Total CPU time + cpu_info.total = cpu_info.user + cpu_info.nice + cpu_info.system + + cpu_info.idle + cpu_info.iowait + cpu_info.irq + + cpu_info.softirq + cpu_info.steal; + } + + file.close(); + return cpu_info; +} + +double ResourceStatus::calculateCpuLoad(const am::CpuInfo &ci, const am::CpuInfo &ci_old) +{ + unsigned long long totalDiff = ci.total - ci_old.total; + unsigned long long idleDiff = (ci.idle + ci.iowait) - (ci_old.idle + ci_old.iowait); + + cpu_usage_ = (totalDiff - idleDiff) * 100.0 / totalDiff; + + return cpu_usage_; +} + +double ResourceStatus::getUpTime() +{ + std::ifstream file("/proc/uptime"); + if (!file.is_open()) { + ROS_INFO("Error: Unable to open /proc/uptime"); + + return 0.0; + } + + double idleSeconds; + file >> uptime_seconds_ >> idleSeconds; + file.close(); + + return uptime_seconds_; +} + +void ResourceStatus::print() +{ + ROS_INFO("MemoryInfo---> Total: %ld MB, Free: %ld MB, Used: %ld MB, Available: %ld MB", (mi.total / 1024), (mi.free / 1024), (mi.used / 1024), (mi.available / 1024)); + + std::string msg = ""; + for(int i = 0; i < cpu_loads_.size(); i++) + { + msg += std::string(" Core[") + std::to_string(i) + "] Usage: " + std::to_string(cpu_loads_[i]); + } + + ROS_INFO("CPUInfo---> Cores: %d , %s", cpu_cnt_, msg.c_str()); + + ROS_INFO("UpTime: %f", uptime_seconds_); + + msg = ""; + msg += "GPU: Temp[C] = " + std::to_string(gpu_info_.temp) + ", Used[%]: " + std::to_string(gpu_info_.load_percent); + + ROS_INFO("%s", msg.c_str()); +} + +void ResourceStatus::checkNodeNames() +{ + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph = am::Node::node->get_node_graph_interface(); + + std::vector running_nodes = node_graph->get_node_names(); + + std::unordered_map string_count; + + // Count occurrences of each string + for (const std::string& str : running_nodes) + { + if(str.find("plugin_name") != std::string::npos) + { + continue; + } + string_count[str]++; + } + + // Collect strings that appear more than once + bool node_check = true; + for (const auto& [str, count] : string_count) + { + if (count > 1) + { + ROS_ERROR("Found a duplicate Node: %s", str.c_str()); + node_check = false; + } + } + stats_->node_stats = (node_check?50:100); +} + +void ResourceStatus::checkTransforms() +{ + bool tf_check = true; + for(std::pair &tf_str : transform_list_) + { + geometry_msgs::msg::TransformStamped transform; + if(!transformer_->getTransform(tf_str.first, tf_str.second, transform, 1.0, false)) + { + //ROS_ERROR("Transform tree is broken: %s, %s", tf_str.first.c_str(), tf_str.second.c_str()); + tf_check = false; + } + } + stats_->tf_stats = (tf_check?50:100); +} + +void ResourceStatus::checkSensorIPs() +{ + + //IP Address Check + stats_->lidar_ip = 50; + stats_->fl_ip = 50; + stats_->fr_ip = 50; + stats_->rl_ip = 50; + stats_->rr_ip = 50; + //Only if you have the subnet + if(ip_check_) + { + std::map::iterator it = ip_addresses_.begin(); + for(; it != ip_addresses_.end(); ++it) + { + + if(!isReachable(it->first)) + { + //THE DEVICE CANNOT BE REACHED + if(it->second == "lidar") + { + stats_->lidar_ip = 100; + ROS_ERROR("Lidar is not reachable"); + } + if(it->second == "front_left") + { + stats_->fl_ip = 100; + ROS_ERROR("Front Left Camera is not reachable"); + } + if(it->second == "front_right") + { + stats_->fr_ip = 100; + ROS_ERROR("Front Right Camera is not reachable"); + } + if(it->second == "rear_right") + { + stats_->rr_ip = 100; + ROS_ERROR("Rear Right Camera is not reachable"); + } + if(it->second == "rear_left") + { + stats_->rl_ip = 100; + ROS_ERROR("Rear Left Camera is not reachable"); + } + } + } + } +} + + +// Function to execute the nmap command and capture the output +std::unordered_set ResourceStatus::getActiveIPs(const std::string& subnet) +{ + std::unordered_set activeIPs; + std::string command = "nmap -sn " + subnet; + + // Open a pipe to execute the command and read its output + std::unique_ptr pipe(popen(command.c_str(), "r"), pclose); + if (!pipe) { + std::cerr << "Error: Failed to run nmap command.\n"; + return activeIPs; + } + + // Read the command output line by line + char buffer[128]; + std::string line; + while (fgets(buffer, sizeof(buffer), pipe.get()) != nullptr) + { + line = buffer; + // Check if the line contains "Nmap scan report for", indicating a live IP + if (line.find("Nmap scan report for") != std::string::npos) + { + std::string ip = line.substr(line.find_last_of(' ') + 1); + ip.erase(ip.find('\n')); // Remove the newline character + activeIPs.insert(ip); + } + } + + return activeIPs; +} + + +// Function to execute ifconfig and extract inet addresses +std::vector ResourceStatus::getInetAddresses() +{ + std::vector inetAddresses; + std::string command = "ifconfig"; + std::unique_ptr pipe(popen(command.c_str(), "r"), pclose); + + if (!pipe) { + std::cerr << "Error: Failed to run ifconfig command.\n"; + return inetAddresses; + } + + char buffer[256]; + std::string output; + + // Read the entire output of ifconfig + while (fgets(buffer, sizeof(buffer), pipe.get()) != nullptr) { + output += buffer; + } + + // Regular expression to match inet (IPv4) addresses + std::regex inetRegex(R"(inet\s+(\d+\.\d+\.\d+\.\d+))"); + std::smatch match; + + // Search for inet addresses in the output + auto begin = output.cbegin(); + auto end = output.cend(); + while (std::regex_search(begin, end, match, inetRegex)) { + inetAddresses.push_back(match[1]); + begin = match.suffix().first; // Move to the next match + } + + return inetAddresses; +} + + +// Function to get disk usage information +DiskInfo ResourceStatus::getDiskInfo(const std::string& path) +{ + struct statvfs stat; + + if (statvfs(path.c_str(), &stat) != 0) { + throw std::runtime_error("Error: Unable to get disk information for " + path); + } + + DiskInfo info; + info.totalSpace = stat.f_blocks * stat.f_frsize; // Total blocks * block size + info.availableSpace = stat.f_bavail * stat.f_frsize; // Available blocks * block size + info.usedSpace = info.totalSpace - info.availableSpace; + info.percentUsed = (info.usedSpace * 100.0) / info.totalSpace; + + return info; +} + +/* + Timer Callback: this is where everything is updated + */ +void ResourceStatus::timerCB() +{ + //Checking the repeated node name + checkNodeNames(); + + //Transform check + checkTransforms(); + + //sensor ip check + checkSensorIPs(); + + //Resource Check + updateInfos(); + +} +} \ 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