From fc4f31af7f4ae39438730ab7c7b64a8d3c7a12bb Mon Sep 17 00:00:00 2001 From: AJ Date: Mon, 19 Dec 2022 22:54:32 -0800 Subject: [PATCH 01/45] feat: changes for am_super so far --- CMakeLists.txt | 300 +++++++----------------- CMakeLists_ros1.txt | 245 +++++++++++++++++++ include/am_super/baby_sitter.h | 110 ++++----- include/am_super/super_state.h | 26 +- include/super_lib/am_life_cycle.h | 20 +- include/super_lib/am_life_cycle_types.h | 48 ++-- include/super_lib/am_stat.h | 4 +- include/super_lib/am_stat_list.h | 2 +- package.xml | 36 +-- package_ros1.xml | 26 ++ src/am_super/am_super.cpp | 246 ++++++++++--------- src/super_lib/am_life_cycle.cpp | 26 +- 12 files changed, 625 insertions(+), 464 deletions(-) create mode 100644 CMakeLists_ros1.txt create mode 100644 package_ros1.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index bd1a0d96..351c5ec4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,114 +1,50 @@ -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) - -## 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 -) +# 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 - am_utils - diagnostic_msgs - rosbag - roscpp - rospy - std_msgs - std_srvs -) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + #to view [-Wunused-parameter] uncomment the following line + #add_compile_options(-Wall -Wextra -Wpedantic) +endif() -########### -## Build ## -########### +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(brain_box_msgs REQUIRED) +find_package(am_utils REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rosbag2 REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(rosbag2_cpp REQUIRED) +find_package(diagnostic_msgs REQUIRED) +find_package(diagnostic_updater REQUIRED) + +set(dependencies + brain_box_msgs + am_utils + builtin_interfaces + std_msgs + rosbag2 + rclcpp + rclpy + rosbag2_cpp + diagnostic_msgs + diagnostic_updater + ) -## Specify additional locations of header files -## Your package locations should be listed before other locations include_directories( - include - ${catkin_INCLUDE_DIRS} -) + include + ) file(GLOB super_lib_cpp_files @@ -127,119 +63,51 @@ 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 +target_link_libraries(super_lib) + +ament_target_dependencies(super_lib ${dependencies}) +ament_export_targets(super_libTargets HAS_LIBRARY_TARGET) +ament_export_include_directories(include) +ament_export_libraries(super_lib) + + +#add_executable(am_super ${am_super_cpp_files}) +#target_link_libraries(am_super super_lib) +#ament_target_dependencies(am_super ${dependencies}) + + +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +install(DIRECTORY include/ + DESTINATION include/ + ) +install( + TARGETS super_lib + EXPORT super_libTargets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include ) +#install(TARGETS +# am_super +# DESTINATION lib/${PROJECT_NAME}) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() - -# 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 +ament_export_include_directories(include) +ament_export_libraries(am_utils) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/CMakeLists_ros1.txt b/CMakeLists_ros1.txt new file mode 100644 index 00000000..bd1a0d96 --- /dev/null +++ b/CMakeLists_ros1.txt @@ -0,0 +1,245 @@ +cmake_minimum_required(VERSION 2.8.3) +project(am_super) + +## Compile as C++11, supported in ROS melodic and newer +add_compile_options(-std=c++17) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED + am_utils + diagnostic_msgs + rosbag + roscpp + rospy + std_msgs + std_srvs + am_rostest +) + +#find_package(CUDA) + +#if(CUDA_FOUND) +# add_definitions(-DCUDA_FLAG) +# message([STATUS] "CUDA FOUND, am_super will be built with GPU monitoring") +# SET(CUDA_NVCC_FLAGS "-arch=sm_62" CACHE STRING "nvcc flags" FORCE) +# SET(CUDA_VERBOSE_BUILD ON CACHE BOOL "nvcc verbose" FORCE) +# CUDA_ADD_LIBRARY(cuda_utility ${LIB_TYPE} src/cuda/cuda_utility.cu) +#endif(CUDA_FOUND) + + +## System dependencies are found with CMake's conventions + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES super_lib + CATKIN_DEPENDS + am_utils + diagnostic_msgs + rosbag + roscpp + rospy + std_msgs + std_srvs +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + + +file(GLOB super_lib_cpp_files + src/super_lib/*.cpp +) + +file(GLOB cuda_cpp_files + src/cuda/*.cpp +) + +file(GLOB am_super_cpp_files + src/am_super/*.cpp +) + +file(GLOB SUPER_MEDIATOR_FILES + src/am_super/*_mediator.cpp +) + +## Declare a C++ library +add_library(super_lib + ${super_lib_cpp_files} +) +target_link_libraries(super_lib ${catkin_LIBRARIES}) +add_dependencies(super_lib ${catkin_EXPORTED_TARGETS}) + +#if(CUDA_FOUND) +# add_executable(am_super ${am_super_cpp_files} ${cuda_cpp_files} ) +# target_link_libraries(am_super ${catkin_LIBRARIES} super_lib cuda_utility) +#else() +# add_executable(am_super ${am_super_cpp_files}) +# target_link_libraries(am_super ${catkin_LIBRARIES} super_lib) +#endif(CUDA_FOUND) + +add_executable(am_super ${am_super_cpp_files}) +target_link_libraries(am_super ${catkin_LIBRARIES} super_lib) +add_dependencies(am_super ${catkin_EXPORTED_TARGETS}) + +############# +## Install ## +############# +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html + install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + +# Mark libraries for installation +# See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html + install(TARGETS super_lib + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) + +#Mark cpp header files for installation +install(DIRECTORY include/super_lib + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +if(CATKIN_ENABLE_TESTING AND ENABLE_COVERAGE_TESTING) + find_package(code_coverage REQUIRED) + # Add compiler flags for coverage instrumentation before defining any targets + APPEND_COVERAGE_COMPILER_FLAGS() +endif() + +file(GLOB TEST_FILES + test/*_tests.cpp +) + + +# code coverage setup: https://github.com/mikeferguson/code_coverage +if (CATKIN_ENABLE_TESTING) + + ## Add gtest based cpp test target and link libraries + + catkin_add_gtest(${PROJECT_NAME}_test ${TEST_FILES} ${SUPER_MEDIATOR_FILES}) + + if(TARGET ${PROJECT_NAME}_test) + target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES} super_lib) + endif() + + # For ros testing + find_package(rostest REQUIRED) + + #follow test naming convention of files in a folder and the cpp and launch have the test name with _rostest + set(TEST_NAMES abort_to_disarming + abort_to_manual + armed_to_ready + auto_to_abort + auto_to_manual + auto_to_semiauto + error_configure_tolerance + error_forced + error_status + error_status_without_stats + error_terminal_before_config + error_tolerant_before_config + hz_config + manual_to_disarming + param + platform_app_required_fail + platform_app_required_pass + platform_required_fail + platform_required_pass + primary + ready_to_shutdown + semi_auto_to_manual + ) + foreach(TEST_NAME ${TEST_NAMES}) + set(PROJECT_TEST_NAME ${TEST_NAME}_${PROJECT_NAME}_rostest) + add_rostest_gtest(${PROJECT_TEST_NAME} rostest/${TEST_NAME}/${TEST_NAME}_rostest.test rostest/${TEST_NAME}/${TEST_NAME}_rostest.cpp) + target_link_libraries(${PROJECT_TEST_NAME} ${catkin_LIBRARIES} super_lib) + endforeach() +endif() \ No newline at end of file diff --git a/include/am_super/baby_sitter.h b/include/am_super/baby_sitter.h index ead1fe76..78b70797 100644 --- a/include/am_super/baby_sitter.h +++ b/include/am_super/baby_sitter.h @@ -3,12 +3,13 @@ #include #include -#include +#include -#include -#include +#include +#include #include #include +#include namespace am { @@ -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::Subscription::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(const M::SharedPtr event); + void heartbeatCB(); void checkNodeState(); void setNodeState(LifeCycleState node_state); std::string parseNodeState(LifeCycleState state); @@ -92,29 +93,29 @@ 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_); + 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_); + 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_); + 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_); + RCLCPP_INFO_STREAM(nh_->get_logger(), nh_->get_name() << parm << " = " << timeout_ms_); min_ms_ = 1000; max_ms_ = 0; @@ -135,11 +136,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_subscription(topic, 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 +213,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 +235,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 +243,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 +255,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(const M::SharedPtr 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 +284,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 +322,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,7 +345,7 @@ 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_; @@ -356,7 +358,7 @@ void BabySitter::heartbeatCB(const ros::TimerEvent& event) 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(), NODE_FUNC << node_name_ << ": timed out"); device_state_ = DeviceState::ERROR; checkNodeState(); } @@ -364,15 +366,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/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/super_lib/am_life_cycle.h b/include/super_lib/am_life_cycle.h index d1fca67a..e8940afc 100644 --- a/include/super_lib/am_life_cycle.h +++ b/include/super_lib/am_life_cycle.h @@ -3,9 +3,9 @@ #include -#include +#include -#include +#include #include #include @@ -53,7 +53,7 @@ class AMLifeCycle /**The moment configuration is requested for this node. Used with * max_configure_seconds_ to allow startup error tolerance.*/ - ros::Time configure_start_time_; + rclcpp::Time configure_start_time_; void setState(const LifeCycleState state); @@ -85,15 +85,15 @@ class AMLifeCycle diagnostic_updater::Updater updater_; AMStatList stats_list_; - ros::NodeHandle nh_; - ros::Timer heartbeat_timer_; - ros::Publisher state_pub_; - ros::Subscriber lifecycle_sub_; + rclcpp::Node::SharedPtr node_; + rclcpp::TimerBase::SharedPtr heartbeat_timer_; + rclcpp::Publisher::SharedPtr state_pub_; + rclcpp::Subscription::SharedPtr lifecycle_sub_; /** * @brief Default constructor */ - AMLifeCycle(); + AMLifeCycle(rclcpp::Node::SharedPtr node); /** * @brief Virtual destructor @@ -226,9 +226,9 @@ class AMLifeCycle * Useful for checking health regularly, but not during * callbacks which can affect performance and be too granular */ - virtual void heartbeatCB(const ros::TimerEvent& event); + virtual void heartbeatCB(); - void lifecycleCB(const brain_box_msgs::LifeCycleCommand::ConstPtr msg); + void lifecycleCB(const brain_box_msgs::msg::LifeCycleCommand::SharedPtr msg); double getThrottleS() const; diff --git a/include/super_lib/am_life_cycle_types.h b/include/super_lib/am_life_cycle_types.h index a623891b..7d006d77 100644 --- a/include/super_lib/am_life_cycle_types.h +++ b/include/super_lib/am_life_cycle_types.h @@ -2,8 +2,8 @@ #define AM_SUPER_INCLUDE_SUPER_LIB_AM_LIFECYCLE_TYPES_H_ #include -#include -#include +#include +#include namespace am { @@ -12,17 +12,17 @@ namespace am */ enum class LifeCycleState : std::uint8_t { - INVALID = brain_box_msgs::LifeCycleState::STATE_INVALID, - UNCONFIGURED = brain_box_msgs::LifeCycleState::STATE_UNCONFIGURED, - INACTIVE = brain_box_msgs::LifeCycleState::STATE_INACTIVE, - ACTIVE = brain_box_msgs::LifeCycleState::STATE_ACTIVE, - FINALIZED = brain_box_msgs::LifeCycleState::STATE_FINALIZED, - CONFIGURING = brain_box_msgs::LifeCycleState::STATE_CONFIGURING, - CLEANING_UP = brain_box_msgs::LifeCycleState::STATE_CLEANING_UP, - SHUTTING_DOWN = brain_box_msgs::LifeCycleState::STATE_SHUTTING_DOWN, - ACTIVATING = brain_box_msgs::LifeCycleState::STATE_ACTIVATING, - DEACTIVATING = brain_box_msgs::LifeCycleState::STATE_DEACTIVATING, - ERROR_PROCESSING = brain_box_msgs::LifeCycleState::STATE_ERROR_PROCESSING + INVALID = brain_box_msgs::msg::LifeCycleState::STATE_INVALID, + UNCONFIGURED = brain_box_msgs::msg::LifeCycleState::STATE_UNCONFIGURED, + INACTIVE = brain_box_msgs::msg::LifeCycleState::STATE_INACTIVE, + ACTIVE = brain_box_msgs::msg::LifeCycleState::STATE_ACTIVE, + FINALIZED = brain_box_msgs::msg::LifeCycleState::STATE_FINALIZED, + CONFIGURING = brain_box_msgs::msg::LifeCycleState::STATE_CONFIGURING, + CLEANING_UP = brain_box_msgs::msg::LifeCycleState::STATE_CLEANING_UP, + SHUTTING_DOWN = brain_box_msgs::msg::LifeCycleState::STATE_SHUTTING_DOWN, + ACTIVATING = brain_box_msgs::msg::LifeCycleState::STATE_ACTIVATING, + DEACTIVATING = brain_box_msgs::msg::LifeCycleState::STATE_DEACTIVATING, + ERROR_PROCESSING = brain_box_msgs::msg::LifeCycleState::STATE_ERROR_PROCESSING }; /** @@ -30,9 +30,9 @@ enum class LifeCycleState : std::uint8_t */ enum class LifeCycleStatus : std::uint8_t { - OK = brain_box_msgs::LifeCycleState::STATUS_OK, - WARN = brain_box_msgs::LifeCycleState::STATUS_WARN, - ERROR = brain_box_msgs::LifeCycleState::STATUS_ERROR + OK = brain_box_msgs::msg::LifeCycleState::STATUS_OK, + WARN = brain_box_msgs::msg::LifeCycleState::STATUS_WARN, + ERROR = brain_box_msgs::msg::LifeCycleState::STATUS_ERROR }; /** @@ -40,16 +40,16 @@ enum class LifeCycleStatus : std::uint8_t */ enum class LifeCycleCommand : std::uint8_t { - CREATE = brain_box_msgs::LifeCycleCommand::COMMAND_CREATE, - CONFIGURE = brain_box_msgs::LifeCycleCommand::COMMAND_CONFIGURE, - CLEANUP = brain_box_msgs::LifeCycleCommand::COMMAND_CLEANUP, - ACTIVATE = brain_box_msgs::LifeCycleCommand::COMMAND_ACTIVATE, - DEACTIVATE = brain_box_msgs::LifeCycleCommand::COMMAND_DEACTIVATE, - SHUTDOWN = brain_box_msgs::LifeCycleCommand::COMMAND_SHUTDOWN, - DESTROY = brain_box_msgs::LifeCycleCommand::COMMAND_DESTROY, + CREATE = brain_box_msgs::msg::LifeCycleCommand::COMMAND_CREATE, + CONFIGURE = brain_box_msgs::msg::LifeCycleCommand::COMMAND_CONFIGURE, + CLEANUP = brain_box_msgs::msg::LifeCycleCommand::COMMAND_CLEANUP, + ACTIVATE = brain_box_msgs::msg::LifeCycleCommand::COMMAND_ACTIVATE, + DEACTIVATE = brain_box_msgs::msg::LifeCycleCommand::COMMAND_DEACTIVATE, + SHUTDOWN = brain_box_msgs::msg::LifeCycleCommand::COMMAND_SHUTDOWN, + DESTROY = brain_box_msgs::msg::LifeCycleCommand::COMMAND_DESTROY, //FIXME: there should be no last command - LAST_COMMAND = brain_box_msgs::LifeCycleCommand::COMMAND_LAST + LAST_COMMAND = brain_box_msgs::msg::LifeCycleCommand::COMMAND_LAST }; }; // namespace am diff --git a/include/super_lib/am_stat.h b/include/super_lib/am_stat.h index e1280304..9868623a 100644 --- a/include/super_lib/am_stat.h +++ b/include/super_lib/am_stat.h @@ -4,10 +4,10 @@ #include #include -#include +#include #include -#include +#include namespace am { diff --git a/include/super_lib/am_stat_list.h b/include/super_lib/am_stat_list.h index 97e69585..86c8d621 100644 --- a/include/super_lib/am_stat_list.h +++ b/include/super_lib/am_stat_list.h @@ -3,7 +3,7 @@ #include -#include +#include #include namespace am diff --git a/package.xml b/package.xml index 1baec1cf..625729f8 100644 --- a/package.xml +++ b/package.xml @@ -1,26 +1,30 @@ - + + am_super 0.0.0 - AutoModality Supervisor - AutoModality + TODO: Package description + alireza + TODO: License declaration - - - - - TODO - - catkin + ament_cmake + am_utils brain_box_msgs diagnostic_msgs - rosbag - roscpp - rospy + rosbag2 + rosbag2_cpp + rclcpp + rclpy std_msgs std_srvs - rosunit - code_coverage - am_rostest + builtin_interfaces + diagnostic_updater + + 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/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index 1fa3de2d..8270f6bf 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -1,28 +1,29 @@ #include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include @@ -59,26 +60,26 @@ class AMSuper : AMLifeCycle /** * the ros node handle */ - ros::NodeHandle nh_; + rclcpp::Node::SharedPtr nh_; /* * see constructor for details */ - ros::Publisher lifecycle_pub_; - ros::Publisher vstate_summary_pub_; - ros::Publisher system_state_pub_; - ros::Publisher super_status_pub_; - ros::Publisher led_pub_; + rclcpp::Publisher::SharedPtr lifecycle_pub_; + rclcpp::Publisher::SharedPtr vstate_summary_pub_; + rclcpp::Publisher::SharedPtr system_state_pub_; + rclcpp::Publisher::SharedPtr super_status_pub_; + rclcpp::Publisher::SharedPtr led_pub_; /** stops the flight plan when SHUTDOWN state */ - ros::Publisher flight_plan_deactivation_pub_; - ros::Subscriber node_state_sub_; - ros::Subscriber operator_command_sub_; - ros::Subscriber controller_state_sub; - ros::Subscriber diagnostics_sub; - ros::Subscriber current_enu_sub; - ros::Timer heartbeat_timer_; - - ros::Subscriber log_control_sub_; + rclcpp::Publisher::SharedPtr flight_plan_deactivation_pub_; + rclcpp::Subscription::SharedPtr node_state_sub_; + rclcpp::Subscription::SharedPtr operator_command_sub_; + rclcpp::Subscription::SharedPtr controller_state_sub; + rclcpp::Subscription::SharedPtr diagnostics_sub; + rclcpp::Subscription::SharedPtr current_enu_sub; + rclcpp::TimerBase::SharedPtr heartbeat_timer_; + + rclcpp::Subscription::SharedPtr log_control_sub_; BagLogger::BagLoggerLevel log_level_; /** manage logic for SuperState transitions */ @@ -108,13 +109,13 @@ class AMSuper : AMLifeCycle // const std::string NODE_BS_ALTIMETER = "can_node"; // TODO: replace with system global const - typedef brain_box_msgs::StampedAltimeter altimeter_bs_msg_type; + typedef brain_box_msgs::msg::StampedAltimeter altimeter_bs_msg_type; am::BabySitter* altimeter_bs_; const std::string ALTIMETER_BS_TOPIC = "/sensor/distance/agl_lw"; // TODO: replace with system global const const int ALTIMETER_HZ = 20; const std::string NODE_BS_DJI = "dji_sdk"; // TODO: replace with system global const - typedef sensor_msgs::Joy dji_bs_msg_type; + typedef sensor_msgs::msg::Joy dji_bs_msg_type; am::BabySitter* dji_bs_; const std::string DJI_BS_TOPIC = "/dji_sdk/rc"; // TODO: replace with system global const const int DJI_HZ = 50; @@ -124,12 +125,12 @@ class AMSuper : AMLifeCycle #endif public: - AMSuper() : nh_("~"), node_mediator_(SuperNodeMediator::nodeNameStripped(ros::this_node::getName())) + AMSuper(rclcpp::Node::SharedPtr nh) : nh_(nh), node_mediator_(SuperNodeMediator::nodeNameStripped(nh->get_name())) { - ROS_INFO_STREAM(NODE_FUNC); + RCLCPP_INFO_STREAM(nh_->get_logger(), nh_->get_name()); ros::param::param("~node_timeout_s", node_timeout_s_, 2.0); - ROS_INFO_STREAM("node_timeout_s = " << node_timeout_s_); + RCLCPP_INFO_STREAM(nh_->get_logger(), "node_timeout_s = " << node_timeout_s_); /* * create initial node list from manifest and create babysitters as needed @@ -147,13 +148,13 @@ class AMSuper : AMLifeCycle // if a manifest has been specified if (!supervisor_.manifest.empty()) { - ROS_INFO_STREAM("configuring nodes from manifest: " << manifest_param); + RCLCPP_INFO_STREAM(nh_->get_logger(), "configuring nodes from manifest: " << manifest_param); for (string& name : supervisor_.manifest) { // create a new node in the list for each name in manifest SuperNodeMediator::SuperNodeInfo nr = node_mediator_.initializeManifestedNode(name); supervisor_.nodes.insert(pair(name, nr)); - ROS_INFO_STREAM(" " << name); + RCLCPP_INFO_STREAM(nh_->get_logger(), " " << name); // create babysitters based on hard coded node names if (!name.compare(NODE_BS_ALTIMETER)) @@ -174,7 +175,7 @@ class AMSuper : AMLifeCycle } else { - ROS_WARN_STREAM("Manifest is empty. No nodes will be monitored."); + RCLCPP_WARN_STREAM(nh_->get_logger(), "Manifest is empty. No nodes will be monitored."); } reportSystemState(); @@ -187,22 +188,22 @@ class AMSuper : AMLifeCycle /** * system status pub */ - vstate_summary_pub_ = nh_.advertise(am_super_topics::SUPER_STATE, 1000); - system_state_pub_ = nh_.advertise(am_topics::SYSTEM_STATE, 1000); + vstate_summary_pub_ = nh_->create_publisher(am_super_topics::SUPER_STATE, 1000); + system_state_pub_ = nh_->create_publisher(am_topics::SYSTEM_STATE, 1000); /**Super * node lifecycle state pub. used to tell nodes to change their lifecycle state. */ - lifecycle_pub_ = nh_.advertise(am_super_topics::NODE_LIFECYCLE, 100); + lifecycle_pub_ = nh_->create_publisher(am_super_topics::NODE_LIFECYCLE, 100); /** * led control pub */ - led_pub_ = nh_.advertise(am::am_topics::LED_BLINK, 1000); + led_pub_ = nh_->create_publisher(am::am_topics::LED_BLINK, 1000); /** * super status contains online naode list for gcs_comms */ - super_status_pub_ = nh_.advertise(am_super_topics::SUPER_STATUS, 1000); + super_status_pub_ = nh_->create_publisher(am_super_topics::SUPER_STATUS, 1000); - flight_plan_deactivation_pub_ = nh_.advertise(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, 1000); + flight_plan_deactivation_pub_ = nh_->create_publisher(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, 1000); supervisor_.system_state = SuperState::BOOTING; supervisor_.flt_ctrl_state = SuperNodeMediator::SuperFltCtrlState::INIT; @@ -210,10 +211,11 @@ class AMSuper : AMLifeCycle /** * amros log control */ - log_control_sub_ = nh_.subscribe(am::am_topics::CTRL_LOG_CONTROL, 10, &AMSuper::logControlCB, this); + log_control_sub_ = nh_->create_subscription(am::am_topics::CTRL_LOG_CONTROL, 10, + std::bind(&AMSuper::logControlCB, this, std::placeholders::_1)); // startup bagfile - gets closed after frist log control command - ROS_INFO_STREAM("start logging to ST, level " << SU_LOG_LEVEL); + RCLCPP_INFO_STREAM(nh_->get_logger(), "start logging to ST, level " << SU_LOG_LEVEL); BagLogger::instance()->startLogging("ST", SU_LOG_LEVEL); log_level_ = intToLoggerLevel (SU_LOG_LEVEL); @@ -222,20 +224,25 @@ class AMSuper : AMLifeCycle /** * node status via LifeCycle */ - node_state_sub_ = nh_.subscribe(am_super_topics::LIFECYCLE_STATE, 100, &AMSuper::nodeStateCB, this); + node_state_sub_ = nh_->create_subscription(am_super_topics::LIFECYCLE_STATE, 100, + std::bind(&AMSuper::nodeStateCB, this, std::placeholders::_1)); /** * commands from operator */ - operator_command_sub_ = nh_.subscribe(am_super_topics::OPERATOR_COMMAND, 100, &AMSuper::operatorCommandCB, this); + operator_command_sub_ = nh_->create_subscription(am_super_topics::OPERATOR_COMMAND, 100, + std::bind(&AMSuper::operatorCommandCB, this, std::placeholders::_1)); - controller_state_sub = nh_.subscribe(am_super_topics::CONTROLLER_STATE, 100, &AMSuper::controllerStateCB, this); + controller_state_sub = nh_->create_subscription(am_super_topics::CONTROLLER_STATE, 100, + std::bind(&AMSuper::controllerStateCB, this, std::placeholders::_1)); - diagnostics_sub = nh_.subscribe("/diagnostics", 100, &AMSuper::diagnosticsCB, this); + diagnostics_sub = nh_->create_subscription("/diagnostics", 100, + std::bind(&AMSuper::diagnosticsCB, this, std::placeholders::_1)); - current_enu_sub = nh_.subscribe(am_topics::CTRL_VX_VEHICLE_CURRENTENU, 100, &AMSuper::currentENUCB, this); + current_enu_sub = nh_->create_subscription(am_topics::CTRL_VX_VEHICLE_CURRENTENU, 100, + std::bind(&AMSuper::currentENUCB, this, std::placeholders::_1)); - heartbeat_timer_ = nh_.createTimer(ros::Duration(1.0), &AMSuper::heartbeatCB, this); + heartbeat_timer_ = nh_->create_wall_timer(am::toDuration(1.0), std::bind(&AMSuper::heartbeatCB, this)); } ~AMSuper() @@ -253,9 +260,10 @@ class AMSuper : AMLifeCycle * LifeCycle messages are sent once a second by the LifeCycle heartbeat, but may * come more frequently if a node chooses. */ - void nodeStateCB(const ros::MessageEvent& event) + //void nodeStateCB(const rclcpp::MessageEvent& event) + void nodeStateCB(const brain_box_msgs::msg::LifeCycleState::SharedPtr rmsg) { - const brain_box_msgs::LifeCycleState::ConstPtr& rmsg = event.getMessage(); + //const brain_box_msgs::LifeCycleState::ConstPtr& rmsg = event.getMessage(); /* * process the message @@ -265,25 +273,27 @@ 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); } - void controllerStateCB(const ros::MessageEvent& event) + //void controllerStateCB(const ros::MessageEvent& event) + void controllerStateCB(const brain_box_msgs::msg::ControllerState::SharedPtr rmsg) { - const brain_box_msgs::ControllerState::ConstPtr& rmsg = event.getMessage(); + //const brain_box_msgs::ControllerState::ConstPtr& rmsg = event.getMessage(); - ROS_INFO("Received Controller State: %s sent %i", rmsg->node_name.c_str(), rmsg->state); + RCLCPP_INFO(nh_->get_logger(), "Received Controller State: %s sent %i", rmsg->node_name.c_str(), rmsg->state); node_mediator_.setControllerState(supervisor_, (ControllerState)rmsg->state); } - void operatorCommandCB(const ros::MessageEvent& event) + //void operatorCommandCB(const ros::MessageEvent& event) + void operatorCommandCB(const brain_box_msgs::msg::OperatorCommand::SharedPtr rmsg) { - const brain_box_msgs::OperatorCommand::ConstPtr& rmsg = event.getMessage(); + //const brain_box_msgs::OperatorCommand::ConstPtr& rmsg = event.getMessage(); - ROS_INFO("Received Operator Command: %s sent '%i'",rmsg->node_name.c_str(),rmsg->command ); + RCLCPP_INFO(nh_->get_logger(), "Received Operator Command: %s sent '%i'",rmsg->node_name.c_str(),rmsg->command ); node_mediator_.setOperatorCommand(supervisor_, (OperatorCommand)rmsg->command); // TODO: topic name should come from vb_util_lib::topics. @@ -301,7 +311,7 @@ class AMSuper : AMLifeCycle */ void processState(const std::string& node_name_in, const am::LifeCycleState state, const am::LifeCycleStatus status, const std::string& subsystem, const std::string& value, const int pid, - const ros::Time& last_contact) + const rclcpp::Time& last_contact) { // strip leading '/' from the node name if needed string node_name = node_mediator_.nodeNameStripped(node_name_in); @@ -316,25 +326,25 @@ class AMSuper : AMLifeCycle SuperNodeMediator::SuperNodeInfo& nr = it->second; if (!nr.online) { - ROS_INFO_STREAM("manifested node '" << node_name << "' came online [PGPG]"); + RCLCPP_INFO_STREAM(nh_->get_logger(), "manifested node '" << node_name << "' came online [PGPG]"); nr.online = true; nodes_changed = true; } if (nr.state != state) { - ROS_INFO_STREAM(node_name << " changed state to = " << life_cycle_mediator_.stateToString(state) << " [38S8]"); + RCLCPP_INFO_STREAM(nh_->get_logger(), node_name << " changed state to = " << life_cycle_mediator_.stateToString(state) << " [38S8]"); nr.state = state; nodes_changed = true; } if (nr.status != status) { - ROS_INFO_STREAM(node_name << " changed status to = " << life_cycle_mediator_.statusToString(status) << " [09SI]"); + RCLCPP_INFO_STREAM(nh_->get_logger(), node_name << " changed status to = " << life_cycle_mediator_.statusToString(status) << " [09SI]"); nr.status = status; nodes_changed = true; if(nr.manifested && nr.status == LifeCycleStatus::ERROR) { supervisor_.status_error = true; - ROS_ERROR_STREAM("Manifested node " << nr.name << " changed status to ERROR. Shutting down nodes... [JHRE]"); + RCLCPP_INFO_STREAM(nh_->get_logger(), "Manifested node " << nr.name << " changed status to ERROR. Shutting down nodes... [JHRE]"); stopFlightPlan(); } } @@ -343,11 +353,11 @@ class AMSuper : AMLifeCycle //process id = 0 observed to be a node coming online. -1 appears to be offline if(pid == 0) { - ROS_INFO_STREAM(node_name << " process is alive [UIRE]"); + RCLCPP_INFO_STREAM(nh_->get_logger(), node_name << " process is alive [UIRE]"); } else { - ROS_WARN_STREAM(node_name << " changed process id from: " << nr.pid << " to: " << pid << " [WNEW]"); + RCLCPP_INFO_STREAM(nh_->get_logger(), node_name << " changed process id from: " << nr.pid << " to: " << pid << " [WNEW]"); } nr.pid = pid; nodes_changed = true; @@ -357,7 +367,7 @@ class AMSuper : AMLifeCycle else { // if we get here, the node is not in the manifest and we've never heard from it before - ROS_WARN_STREAM("unknown node " << node_name << " came online. state: " << life_cycle_mediator_.stateToString(state) + RCLCPP_WARN_STREAM(nh_->get_logger(), "unknown node " << node_name << " came online. state: " << life_cycle_mediator_.stateToString(state) << ", status: " << life_cycle_mediator_.statusToString(status)); SuperNodeMediator::SuperNodeInfo nr; nr.name = node_name; @@ -393,7 +403,7 @@ class AMSuper : AMLifeCycle } if (flt_ctrl_state_changed) { - ROS_INFO_STREAM_THROTTLE(1.0, "flight status: " << value); + RCLCPP_INFO_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 1.0, "flight status: " << value); checkForSystemStateTransition(); } } @@ -404,7 +414,7 @@ class AMSuper : AMLifeCycle * * times out nodes that haven't been heard from recently. reports on status to bag and trace logs. */ - void heartbeatCB(const ros::TimerEvent& event) override + void heartbeatCB() override { #if CUDA_FLAG gpu_info_->display(); @@ -412,33 +422,33 @@ class AMSuper : AMLifeCycle //publish deprecated topic { - brain_box_msgs::VxState state_msg; + brain_box_msgs::msg::VxState state_msg; state_msg.state = (uint8_t)supervisor_.system_state; - vstate_summary_pub_.publish(state_msg); + vstate_summary_pub_->publish(state_msg); } //publish the system state { - brain_box_msgs::SystemState system_state_msg; + brain_box_msgs::msg::SystemState system_state_msg; system_state_msg.state = (uint8_t)supervisor_.system_state; system_state_msg.state_string = state_mediator_.stateToString(supervisor_.system_state); - system_state_pub_.publish(system_state_msg); + system_state_pub_->publish(system_state_msg); } // cycle thru all the nodes in the list to look for a timeout - ros::Time now = ros::Time().now(); + rclcpp::Time now = nh_->now(); map::iterator it; for (it = supervisor_.nodes.begin(); it != supervisor_.nodes.end(); it++) { SuperNodeMediator::SuperNodeInfo& nr = (*it).second; if (nr.online) { - ros::Duration time_since_contact = now - nr.last_contact; - ros::Duration timeout_dur(node_timeout_s_); + rclcpp::Duration time_since_contact = (now - nr.last_contact); + rclcpp::Duration timeout_dur(node_timeout_s_); if (time_since_contact > timeout_dur) { nr.online = false; - ROS_ERROR_STREAM("node timed out:" << nr.name); + RCLCPP_ERROR_STREAM(nh_->get_logger(),"node timed out:" << nr.name); reportSystemState(); } } @@ -449,7 +459,7 @@ class AMSuper : AMLifeCycle int num_manifest_nodes_online = node_mediator_.manifestedNodesOnlineCount(supervisor_); // publish and bag log super status message - brain_box_msgs::Super2Status status_msg; + brain_box_msgs::msg::Super2Status status_msg; status_msg.man = supervisor_.manifest.size(); status_msg.man_run = num_manifest_nodes_online; status_msg.run = node_mediator_.nodesOnlineCount(supervisor_); @@ -460,9 +470,9 @@ class AMSuper : AMLifeCycle status_msg.nodes.push_back(nr.name); } LOG_MSG("/status/super", status_msg, 1); - if (super_status_pub_.getNumSubscribers() > 0) + if (super_status_pub_->get_subscription_count() > 0) { - super_status_pub_.publish(status_msg); + super_status_pub_->publish(status_msg); } // report current status to trace log @@ -472,13 +482,13 @@ class AMSuper : AMLifeCycle if (supervisor_.manifest.size() != num_manifest_nodes_online) { // if all manifested nodes aren't running, report as error - ROS_ERROR_STREAM(ss.str()); - ROS_ERROR_STREAM("not online: " << node_mediator_.manifestedNodesNotOnlineNamesList(supervisor_)); + RCLCPP_ERROR_STREAM(nh_->get_logger(),ss.str()); + RCLCPP_ERROR_STREAM(nh_->get_logger(),"not online: " << node_mediator_.manifestedNodesNotOnlineNamesList(supervisor_)); } else { // if all manifested nodes are running, report as info - ROS_INFO_STREAM_THROTTLE(LOG_THROTTLE_S, ss.str()); + RCLCPP_INFO_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), LOG_THROTTLE_S, ss.str()); } // log stats @@ -488,13 +498,13 @@ class AMSuper : AMLifeCycle { //checking whether the file is open string tp; getline(newfile, tp); - std_msgs::Int16 msg; + std_msgs::msg::Int16 msg; msg.data = std::stoi(tp); LOG_MSG("/watts", msg, SU_LOG_LEVEL); newfile.close(); //close the file object. } - AMLifeCycle::heartbeatCB(event); + AMLifeCycle::heartbeatCB(); } /** @@ -516,7 +526,7 @@ class AMSuper : AMLifeCycle { std::stringstream ss; genSystemState(ss); - ROS_INFO_STREAM_THROTTLE(LOG_THROTTLE_S, ss.str()); + RCLCPP_INFO_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), LOG_THROTTLE_S, ss.str()); } /** @@ -528,11 +538,11 @@ class AMSuper : AMLifeCycle */ void sendLifeCycleCommand(const std::string_view& node_name, const LifeCycleCommand command) { - ROS_DEBUG_STREAM("sending command: " << life_cycle_mediator_.commandToString(command) << " to " << node_name << " lifecycle"); - brain_box_msgs::LifeCycleCommand msg; + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "sending command: " << life_cycle_mediator_.commandToString(command) << " to " << node_name << " lifecycle"); + brain_box_msgs::msg::LifeCycleCommand msg; msg.node_name = node_name; - msg.command = (brain_box_msgs::LifeCycleCommand::_command_type)command; - lifecycle_pub_.publish(msg); + msg.command = (brain_box_msgs::msg::LifeCycleCommand::_command_type)command; + lifecycle_pub_->publish(msg); } /** @@ -554,7 +564,7 @@ class AMSuper : AMLifeCycle { for (const auto & [ node_name, error_message ] : result.second) { - ROS_WARN_STREAM(error_message); + RCLCPP_WARN_STREAM(nh_->get_logger(),error_message); } } return success; @@ -564,10 +574,10 @@ class AMSuper : AMLifeCycle /** Send signal to flight controller that flight is over. */ void stopFlightPlan() { - std_msgs::Bool msg; + std_msgs::msg::Bool msg; msg.data = false; //false means deactivate - flight_plan_deactivation_pub_.publish(msg); - ROS_ERROR_STREAM("Sending flight plan kill command."); + flight_plan_deactivation_pub_->publish(msg); + RCLCPP_ERROR_STREAM(nh_->get_logger(), "Sending flight plan kill command."); } /** @@ -617,7 +627,7 @@ class AMSuper : AMLifeCycle */ void setSystemState(SuperState state) { - ROS_INFO_STREAM(state_mediator_.stateToString(supervisor_.system_state) << " --> " + RCLCPP_INFO_STREAM(nh_->get_logger(),state_mediator_.stateToString(supervisor_.system_state) << " --> " << state_mediator_.stateToString(state)); bool legal = true; if(!node_mediator_.forceTransition(state)) @@ -625,7 +635,7 @@ class AMSuper : AMLifeCycle if (!legal) { - ROS_ERROR_STREAM("illegal state transition from " << state_mediator_.stateToString(supervisor_.system_state) + RCLCPP_ERROR_STREAM(nh_->get_logger(),"illegal state transition from " << state_mediator_.stateToString(supervisor_.system_state) << " to " << state_mediator_.stateToString(state)); } else @@ -638,9 +648,9 @@ class AMSuper : AMLifeCycle sendLEDMessage(); - brain_box_msgs::VxState state_msg; + brain_box_msgs::msg::VxState state_msg; state_msg.state = (uint8_t)supervisor_.system_state; - vstate_summary_pub_.publish(state_msg); + vstate_summary_pub_->publish(state_msg); } } @@ -654,8 +664,8 @@ class AMSuper : AMLifeCycle SuperNodeMediator::PlatformVariant required_platform; SuperNodeMediator::PlatformVariant actual_platform; configurePlatformRequirements(required_platform,actual_platform); - ROS_WARN_STREAM("required" << required_platform.maker); - ROS_WARN_STREAM("actual" << actual_platform.maker); + RCLCPP_WARN_STREAM(nh_->get_logger(),"required" << required_platform.maker); + RCLCPP_WARN_STREAM(nh_->get_logger(),"actual" << actual_platform.maker); if(!node_mediator_.isCorrectPlatform(required_platform,actual_platform)) { std::stringstream message; @@ -703,7 +713,7 @@ class AMSuper : AMLifeCycle } else { - ROS_WARN("platform requirements not set"); + RCLCPP_WARN(nh_->get_logger(),"platform requirements not set"); } } @@ -712,12 +722,12 @@ class AMSuper : AMLifeCycle */ void sendLEDMessage(int r, int g, int b, float period = 0.0) { - brain_box_msgs::BlinkMCommand led_msg; + brain_box_msgs::msg::BlinkMCommand led_msg; led_msg.rgb.red = r; led_msg.rgb.green = g; led_msg.rgb.blue = b; led_msg.blink_rate = period; - led_pub_.publish(led_msg); + led_pub_->publish(led_msg); } static constexpr double LED_SOLID = 0.0; @@ -818,12 +828,12 @@ 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); } - 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); } @@ -850,14 +860,14 @@ class AMSuper : AMLifeCycle } } - void logControlCB(const brain_box_msgs::LogControl::ConstPtr &msg) + void logControlCB(const brain_box_msgs::msg::LogControl::SharedPtr msg) { if (msg->enable) { - ROS_INFO_STREAM("stop logging"); + RCLCPP_INFO_STREAM(nh_->get_logger(),"stop logging"); BagLogger::instance()->stopLogging(); - ROS_INFO_STREAM("start logging to SU, level " << SU_LOG_LEVEL); + RCLCPP_INFO_STREAM(nh_->get_logger(),"start logging to SU, level " << SU_LOG_LEVEL); BagLogger::instance()->startLogging("SU", SU_LOG_LEVEL); log_level_ = intToLoggerLevel (SU_LOG_LEVEL); } @@ -870,12 +880,18 @@ class AMSuper : AMLifeCycle #else int main(int argc, char** argv) { - ros::init(argc, argv, ros::this_node::getName()); + rclcpp::init(argc, argv); - am::AMSuper node; + std::shared_ptr node = std::make_shared("am_super"); - ROS_INFO_STREAM(ros::this_node::getName() << ": running..."); + std::shared_ptr am_super_node = std::make_shared(node); - ros::spin(); + RCLCPP_INFO_STREAM(node->get_logger(), node->get_name() << ": running..."); + + rclcpp::spin(node); + + rclcpp::shutdown(); + + return 0; } #endif diff --git a/src/super_lib/am_life_cycle.cpp b/src/super_lib/am_life_cycle.cpp index 9b157b9f..036d68cb 100644 --- a/src/super_lib/am_life_cycle.cpp +++ b/src/super_lib/am_life_cycle.cpp @@ -1,5 +1,5 @@ #include -#include +#include #include #include @@ -13,7 +13,7 @@ namespace am // static constexpr std::string AMLifeCycle::STATE_INVALID_STRING; // static constexpr std::string AMLifeCycle::STATE_UNCONFIGURED_STRING; -AMLifeCycle::AMLifeCycle() : nh_("~") +AMLifeCycle::AMLifeCycle(rclcpp::Node::SharedPtr node) : node_(node) { std::string init_state_str; //FIXME: This string should come from the enum @@ -40,7 +40,7 @@ AMLifeCycle::AMLifeCycle() : nh_("~") life_cycle_info_.state = LifeCycleState::ACTIVE; } life_cycle_info_.status = LifeCycleStatus::OK; - state_pub_ = nh_.advertise("/node_state", 100); + state_pub_ = node_->create_publisher("/node_state", 100); updater_.setHardwareID("none"); updater_.broadcast(0, "Initializing node"); @@ -49,13 +49,13 @@ AMLifeCycle::AMLifeCycle() : nh_("~") // strip leading '/' if it is there // TODO: this might always be there so just strip it without checking? - if (ros::this_node::getName().at(0) == '/') + if (std::string(node_->get_name()).at(0) == '/') { - node_name_ = ros::this_node::getName().substr(1); + node_name_ = std::string(node_->get_name()).substr(1); } else { - node_name_ = ros::this_node::getName(); + node_name_ = node_->get_name(); } @@ -82,9 +82,9 @@ bool AMLifeCycle::param(const std::string& param_name, T& param_val, const T& de return result; } -void AMLifeCycle::lifecycleCB(const brain_box_msgs::LifeCycleCommand::ConstPtr msg) +void AMLifeCycle::lifecycleCB(const brain_box_msgs::msg::LifeCycleCommand::SharedPtr msg) { - ROS_DEBUG_STREAM_THROTTLE(1.0, life_cycle_mediator_.commandToString((LifeCycleCommand)msg->command)); + RCLCPP_DEBUG_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 1.0, life_cycle_mediator_.commandToString((LifeCycleCommand)msg->command)); if (!msg->node_name.compare(AMLifeCycle::BROADCAST_NODE_NAME) || !msg->node_name.compare(node_name_)) { @@ -124,7 +124,7 @@ void AMLifeCycle::transition(std::string transition_name, LifeCycleState initial { if (life_cycle_info_.state == initial_state) { - ROS_INFO_STREAM(transition_name << ", current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [ASWU]"); + RCLCPP_INFO_STREAM(nh_->get_logger(), transition_name << ", current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [ASWU]"); setState(transition_state); on_function(); } @@ -462,18 +462,18 @@ AMStatReset& AMLifeCycle::configureHzStats(AMStatReset& stats) void AMLifeCycle::sendNodeUpdate() { - brain_box_msgs::LifeCycleState msg; - msg.node_name = ros::this_node::getName(); + brain_box_msgs::msg::LifeCycleState msg; + msg.node_name = node_->get_name(); msg.process_id = 0; msg.state = (uint8_t)life_cycle_info_.state; msg.status = (uint8_t)life_cycle_info_.status; msg.subsystem = ""; msg.value = ""; - state_pub_.publish(msg); + state_pub_->publish(msg); } -void AMLifeCycle::heartbeatCB(const ros::TimerEvent& event) +void AMLifeCycle::heartbeatCB() { updater_.force_update(); From 3190e1de01af9146e9e3ddae4661bd017dad6930 Mon Sep 17 00:00:00 2001 From: AJ Date: Tue, 20 Dec 2022 15:08:44 -0800 Subject: [PATCH 02/45] feat: super_lib is complete --- include/super_lib/am_stat.h | 24 +++++----- include/super_lib/am_stat_reset.h | 10 ++--- src/super_lib/am_life_cycle.cpp | 74 +++++++++++++++++-------------- 3 files changed, 58 insertions(+), 50 deletions(-) diff --git a/include/super_lib/am_stat.h b/include/super_lib/am_stat.h index 9868623a..1260f81d 100644 --- a/include/super_lib/am_stat.h +++ b/include/super_lib/am_stat.h @@ -38,26 +38,26 @@ class AMStat bool validate_max_ = false; bool sample_received_ = false; bool sample_required_ = false; + rclcpp::Node::SharedPtr node_; private: AMStat(); - public: - AMStat(const std::string& short_name, const std::string& long_name) + AMStat(rclcpp::Node::SharedPtr node, const std::string& short_name, const std::string& long_name): node_(node) { short_name_ = short_name; long_name_ = long_name; } - AMStat(const std::string& short_name, const std::string& long_name, uint32_t max_warn, uint32_t max_error) - : AMStat(short_name, long_name) + AMStat(rclcpp::Node::SharedPtr node, const std::string& short_name, const std::string& long_name, uint32_t max_warn, uint32_t max_error) + : AMStat(node, short_name, long_name) { setMaxWarn(max_warn); setMaxError(max_error); } - AMStat(const std::string& short_name, const std::string& long_name, uint32_t min_error, uint32_t min_warn, + AMStat(rclcpp::Node::SharedPtr node, const std::string& short_name, const std::string& long_name, uint32_t min_error, uint32_t min_warn, uint32_t max_warn, uint32_t max_error) - : AMStat(short_name, long_name,max_warn,max_error) + : AMStat(node, short_name, long_name,max_warn,max_error) { setMinError(min_error); setMinWarn(min_warn); @@ -77,13 +77,13 @@ class AMStat { if (value_ > max_error_) { - ROS_ERROR_STREAM_THROTTLE(error_throttle_s, long_name_ << " exceeding max_error: " << value_ + RCLCPP_ERROR_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(),error_throttle_s, long_name_ << " exceeding max_error: " << value_ << " (max:" << max_error_ << ") [TF5R]"); compoundStatus(status, LifeCycleStatus::ERROR); } else if (value_ > max_warn_) { - ROS_WARN_STREAM_THROTTLE(warn_throttle_s, long_name_ << " exceeding max_warn: " << value_ << " (max:" << max_warn_ + RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), warn_throttle_s, long_name_ << " exceeding max_warn: " << value_ << " (max:" << max_warn_ << ") [PO9P]"); compoundStatus(status, LifeCycleStatus::WARN); } @@ -93,13 +93,13 @@ class AMStat { if (value_ < min_error_) { - ROS_ERROR_STREAM_THROTTLE(error_throttle_s, long_name_ << " exceeding min_error: " << value_ + RCLCPP_ERROR_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), error_throttle_s, long_name_ << " exceeding min_error: " << value_ << " (min:" << min_error_ << ") [K08K]"); compoundStatus(status, LifeCycleStatus::ERROR); } else if (value_ < min_warn_) { - ROS_WARN_STREAM_THROTTLE(warn_throttle_s, long_name_ << " exceeding min_warn: " << value_ << " (min:" << min_warn_ + RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), warn_throttle_s, long_name_ << " exceeding min_warn: " << value_ << " (min:" << min_warn_ << ") [H9H8]"); compoundStatus(status, LifeCycleStatus::WARN); } @@ -108,14 +108,14 @@ class AMStat if(!validate_max_ && !validate_min_) { //report this warning once during first validation - ROS_WARN_STREAM_THROTTLE(9999, long_name_ << " lacks validation since min/max is not set [TRB5]"); + RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 9999, long_name_ << " lacks validation since min/max is not set [TRB5]"); } } else { //sample is required and not yet received status = LifeCycleStatus::ERROR; - ROS_ERROR_STREAM_THROTTLE(error_throttle_s, long_name_ << " no samples received [NAQE] "); + RCLCPP_ERROR_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), error_throttle_s, long_name_ << " no samples received [NAQE] "); } return status; } diff --git a/include/super_lib/am_stat_reset.h b/include/super_lib/am_stat_reset.h index 08097cd6..dea8827e 100644 --- a/include/super_lib/am_stat_reset.h +++ b/include/super_lib/am_stat_reset.h @@ -22,20 +22,20 @@ class AMStatReset : public AMStat sample_required_ = true; } public: - AMStatReset(const std::string& short_name, const std::string& long_name) : AMStat(short_name, long_name) + AMStatReset(rclcpp::Node::SharedPtr node, const std::string& short_name, const std::string& long_name) : AMStat(node, short_name, long_name) { init(); } - AMStatReset(const std::string& short_name, const std::string& long_name, uint32_t max_warn, uint32_t max_error) - : AMStat(short_name, long_name, max_warn, max_error) + AMStatReset(rclcpp::Node::SharedPtr node, const std::string& short_name, const std::string& long_name, uint32_t max_warn, uint32_t max_error) + : AMStat(node, short_name, long_name, max_warn, max_error) { init(); } - AMStatReset(const std::string& short_name, const std::string& long_name, uint32_t min_error, uint32_t min_warn, + AMStatReset(rclcpp::Node::SharedPtr node, const std::string& short_name, const std::string& long_name, uint32_t min_error, uint32_t min_warn, uint32_t max_warn, uint32_t max_error) - : AMStat(short_name, long_name,min_error,min_warn, max_warn, max_error) + : AMStat(node, short_name, long_name,min_error,min_warn, max_warn, max_error) { init(); } diff --git a/src/super_lib/am_life_cycle.cpp b/src/super_lib/am_life_cycle.cpp index 036d68cb..f60de95a 100644 --- a/src/super_lib/am_life_cycle.cpp +++ b/src/super_lib/am_life_cycle.cpp @@ -2,9 +2,7 @@ #include #include #include - - - +#include namespace am { @@ -13,7 +11,7 @@ namespace am // static constexpr std::string AMLifeCycle::STATE_INVALID_STRING; // static constexpr std::string AMLifeCycle::STATE_UNCONFIGURED_STRING; -AMLifeCycle::AMLifeCycle(rclcpp::Node::SharedPtr node) : node_(node) +AMLifeCycle::AMLifeCycle(rclcpp::Node::SharedPtr node) : node_(node), updater_(node) { std::string init_state_str; //FIXME: This string should come from the enum @@ -64,9 +62,10 @@ AMLifeCycle::AMLifeCycle(rclcpp::Node::SharedPtr node) : node_(node) /** * node status via LifeCycle */ - lifecycle_sub_ = nh_.subscribe("/node_lifecycle", 100, &AMLifeCycle::lifecycleCB, this); + lifecycle_sub_ = node_->create_subscription("/node_lifecycle", 100, + std::bind(&AMLifeCycle::lifecycleCB, this, std::placeholders::_1)); - heartbeat_timer_ = nh_.createTimer(ros::Duration(1.0), &AMLifeCycle::heartbeatCB, this); + heartbeat_timer_ = node_->create_wall_timer(am::toDuration(1.0), std::bind(&AMLifeCycle::heartbeatCB, this)); } @@ -77,14 +76,16 @@ AMLifeCycle::~AMLifeCycle() template bool AMLifeCycle::param(const std::string& param_name, T& param_val, const T& default_val) const { - bool result = nh_.param(param_name, param_val, default_val); - ROS_INFO_STREAM(param_name << " = " << param_val); + //todo: fix the parameter + //bool result = nh_.param(param_name, param_val, default_val); + bool result = true; + RCLCPP_INFO_STREAM(node_->get_logger(), param_name << " = " << param_val); return result; } void AMLifeCycle::lifecycleCB(const brain_box_msgs::msg::LifeCycleCommand::SharedPtr msg) { - RCLCPP_DEBUG_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 1.0, life_cycle_mediator_.commandToString((LifeCycleCommand)msg->command)); + RCLCPP_DEBUG_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 1.0, life_cycle_mediator_.commandToString((LifeCycleCommand)msg->command)); if (!msg->node_name.compare(AMLifeCycle::BROADCAST_NODE_NAME) || !msg->node_name.compare(node_name_)) { @@ -103,7 +104,7 @@ void AMLifeCycle::lifecycleCB(const brain_box_msgs::msg::LifeCycleCommand::Share configure(); break; case LifeCycleCommand::CREATE: - ROS_WARN_STREAM("illegal command " << life_cycle_mediator_.commandToString(LifeCycleCommand::CREATE) << " [7YT8]"); + RCLCPP_WARN_STREAM(node_->get_logger(),"illegal command " << life_cycle_mediator_.commandToString(LifeCycleCommand::CREATE) << " [7YT8]"); break; case LifeCycleCommand::DEACTIVATE: transition("deactivate", LifeCycleState::ACTIVE, LifeCycleState::DEACTIVATING, LifeCycleState::INACTIVE, @@ -124,17 +125,17 @@ void AMLifeCycle::transition(std::string transition_name, LifeCycleState initial { if (life_cycle_info_.state == initial_state) { - RCLCPP_INFO_STREAM(nh_->get_logger(), transition_name << ", current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [ASWU]"); + RCLCPP_INFO_STREAM(node_->get_logger(), transition_name << ", current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [ASWU]"); setState(transition_state); on_function(); } else if (life_cycle_info_.state == transition_state || life_cycle_info_.state == final_state) { - ROS_DEBUG_STREAM("ignoring redundant " << transition_name << " [0393]"); + RCLCPP_DEBUG_STREAM(node_->get_logger(),"ignoring redundant " << transition_name << " [0393]"); } else { - ROS_WARN_STREAM("received illegal " << transition_name << " in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [JGV5]"); + RCLCPP_WARN_STREAM(node_->get_logger(),"received illegal " << transition_name << " in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [JGV5]"); } } @@ -144,12 +145,12 @@ void AMLifeCycle::doTransition(std::string transition_name, bool success, LifeCy logState(); if (success) { - ROS_INFO_STREAM(transition_name << " succeeded"); + RCLCPP_INFO_STREAM(node_->get_logger(), transition_name << " succeeded"); setState(success_state); } else { - ROS_INFO_STREAM(transition_name << " failed"); + RCLCPP_INFO_STREAM(node_->get_logger(),transition_name << " failed"); setState(failure_state); } } @@ -180,7 +181,7 @@ void AMLifeCycle::doCleanup(bool success) void AMLifeCycle::onConfigure() { - ROS_INFO("onConfigure called [POMH]"); + RCLCPP_INFO(node_->get_logger(),"onConfigure called [POMH]"); if(stats_list_.hasStats()) { LifeCycleStatus status = stats_list_.process(throttle_info_.warn_throttle_s, throttle_info_.error_throttle_s); @@ -190,7 +191,7 @@ void AMLifeCycle::onConfigure() } else if (!withinConfigureTolerance()) { - ROS_WARN_STREAM_THROTTLE(5, stats_list_.getStatsStr() << " blocked by stats past configure tolerance with status " << life_cycle_mediator_.statusToString(status) ); + RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(),5, stats_list_.getStatsStr() << " blocked by stats past configure tolerance with status " << life_cycle_mediator_.statusToString(status) ); } } //if there are no stats and request to configure, then configure @@ -214,7 +215,7 @@ void AMLifeCycle::onDeactivate() void AMLifeCycle::logState() { - ROS_INFO_STREAM("LifeCycle: " << life_cycle_mediator_.stateToString(life_cycle_info_.state)); + RCLCPP_INFO_STREAM(node_->get_logger(),"LifeCycle: " << life_cycle_mediator_.stateToString(life_cycle_info_.state)); } void AMLifeCycle::doDeactivate(bool success) @@ -227,7 +228,7 @@ void AMLifeCycle::configure() //mark the configuration start time once if(getState() != LifeCycleState::CONFIGURING) { - configure_start_time_=ros::Time().now(); + configure_start_time_=node_->now(); } transition("configure", LifeCycleState::UNCONFIGURED, LifeCycleState::CONFIGURING, LifeCycleState::INACTIVE, std::bind(&AMLifeCycle::onConfigure, this)); @@ -237,12 +238,12 @@ void AMLifeCycle::destroy() { if (life_cycle_mediator_.illegalDestroy(life_cycle_info_)) { - ROS_INFO_STREAM("received illegal activate in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [45RT]"); + RCLCPP_INFO_STREAM(node_->get_logger(),"received illegal activate in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [45RT]"); } /* This condition is hit only if state equals FINALIZED. Checking SHUTTING_DOWN is redundant */ else { - ROS_INFO_STREAM("current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [RE45]"); + RCLCPP_INFO_STREAM(node_->get_logger(),"current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [RE45]"); onDestroy(); } } @@ -265,8 +266,8 @@ bool AMLifeCycle::withinConfigureTolerance() //outside of configuring, we have no tolerance if(life_cycle_mediator_.unconfigured(life_cycle_info_)) { - ros::Duration duration_since_configure = ros::Time::now() - configure_start_time_; - if (life_cycle_info_.state == LifeCycleState::UNCONFIGURED || duration_since_configure <= ros::Duration(configure_tolerance_s) ) + rclcpp::Duration duration_since_configure = node_->now() - configure_start_time_; + if (life_cycle_info_.state == LifeCycleState::UNCONFIGURED || duration_since_configure <= rclcpp::Duration(am::toDuration(configure_tolerance_s)) ) { tolerated = true; } @@ -284,7 +285,7 @@ void AMLifeCycle::error(std::string message, std::string error_code, bool forced std::string error_code_message = "Error[" + error_code + "] "; if(withinConfigureTolerance() && !forced) { - ROS_WARN_STREAM_THROTTLE(throttle_info_.warn_throttle_s,"Ignoring tolerant error for (" << configure_tolerance_s << "s) `" << message << "` " << error_code_message << "[GFRT]"); + RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(),throttle_info_.warn_throttle_s,"Ignoring tolerant error for (" << configure_tolerance_s << "s) `" << message << "` " << error_code_message << "[GFRT]"); } else { @@ -302,7 +303,7 @@ void AMLifeCycle::error(std::string message, std::string error_code, bool forced repeat_prefix = "Repeated "; } std::string error_explanation=forced_prefix + repeat_prefix + error_code_message; - ROS_ERROR_STREAM(message << " -> " << error_explanation << " [R45Y]" ); + RCLCPP_ERROR_STREAM(node_->get_logger(), message << " -> " << error_explanation << " [R45Y]" ); } } @@ -341,17 +342,17 @@ void AMLifeCycle::shutdown() { if (life_cycle_mediator_.shutdown(life_cycle_info_)) { - ROS_INFO_STREAM("current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state)); + RCLCPP_INFO_STREAM(node_->get_logger(),"current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state)); setState(LifeCycleState::SHUTTING_DOWN); onShutdown(); } else if (life_cycle_mediator_.redundantShutdown(life_cycle_info_)) { - ROS_DEBUG_STREAM("ignoring redundant shutdown"); + RCLCPP_DEBUG_STREAM(node_->get_logger(),"ignoring redundant shutdown"); } else { - ROS_INFO_STREAM("received illegal activate in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state)); + RCLCPP_INFO_STREAM(node_->get_logger(),"received illegal activate in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state)); } } @@ -453,11 +454,16 @@ void AMLifeCycle::configureStat(AMStat& stat) AMStatReset& AMLifeCycle::configureHzStat(AMStatReset& stat) { configureStat(stat, stat.getLongName(), "hz"); + + return stat; + } AMStatReset& AMLifeCycle::configureHzStats(AMStatReset& stats) { configureStat(stats, stats.getShortName(), "hz"); + + return stats; } void AMLifeCycle::sendNodeUpdate() @@ -482,7 +488,7 @@ void AMLifeCycle::heartbeatCB() << stats_list_.getStatsStrShort(); double throttle_s = getThrottle(); - ROS_INFO_STREAM_THROTTLE(throttle_s, "LifeCycle heartbeat: " << ss.str()); + RCLCPP_INFO_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), throttle_s, "LifeCycle heartbeat: " << ss.str()); stats_list_.reset(); @@ -505,12 +511,12 @@ void AMLifeCycle::setState(const LifeCycleState state) if (life_cycle_mediator_.setState(state, life_cycle_info_)) { - ROS_INFO_STREAM("changing state from " << life_cycle_mediator_.stateToString(initial_state) << " to " << life_cycle_mediator_.stateToString(state)); + RCLCPP_INFO_STREAM(node_->get_logger(), "changing state from " << life_cycle_mediator_.stateToString(initial_state) << " to " << life_cycle_mediator_.stateToString(state)); sendNodeUpdate(); } else { - ROS_ERROR_STREAM("illegal state: " << (int)state); + RCLCPP_ERROR_STREAM(node_->get_logger(), "illegal state: " << (int)state); } } @@ -524,7 +530,7 @@ bool AMLifeCycle::setStatus(const LifeCycleStatus status) //if we are in error and want to leave it if(life_cycle_info_.status == LifeCycleStatus::ERROR && status != LifeCycleStatus::ERROR) { - ROS_WARN_STREAM_THROTTLE(getThrottle(), "requested to change status from ERROR to " << life_cycle_mediator_.statusToString(status) << " [DFRE]"); + RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), getThrottle(), "requested to change status from ERROR to " << life_cycle_mediator_.statusToString(status) << " [DFRE]"); } else if (life_cycle_mediator_.setStatus(status, life_cycle_info_)) { @@ -533,8 +539,10 @@ bool AMLifeCycle::setStatus(const LifeCycleStatus status) else { - ROS_ERROR_STREAM("illegal status: " << life_cycle_mediator_.statusToString(status)); + RCLCPP_ERROR_STREAM(node_->get_logger(), "illegal status: " << life_cycle_mediator_.statusToString(status)); } + + return true; } const std::string_view& AMLifeCycle::getStatusName() From b59b304339ae59970da8fdc628921b2ee9bf05e1 Mon Sep 17 00:00:00 2001 From: AJ Date: Tue, 20 Dec 2022 21:00:03 -0800 Subject: [PATCH 03/45] feat: am_super sofar --- CMakeLists.txt | 6 +++--- include/am_super/baby_sitter.h | 11 ++++++----- include/am_super/controller_state.h | 6 +++--- include/am_super/operator_command.h | 22 +++++++++++----------- include/am_super/super_node_mediator.h | 11 +++++++---- src/am_super/am_super.cpp | 6 +++--- src/am_super/super_node_mediator.cpp | 10 +++++----- src/am_super/super_state_mediator.cpp | 4 ++-- 8 files changed, 40 insertions(+), 36 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 351c5ec4..a14b7305 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -74,9 +74,9 @@ ament_export_include_directories(include) ament_export_libraries(super_lib) -#add_executable(am_super ${am_super_cpp_files}) -#target_link_libraries(am_super super_lib) -#ament_target_dependencies(am_super ${dependencies}) +add_executable(am_super ${am_super_cpp_files}) +target_link_libraries(am_super super_lib) +ament_target_dependencies(am_super ${dependencies}) # uncomment the following section in order to fill in diff --git a/include/am_super/baby_sitter.h b/include/am_super/baby_sitter.h index 78b70797..f007891e 100644 --- a/include/am_super/baby_sitter.h +++ b/include/am_super/baby_sitter.h @@ -102,19 +102,20 @@ BabySitter::BabySitter(const rclcpp::Node::SharedPtr nh, BagLogger* logger, c nh_ = nh; std::string parm = "~" + node_name + "/warn_ms"; - ros::param::param(parm, warn_ms_, warn_ms); + + am::getParam(nh_, parm, warn_ms_, warn_ms); RCLCPP_INFO_STREAM(nh_->get_logger(), nh_->get_name() << parm << " = " << warn_ms_); parm = "~" + node_name + "/error_ms"; - ros::param::param(parm, error_ms_, error_ms); + am::getParam(nh_,parm, error_ms_, error_ms); RCLCPP_INFO_STREAM(nh_->get_logger(), nh_->get_name() << parm << " = " << error_ms_); parm = "~" + node_name + "/warn_count_thresh"; - ros::param::param(parm, warn_count_thresh_, warn_count_thresh); + am::getParam(nh_,parm, warn_count_thresh_, warn_count_thresh); RCLCPP_INFO_STREAM(nh_->get_logger(), nh_->get_name() << parm << " = " << warn_count_thresh_); parm = "~" + node_name + "/timeout_ms"; - ros::param::param(parm, timeout_ms_, timeout_ms); + am::getParam(nh_,parm, timeout_ms_, timeout_ms); RCLCPP_INFO_STREAM(nh_->get_logger(), nh_->get_name() << parm << " = " << timeout_ms_); min_ms_ = 1000; @@ -358,7 +359,7 @@ void BabySitter::heartbeatCB() int time_since_contact = nowMS() - last_contact_ms_; if (time_since_contact > timeout_ms_) { - RCLCPP_ERROR_STREAM(nh_->get_logger(), NODE_FUNC << node_name_ << ": timed out"); + RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() << node_name_ << ": timed out"); device_state_ = DeviceState::ERROR; checkNodeState(); } 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..17754d8f 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 @@ -21,7 +21,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 +44,7 @@ class SuperNodeMediator LifeCycleStatus status; // node lifecycle status bool manifested; // nodes was in manfiest bool online; // node is online - ros::Time last_contact; // last time a message was received from the node + rclcpp::Time last_contact; // last time a message was received from the node }; /** @@ -331,6 +331,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/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index 8270f6bf..ad3be5ab 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -129,7 +129,7 @@ class AMSuper : AMLifeCycle { RCLCPP_INFO_STREAM(nh_->get_logger(), nh_->get_name()); - ros::param::param("~node_timeout_s", node_timeout_s_, 2.0); + am::getParam(nh_, "node_timeout_s", node_timeout_s_, 2.0); RCLCPP_INFO_STREAM(nh_->get_logger(), "node_timeout_s = " << node_timeout_s_); /* @@ -138,7 +138,7 @@ class AMSuper : AMLifeCycle supervisor_.system_state = SuperState::OFF; // strip spaces from manifest param string manifest_param; - ros::param::param("~manifest", manifest_param, ""); + am::getParam(nh_, "manifest", manifest_param, ""); node_mediator_.parseManifest(supervisor_, manifest_param); @@ -606,7 +606,7 @@ class AMSuper : AMLifeCycle LifeCycleCommand command = transition_instructions.life_cycle_command; std::string failed_nodes_string = boost::algorithm::join(transition_instructions.failed_nodes, ", "); std::string failed_nodes_reasons_string = boost::algorithm::join(transition_instructions.failed_nodes_reasons, ", "); - ROS_INFO_STREAM_THROTTLE(5,state_mediator_.stateToString(supervisor_.system_state) + RCLCPP_INFO_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 5,state_mediator_.stateToString(supervisor_.system_state) << ": sending " << life_cycle_mediator_.commandToString(command) << " to " << failed_nodes_string << " because " << failed_nodes_reasons_string); diff --git a/src/am_super/super_node_mediator.cpp b/src/am_super/super_node_mediator.cpp index 3481091e..4a854874 100644 --- a/src/am_super/super_node_mediator.cpp +++ b/src/am_super/super_node_mediator.cpp @@ -9,8 +9,8 @@ namespace am /** * The state of the system as the supervisor sees it.*/ -SuperNodeMediator::SuperNodeMediator(const std::string& node_name): - SUPER_NODE_NAME(node_name), +SuperNodeMediator::SuperNodeMediator(rclcpp::Node::SharedPtr node, const std::string& node_name): + SUPER_NODE_NAME(node_name), node_(node), state_transitions_({ { SuperState::BOOTING, { {SuperState::READY, {SuperState::READY, SuperNodeMediator::checkReadyToArm, LifeCycleCommand::CONFIGURE}}}}, @@ -81,7 +81,7 @@ SuperNodeMediator::SuperNodeInfo SuperNodeMediator::initializeManifestedNode(std nr.name = node_name; nr.pid = -1; nr.online = false; - nr.last_contact = ros::Time(0); + nr.last_contact = node_->now(); nr.manifested = true; nr.state = LifeCycleState::UNCONFIGURED; nr.status = LifeCycleStatus::OK; @@ -274,7 +274,7 @@ pair> SuperNodeMediator::allManifestedNodesCheck( failed_nodes.insert(pair(node.name, error_message)); } } // for each node - return pair(success, failed_nodes); + return std::pair(success, failed_nodes); } void SuperNodeMediator::parseManifest(Supervisor& supervisor, string manifest) @@ -405,4 +405,4 @@ bool SuperNodeMediator::isCorrectPlatform(const SuperNodeMediator::PlatformVaria return pass; } -} \ No newline at end of file +} diff --git a/src/am_super/super_state_mediator.cpp b/src/am_super/super_state_mediator.cpp index 7af11809..ba1f40ab 100644 --- a/src/am_super/super_state_mediator.cpp +++ b/src/am_super/super_state_mediator.cpp @@ -1,5 +1,5 @@ #include - +#include namespace am { /**Local data class providing capabilities for states. @@ -86,4 +86,4 @@ std::string_view SuperStateMediator::stateToString(SuperState state) return info(state).name_; } -} \ No newline at end of file +} From 3976ff94abfed2307514d0d5263fd8d55e3baf10 Mon Sep 17 00:00:00 2001 From: AJ Date: Thu, 22 Dec 2022 14:20:22 -0800 Subject: [PATCH 04/45] feat: am_super compiles fine --- include/am_super/baby_sitter.h | 12 ++++++------ src/am_super/am_super.cpp | 17 +++++++++-------- 2 files changed, 15 insertions(+), 14 deletions(-) diff --git a/include/am_super/baby_sitter.h b/include/am_super/baby_sitter.h index f007891e..78577a40 100644 --- a/include/am_super/baby_sitter.h +++ b/include/am_super/baby_sitter.h @@ -13,7 +13,7 @@ namespace am { -template +template class BabySitter { private: @@ -47,7 +47,7 @@ class BabySitter std::string node_name_; rclcpp::Node::SharedPtr nh_; - rclcpp::Subscription::SharedPtr device_data_sub_; + rclcpp::GenericSubscription::SharedPtr device_data_sub_; rclcpp::Publisher::SharedPtr node_status_pub_; rclcpp::TimerBase::SharedPtr heartbeat_timer_; @@ -82,7 +82,7 @@ class BabySitter int getAveLatencyMs(); private: - void deviceCB(const M::SharedPtr event); + void deviceCB(std::shared_ptr msg); void heartbeatCB(); void checkNodeState(); void setNodeState(LifeCycleState node_state); @@ -140,7 +140,7 @@ BabySitter::BabySitter(const rclcpp::Node::SharedPtr nh, BagLogger* logger, c node_status_pub_ = nh_->create_publisher("/process/status", 1000); - device_data_sub_ = nh_->create_subscription(topic, 10, std::bind(&BabySitter::deviceCB, this, std::placeholders::_1)); + 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)); } @@ -263,7 +263,7 @@ void BabySitter::setNodeState(LifeCycleState node_state) } template -void BabySitter::deviceCB(const M::SharedPtr msg) +void BabySitter::deviceCB(std::shared_ptr msg) { message_count_++; long now_ms = nowMS(); @@ -352,7 +352,7 @@ void BabySitter::heartbeatCB() 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(log_msg, std::string("/status/super/" + node_name_), "brain_box_msgs/msg/BabySitterStatus", nh_->now(), 1); if (node_state_ == LifeCycleState::ACTIVE) { diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index ad3be5ab..9ab969a9 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -36,6 +36,7 @@ #include #include #include + #if CUDA_FLAG #include #endif @@ -125,7 +126,7 @@ class AMSuper : AMLifeCycle #endif public: - AMSuper(rclcpp::Node::SharedPtr nh) : nh_(nh), node_mediator_(SuperNodeMediator::nodeNameStripped(nh->get_name())) + AMSuper(rclcpp::Node::SharedPtr nh) : nh_(nh), AMLifeCycle(nh_), node_mediator_(nh_, SuperNodeMediator::nodeNameStripped(nh->get_name())) { RCLCPP_INFO_STREAM(nh_->get_logger(), nh_->get_name()); @@ -276,7 +277,7 @@ class AMSuper : AMLifeCycle 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(*rmsg, am_super_topics::LIFECYCLE_STATE, "brain_box_msgs/msg/LifeCycleState", rmsg->header.stamp, SU_LOG_LEVEL); } //void controllerStateCB(const ros::MessageEvent& event) @@ -297,7 +298,7 @@ class AMSuper : AMLifeCycle 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(*rmsg, "/operator/command", "brain_box_msgs/msg/OperatorCommand", nh_->now(), SU_LOG_LEVEL); } /** * process state @@ -444,7 +445,7 @@ class AMSuper : AMLifeCycle if (nr.online) { rclcpp::Duration time_since_contact = (now - nr.last_contact); - rclcpp::Duration timeout_dur(node_timeout_s_); + rclcpp::Duration timeout_dur(am::toDuration(node_timeout_s_)); if (time_since_contact > timeout_dur) { nr.online = false; @@ -469,7 +470,7 @@ class AMSuper : AMLifeCycle SuperNodeMediator::SuperNodeInfo& nr = (*it).second; status_msg.nodes.push_back(nr.name); } - LOG_MSG("/status/super", status_msg, 1); + LOG_MSG(status_msg, "/status/super", "brain_box_msgs/msg/Super2Status", nh_->now(), 1); if (super_status_pub_->get_subscription_count() > 0) { super_status_pub_->publish(status_msg); @@ -500,7 +501,7 @@ class AMSuper : AMLifeCycle getline(newfile, tp); std_msgs::msg::Int16 msg; msg.data = std::stoi(tp); - LOG_MSG("/watts", msg, SU_LOG_LEVEL); + LOG_MSG(msg, "/watts", "std_msgs/msg/Int16", nh_->now(), SU_LOG_LEVEL); newfile.close(); //close the file object. } @@ -830,12 +831,12 @@ class AMSuper : AMLifeCycle void diagnosticsCB(const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg) { - LOG_MSG("/diagnostics", msg, SU_LOG_LEVEL); + LOG_MSG(*msg, "/diagnostics", "diagnostic_msgs/msg/DiagnosticArray", nh_->now(), SU_LOG_LEVEL); } void currentENUCB(const nav_msgs::msg::Odometry::SharedPtr msg) { - LOG_MSG(am_topics::CTRL_VX_VEHICLE_CURRENTENU, msg, SU_LOG_LEVEL); + LOG_MSG(*msg, am_topics::CTRL_VX_VEHICLE_CURRENTENU, "nav_msgs/msg/Odometry", nh_->now(), SU_LOG_LEVEL); } BagLogger::BagLoggerLevel intToLoggerLevel(int level) From e14b79ffd5530d92ee4decf92b9f5c998faf474c Mon Sep 17 00:00:00 2001 From: AJ Date: Fri, 23 Dec 2022 08:32:11 -0800 Subject: [PATCH 05/45] feat: CMAKE policy --- CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a14b7305..ea9d7e14 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.5) project(am_super) - +if(POLICY CMP0074) + cmake_policy(SET CMP0074 NEW) +endif() # Default to C99 if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) From 00d8106ac72a22cf7b1e50881b1ee92650733104 Mon Sep 17 00:00:00 2001 From: danhennage Date: Tue, 3 Jan 2023 09:35:37 -0800 Subject: [PATCH 06/45] ported to ros2 --- CMakeLists.txt | 103 ++++---- include/super_lib/am_super_test.h | 160 ++++++++++++ launch/am_super.yaml | 3 - launch/am_super_launch.py | 21 ++ package.xml | 6 +- .../abort_to_disarming_rostest.cpp | 27 --- src/am_super/am_super.cpp | 2 +- src/super_lib/am_life_cycle.cpp | 1 + src/super_lib/am_super_test.cpp | 227 ++++++++++++++++++ .../abort_to_disarming_rostest.cpp | 31 +++ .../abort_to_disarming_rostest.test | 0 .../abort_to_disarming.test.cpython-38.pyc | Bin 0 -> 1769 bytes .../launch/abort_to_disarming.test.py | 52 ++++ .../abort_to_manual_rostest.cpp | 0 .../abort_to_manual_rostest.test | 0 {rostest => super_test}/am_super_rostest.test | 0 .../armed_to_ready/armed_to_ready_rostest.cpp | 0 .../armed_to_ready_rostest.test | 0 .../auto_to_abort/auto_to_abort_rostest.cpp | 0 .../auto_to_abort/auto_to_abort_rostest.test | 0 .../auto_to_manual/auto_to_manual_rostest.cpp | 0 .../auto_to_manual_rostest.test | 0 .../auto_to_semiauto_rostest.cpp | 0 .../auto_to_semiauto_rostest.test | 0 .../error_configure_tolerance_rostest.cpp | 0 .../error_configure_tolerance_rostest.test | 0 .../error_forced/error_forced_rostest.cpp | 0 .../error_forced/error_forced_rostest.test | 0 .../error_status/error_status_rostest.cpp | 0 .../error_status/error_status_rostest.test | 0 .../error_status_without_stats_rostest.cpp | 0 .../error_status_without_stats_rostest.test | 0 .../error_terminal_before_config_rostest.cpp | 0 .../error_terminal_before_config_rostest.test | 0 .../error_tolerant_before_config_rostest.cpp | 0 .../error_tolerant_before_config_rostest.test | 0 .../hz_config/hz_config_rostest.cpp | 0 .../hz_config/hz_config_rostest.test | 0 .../hz_config/hz_config_rostest.yaml | 0 .../manual_to_disarming_rostest.cpp | 0 .../manual_to_disarming_rostest.test | 0 .../param/param_rostest.cpp | 0 .../param/param_rostest.test | 0 .../platform_app_required_fail_rostest.cpp | 0 .../platform_app_required_fail_rostest.test | 0 .../platform_app_required_pass_rostest.cpp | 0 .../platform_app_required_pass_rostest.test | 0 .../platform_required_fail_rostest.cpp | 0 .../platform_required_fail_rostest.test | 0 .../platform_required_pass_rostest.cpp | 0 .../platform_required_pass_rostest.test | 0 .../primary/primary_rostest.cpp | 0 .../primary/primary_rostest.test | 0 .../ready_to_shutdown_rostest.cpp | 0 .../ready_to_shutdown_rostest.test | 0 .../semi_auto_to_manual_rostest.cpp | 0 .../semi_auto_to_manual_rostest.test | 0 57 files changed, 554 insertions(+), 79 deletions(-) create mode 100644 include/super_lib/am_super_test.h create mode 100644 launch/am_super_launch.py delete mode 100644 rostest/abort_to_disarming/abort_to_disarming_rostest.cpp create mode 100644 src/super_lib/am_super_test.cpp create mode 100644 super_test/abort_to_disarming/abort_to_disarming_rostest.cpp rename {rostest => super_test}/abort_to_disarming/abort_to_disarming_rostest.test (100%) create mode 100644 super_test/abort_to_disarming/launch/__pycache__/abort_to_disarming.test.cpython-38.pyc create mode 100644 super_test/abort_to_disarming/launch/abort_to_disarming.test.py rename {rostest => super_test}/abort_to_manual/abort_to_manual_rostest.cpp (100%) rename {rostest => super_test}/abort_to_manual/abort_to_manual_rostest.test (100%) rename {rostest => super_test}/am_super_rostest.test (100%) rename {rostest => super_test}/armed_to_ready/armed_to_ready_rostest.cpp (100%) rename {rostest => super_test}/armed_to_ready/armed_to_ready_rostest.test (100%) rename {rostest => super_test}/auto_to_abort/auto_to_abort_rostest.cpp (100%) rename {rostest => super_test}/auto_to_abort/auto_to_abort_rostest.test (100%) rename {rostest => super_test}/auto_to_manual/auto_to_manual_rostest.cpp (100%) rename {rostest => super_test}/auto_to_manual/auto_to_manual_rostest.test (100%) rename {rostest => super_test}/auto_to_semiauto/auto_to_semiauto_rostest.cpp (100%) rename {rostest => super_test}/auto_to_semiauto/auto_to_semiauto_rostest.test (100%) rename {rostest => super_test}/error_configure_tolerance/error_configure_tolerance_rostest.cpp (100%) rename {rostest => super_test}/error_configure_tolerance/error_configure_tolerance_rostest.test (100%) rename {rostest => super_test}/error_forced/error_forced_rostest.cpp (100%) rename {rostest => super_test}/error_forced/error_forced_rostest.test (100%) rename {rostest => super_test}/error_status/error_status_rostest.cpp (100%) rename {rostest => super_test}/error_status/error_status_rostest.test (100%) rename {rostest => super_test}/error_status_without_stats/error_status_without_stats_rostest.cpp (100%) rename {rostest => super_test}/error_status_without_stats/error_status_without_stats_rostest.test (100%) rename {rostest => super_test}/error_terminal_before_config/error_terminal_before_config_rostest.cpp (100%) rename {rostest => super_test}/error_terminal_before_config/error_terminal_before_config_rostest.test (100%) rename {rostest => super_test}/error_tolerant_before_config/error_tolerant_before_config_rostest.cpp (100%) rename {rostest => super_test}/error_tolerant_before_config/error_tolerant_before_config_rostest.test (100%) rename {rostest => super_test}/hz_config/hz_config_rostest.cpp (100%) rename {rostest => super_test}/hz_config/hz_config_rostest.test (100%) rename {rostest => super_test}/hz_config/hz_config_rostest.yaml (100%) rename {rostest => super_test}/manual_to_disarming/manual_to_disarming_rostest.cpp (100%) rename {rostest => super_test}/manual_to_disarming/manual_to_disarming_rostest.test (100%) rename {rostest => super_test}/param/param_rostest.cpp (100%) rename {rostest => super_test}/param/param_rostest.test (100%) rename {rostest => super_test}/platform_app_required_fail/platform_app_required_fail_rostest.cpp (100%) rename {rostest => super_test}/platform_app_required_fail/platform_app_required_fail_rostest.test (100%) rename {rostest => super_test}/platform_app_required_pass/platform_app_required_pass_rostest.cpp (100%) rename {rostest => super_test}/platform_app_required_pass/platform_app_required_pass_rostest.test (100%) rename {rostest => super_test}/platform_required_fail/platform_required_fail_rostest.cpp (100%) rename {rostest => super_test}/platform_required_fail/platform_required_fail_rostest.test (100%) rename {rostest => super_test}/platform_required_pass/platform_required_pass_rostest.cpp (100%) rename {rostest => super_test}/platform_required_pass/platform_required_pass_rostest.test (100%) rename {rostest => super_test}/primary/primary_rostest.cpp (100%) rename {rostest => super_test}/primary/primary_rostest.test (100%) rename {rostest => super_test}/ready_to_shutdown/ready_to_shutdown_rostest.cpp (100%) rename {rostest => super_test}/ready_to_shutdown/ready_to_shutdown_rostest.test (100%) rename {rostest => super_test}/semi_auto_to_manual/semi_auto_to_manual_rostest.cpp (100%) rename {rostest => super_test}/semi_auto_to_manual/semi_auto_to_manual_rostest.test (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index ea9d7e14..4cd39a58 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,8 +1,12 @@ cmake_minimum_required(VERSION 3.5) project(am_super) + +set(CMAKE_BUILD_TYPE Debug) + if(POLICY CMP0074) - cmake_policy(SET CMP0074 NEW) + cmake_policy(SET CMP0074 NEW) endif() + # Default to C99 if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) @@ -13,41 +17,30 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - #to view [-Wunused-parameter] uncomment the following line - #add_compile_options(-Wall -Wextra -Wpedantic) -endif() +set(dependencies + am_utils + brain_box_msgs + builtin_interfaces + diagnostic_msgs + diagnostic_updater + rclcpp + rclpy + rosbag2 + rosbag2_cpp + std_msgs + rosbag2_cpp +) # find dependencies find_package(ament_cmake REQUIRED) -find_package(brain_box_msgs REQUIRED) -find_package(am_utils REQUIRED) -find_package(builtin_interfaces REQUIRED) -find_package(std_msgs REQUIRED) -find_package(rosbag2 REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclpy REQUIRED) -find_package(rosbag2_cpp REQUIRED) -find_package(diagnostic_msgs REQUIRED) -find_package(diagnostic_updater REQUIRED) -set(dependencies - brain_box_msgs - am_utils - builtin_interfaces - std_msgs - rosbag2 - rclcpp - rclpy - rosbag2_cpp - diagnostic_msgs - diagnostic_updater - ) +foreach(Dependency IN ITEMS ${dependencies}) + find_package(${Dependency} REQUIRED) +endforeach() include_directories( - include - ) - + include +) file(GLOB super_lib_cpp_files src/super_lib/*.cpp @@ -75,18 +68,14 @@ ament_export_targets(super_libTargets HAS_LIBRARY_TARGET) ament_export_include_directories(include) ament_export_libraries(super_lib) - add_executable(am_super ${am_super_cpp_files}) target_link_libraries(am_super super_lib) ament_target_dependencies(am_super ${dependencies}) - -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) install(DIRECTORY include/ DESTINATION include/ - ) +) + install( TARGETS super_lib EXPORT super_libTargets @@ -95,21 +84,43 @@ install( RUNTIME DESTINATION bin INCLUDES DESTINATION include ) -#install(TARGETS -# am_super -# DESTINATION lib/${PROJECT_NAME}) + +install(TARGETS + am_super + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) + 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() + + ament_add_gtest_executable(abort_to_disarming + super_test/abort_to_disarming/abort_to_disarming_rostest.cpp + ) + + target_link_libraries(abort_to_disarming super_lib) + ament_target_dependencies(abort_to_disarming am_utils ${dependencies}) + add_ros_test(super_test/abort_to_disarming/launch/abort_to_disarming.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") endif() ament_export_include_directories(include) -ament_export_libraries(am_utils) +ament_export_libraries(super_lib) ament_export_dependencies(${dependencies}) ament_package() diff --git a/include/super_lib/am_super_test.h b/include/super_lib/am_super_test.h new file mode 100644 index 00000000..ffd6a346 --- /dev/null +++ b/include/super_lib/am_super_test.h @@ -0,0 +1,160 @@ +#ifndef _AM_SUPER_SUPER_LIB_AM_SUPER_TEST_H_ +#define _AM_SUPER_SUPER_LIB_AM_SUPER_TEST_H_ + +#include + +#include + +#include // googletest header file + +#include // to be armed, launch for state transitions +#include +#include // msg for status +#include // msg for status +#include +#include +#include +#include +#include + +using namespace std; +using namespace am; + +/** + * Base class for testing the LifeCycle transitions of a target node along with the + * state transition of the supervisor node. + */ +class AMSuperTest : public ::testing::Test +{ +private: + rclcpp::Subscription::SharedPtr nodeLifeCycleStateSubscription_; + rclcpp::Subscription::SharedPtr missionStateSubscription_; + rclcpp::Publisher::SharedPtr operatorCommandPublisher_; + rclcpp::Publisher::SharedPtr controllerStatePublisher_; + std::multimap node_states_; + std::vector mission_states_; + + /** Act as the operator (typically via a remote or ground station) to send one + * signal to transition to proceed, cancel, abort, etc. + * + * @param command one of brain_box_msgs::OperatorCommand::ARM; + * + * @see arm() + * @see launch() + */ + void publishOperatorCommand(uint8_t command); + + /** Acting as the controller, this publishes the controller state + * to signal transitions due to autonomous processing + * @param state one of the brain_box_msgs::ControllerState state enums + * @see landed() + * */ + void publishControllerState(uint8_t state); + +public: + rclcpp::Node::SharedPtr nh_; + string target_node_name_; + + AMSuperTest(); + AMSuperTest(string target_node_name); + + /** when configured and Inactive, arm is sent to prepare all nodes for takeoff*/ + void arm(); + + /** when armed, signals for the props to spin and takeoff */ + void launch(); + + /** to transition super into disarming, flight must be completed by landing or canceled */ + void landed(); + + /** to transition super into disarming from armed */ + void cancel(); + + /** to transition super into semi-auto from auto */ + void pause(); + + /** to transition super into auto from semi-auto */ + void resume(); + + /** to transition super into abort while in flight mode */ + void abort(); + + /** to transition super into manual from an auto mode*/ + void manual(); + + /** to shut super down. Must be in BOOTING or READY */ + void shutdown(); + + + /** searches the node states matching the lifecycle given. + * + * @return true if the state is found for the node given, false otherwise + */ + bool nodeStateReceived(string node_name,LifeCycleState state); + + /** + * @return true if the desired state is anywhere in the list, regardless of order + */ + bool missionStateReceived(uint8_t mission_state); + + /** + * Callback sniffing the state of nodes, as if it were am_super, to see + * if the target node is transitioning as expected. + * + * Simply registers states in multimap for later inspection. + */ + void nodeLifeCycleStateCallback(const brain_box_msgs::msg::LifeCycleState& msg); + + void missionStateCallback(const brain_box_msgs::msg::VxState& msg); + + /** Loop until a am_super is broadcasting the desired state or until + * ros says its time to quit. + * FIXME: export SuperState into a library and use instead of the untyped messages. + * @param log_code to corrleate the log output to the source code + */ + void waitUntilMissionState(const uint8_t mssion_state, std::string log_code, float sleep = 1); + + [[deprecated("use waitUntilMissionState(status,log_code,sleep)")]] + void waitUntilMissionState(const uint8_t mssion_state, float sleep = 1); + + /** + * spin until the desired state is found or until the test times out. + */ + void waitUntil(const LifeCycleState state, float sleep = 1); + + /** + * @param log_code makes it easy to find the source of the log messages + */ + void waitUntil(const LifeCycleState state, const std::string log_code, float sleep = 1); + + + /** + * Wait until status is received or the test times out. + */ + [[deprecated("use waitUntil(status,log_code,sleep)")]] + void waitUntilStatus(const LifeCycleStatus& status, float sleep = 1); + + /** + * @brief look for status periodically, based on the sleep, or timeout based on the test time limit. + * @param log_code makes it easy to find the source of the log messages + */ + void waitUntil(const LifeCycleStatus& status, const std::string log_code,float sleep = 1); + + /** + * Looks to see if the given status has ever been received. + */ + bool nodeStatusReceived(string node_name, LifeCycleStatus status); + + /** + * Generate publishers and subscriptions. + */ + void createPubsSubs(); + +#define TEST_LOG( args, code) \ + do \ + { \ + RCLCPP_WARN_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 1.0, args << " [" << code << "]"); \ + } while (0) +}; + +#endif \ No newline at end of file diff --git a/launch/am_super.yaml b/launch/am_super.yaml index e4502276..e69de29b 100644 --- a/launch/am_super.yaml +++ b/launch/am_super.yaml @@ -1,3 +0,0 @@ - -platform: - actual: $(arg platform) \ No newline at end of file diff --git a/launch/am_super_launch.py b/launch/am_super_launch.py new file mode 100644 index 00000000..f0c4b456 --- /dev/null +++ b/launch/am_super_launch.py @@ -0,0 +1,21 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + + ld = LaunchDescription() + config = os.path.join( + get_package_share_directory('am_super'), + 'launch', + 'am_super.yaml' + ) + node=Node( + package = 'am_super', + name = 'am_super', + executable = 'am_super', + parameters = [config] + ) + ld.add_action(node) + return ld diff --git a/package.xml b/package.xml index 625729f8..98c3e4af 100644 --- a/package.xml +++ b/package.xml @@ -9,17 +9,19 @@ ament_cmake + ros2launch + am_utils brain_box_msgs + builtin_interfaces diagnostic_msgs + diagnostic_updater rosbag2 rosbag2_cpp rclcpp rclpy std_msgs std_srvs - builtin_interfaces - diagnostic_updater ament_lint_auto ament_lint_common 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 9ab969a9..59be6f33 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -126,7 +126,7 @@ class AMSuper : AMLifeCycle #endif public: - AMSuper(rclcpp::Node::SharedPtr nh) : nh_(nh), AMLifeCycle(nh_), node_mediator_(nh_, SuperNodeMediator::nodeNameStripped(nh->get_name())) + AMSuper(rclcpp::Node::SharedPtr nh) : nh_(nh), AMLifeCycle(nh), node_mediator_(nh, SuperNodeMediator::nodeNameStripped(nh->get_name())) { RCLCPP_INFO_STREAM(nh_->get_logger(), nh_->get_name()); diff --git a/src/super_lib/am_life_cycle.cpp b/src/super_lib/am_life_cycle.cpp index f60de95a..4058bf09 100644 --- a/src/super_lib/am_life_cycle.cpp +++ b/src/super_lib/am_life_cycle.cpp @@ -469,6 +469,7 @@ AMStatReset& AMLifeCycle::configureHzStats(AMStatReset& stats) void AMLifeCycle::sendNodeUpdate() { brain_box_msgs::msg::LifeCycleState msg; + msg.header.stamp = node_->now(); msg.node_name = node_->get_name(); msg.process_id = 0; msg.state = (uint8_t)life_cycle_info_.state; diff --git a/src/super_lib/am_super_test.cpp b/src/super_lib/am_super_test.cpp new file mode 100644 index 00000000..e5b6d7d3 --- /dev/null +++ b/src/super_lib/am_super_test.cpp @@ -0,0 +1,227 @@ +#include +#include + +#include + +AMSuperTest::AMSuperTest(string target_node_name) : nh_(std::make_shared(target_node_name)) +{ + createPubsSubs(); + + target_node_name_= target_node_name; + if(target_node_name_[0] != '/') + { + target_node_name_= '/' + target_node_name_; + } + TEST_LOG("Target node name:" << target_node_name_,"AXSO"); +} + +AMSuperTest::AMSuperTest() : nh_(std::make_shared("am_super_test")) +{ + createPubsSubs(); + + am::getParam(nh_, "~target_node_name", target_node_name_, nh_->get_name()); + if(target_node_name_[0] != '/') + { + target_node_name_= '/' + target_node_name_; + } + + TEST_LOG("Target node name:" << target_node_name_, "ANNQ"); +} + +void AMSuperTest::createPubsSubs() +{ + nodeLifeCycleStateSubscription_ = nh_->create_subscription + (am_super_topics::LIFECYCLE_STATE, 1000, + std::bind(&AMSuperTest::nodeLifeCycleStateCallback, this, std::placeholders::_1)); + missionStateSubscription_ = nh_->create_subscription + (am_super_topics::SUPER_STATE, 1000, + std::bind(&AMSuperTest::missionStateCallback, this, std::placeholders::_1)); + operatorCommandPublisher_ = nh_->create_publisher + (am_super_topics::OPERATOR_COMMAND,100); + controllerStatePublisher_ = nh_->create_publisher + (am_super_topics::CONTROLLER_STATE, 100); +} + +void AMSuperTest::publishOperatorCommand(uint8_t command) +{ + brain_box_msgs::msg::OperatorCommand msg; + msg.node_name = nh_->get_name(); + msg.command = command; + operatorCommandPublisher_->publish(msg); +} + +void AMSuperTest::publishControllerState(uint8_t state) +{ + brain_box_msgs::msg::ControllerState msg; + msg.node_name = nh_->get_name(); + msg.state = state; + controllerStatePublisher_->publish(msg); +} + +/** when configured and Inactive, arm is sent to prepare all nodes for takeoff*/ +void AMSuperTest::arm() +{ + TEST_LOG("Operator sending ARM command","NQNA"); + publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::ARM); +} + +/** when armed, signals for the props to spin and takeoff */ +void AMSuperTest::launch() +{ + TEST_LOG("Operator sending LAUNCH command","ANQP"); + publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::LAUNCH); +} + +void AMSuperTest::landed() +{ + TEST_LOG("Controller sending LANDED state","BSJO"); + publishControllerState(brain_box_msgs::msg::ControllerState::COMPLETED); +} + +void AMSuperTest::cancel() +{ + TEST_LOG("Operator sending CANCEL command","LPOQ"); + publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::CANCEL); +} + +void AMSuperTest::pause() +{ + TEST_LOG("Operator sending PAUSE command","WOEB"); + publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::PAUSE); +} + +void AMSuperTest::resume() +{ + TEST_LOG("Operator sending RESUME command","ANQE"); + publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::RESUME); +} + +void AMSuperTest::abort() +{ + TEST_LOG("Operator sending ABORT command","EEEN"); + publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::ABORT); +} + +void AMSuperTest::manual() +{ + TEST_LOG("Operator sending MANUAL command","NQAY"); + publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::MANUAL); +} + +void AMSuperTest::shutdown() +{ + TEST_LOG("Operator sending SHUTDOWN command","VKSP"); + publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::SHUTDOWN); +} + +bool AMSuperTest::nodeStateReceived(string node_name,LifeCycleState state) +{ + if(node_states_.count(node_name)){ + int key = 2; + auto lower_it = node_states_.lower_bound(node_name); + auto upper_it = node_states_.upper_bound(node_name); + + while (lower_it != upper_it) + { + if (lower_it -> first == node_name) { + brain_box_msgs::msg::LifeCycleState state_msg = lower_it -> second; + if((LifeCycleState)state_msg.state == state){ + return true; + } + } + lower_it++; + } + } + return false; +} + +bool AMSuperTest::missionStateReceived(uint8_t mission_state) +{ + for (auto state : mission_states_) + { + if(state == mission_state) + { + //Erase-remove idiom for removing all occurences from list + mission_states_.erase(remove(mission_states_.begin(), mission_states_.end(), state), mission_states_.end()); + return true; + } + } + return false; +} + +void AMSuperTest::nodeLifeCycleStateCallback(const brain_box_msgs::msg::LifeCycleState& msg) +{ + node_states_.emplace(msg.node_name,msg); +} + +void AMSuperTest::missionStateCallback(const brain_box_msgs::msg::VxState& msg) +{ + mission_states_.insert(mission_states_.end(),msg.state); +} + +void AMSuperTest::waitUntilMissionState(const uint8_t mission_state, float sleep) +{ + waitUntilMissionState(mission_state,"NAWU",sleep); +} + +void AMSuperTest::waitUntilMissionState(const uint8_t mission_state, std::string error_code, float sleep) +{ + rclcpp::Rate loop_rate(sleep); + while (!missionStateReceived(mission_state) && rclcpp::ok() ) + { + rclcpp::spin_some(nh_); + loop_rate.sleep(); + TEST_LOG("[" << error_code << "] waiting to receive mission state: " << mission_state, "08JU"); + } +} + +void AMSuperTest::waitUntil(const LifeCycleState state, float sleep){ + waitUntil(state,"XS32",sleep); +} + +void AMSuperTest::waitUntil(const LifeCycleState state, const std::string log_code, float sleep){ + rclcpp::Rate loop_rate(sleep); + while (!nodeStateReceived(nh_->get_name(),state) && rclcpp::ok() ) + { + rclcpp::spin_some(nh_); + loop_rate.sleep(); + TEST_LOG("[" << log_code << "]" << " waiting to receive node state: " << (int)state,"NBDC"); + } +} + +void AMSuperTest::waitUntilStatus(const LifeCycleStatus& status, float sleep) +{ + waitUntil(status,"XWSQ",sleep); + +} +void AMSuperTest::waitUntil(const LifeCycleStatus& status, const std::string log_code, float sleep) +{ + rclcpp::Rate loop_rate(sleep); + while (!nodeStatusReceived(nh_->get_name(),status) && rclcpp::ok() ) + { + rclcpp::spin_some(nh_); + loop_rate.sleep(); + TEST_LOG("[" << log_code << "]" << " waiting to receive node status: " << (int)status, "YTNJ" ); + } +} + +bool AMSuperTest::nodeStatusReceived(string node_name, LifeCycleStatus status) +{ + if(node_states_.count(node_name)){ + int key = 2; + auto lower_it = node_states_.lower_bound(node_name); + auto upper_it = node_states_.upper_bound(node_name); + + while (lower_it != upper_it) + { + if (lower_it -> first == node_name) { + brain_box_msgs::msg::LifeCycleState state_msg = lower_it -> second; + if((LifeCycleStatus)state_msg.status == status){ + return true; + } + } + lower_it++; + } + } + return false; +} \ No newline at end of file diff --git a/super_test/abort_to_disarming/abort_to_disarming_rostest.cpp b/super_test/abort_to_disarming/abort_to_disarming_rostest.cpp new file mode 100644 index 00000000..37e5e9a4 --- /dev/null +++ b/super_test/abort_to_disarming/abort_to_disarming_rostest.cpp @@ -0,0 +1,31 @@ +#include + +class AbortToDisarming : public AMSuperTest +{ +protected: + AbortToDisarming() : AMSuperTest("abort_to_disarming") {} +}; + +TEST_F(AbortToDisarming, testState_AbortToDisarming) +{ + waitUntilMissionState(brain_box_msgs::msg::VxState::READY,"N3DJ"); + arm(); + waitUntilMissionState(brain_box_msgs::msg::VxState::ARMED,"XX3X"); + launch(); + waitUntilMissionState(brain_box_msgs::msg::VxState::AUTO,"YYUI"); + abort(); + waitUntilMissionState(brain_box_msgs::msg::VxState::ABORT,"NSKE"); + landed(); + waitUntilMissionState(brain_box_msgs::msg::VxState::DISARMING,"XXCV"); +} + +int main(int argc, char** argv) +{ + std::cout << "STARTING ROS" << std::endl; + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + std::cout << "DONE SHUTTING DOWN ROS" << std::endl; + return result; +} \ No newline at end of file diff --git a/rostest/abort_to_disarming/abort_to_disarming_rostest.test b/super_test/abort_to_disarming/abort_to_disarming_rostest.test similarity index 100% rename from rostest/abort_to_disarming/abort_to_disarming_rostest.test rename to super_test/abort_to_disarming/abort_to_disarming_rostest.test diff --git a/super_test/abort_to_disarming/launch/__pycache__/abort_to_disarming.test.cpython-38.pyc b/super_test/abort_to_disarming/launch/__pycache__/abort_to_disarming.test.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..faae1222fac3944fb7fd3186edc725cad1118acb GIT binary patch literal 1769 zcmbVMPjBNy6rUM8j^m_Tw%hV&DG-SZL8uXX;et??EUdIbQALZ8e86%&)6~KCgqg8+ zQ-#y^+V9XFIdR~#aG5J7J^&JM0NxuXZB|=2FxLF$@0<7D@4e@*HZ~#xt^WC^Oaz4d zf{XRmg2g^`eGb40rv-^|WCbl*%qY&i!YlpQcXPjJl|dZ1IW5|8+sTA+=*CXmabpxm zyv2ia5_caH-sa&s;h~78z2{!MA=;OBt_bXRcm(@Vv;UmNo17k!LHAd%gp3E&c8~H> zsE3)!Wd+OagVdZpk$H7kA8V7Fdb#h9B^MbAtr~?1I7>#*^%DR?44tw|&!Nxh73FNg z06p$suQLo-i=4J&rex&?Y&~t5nSso-4bR z9!q5sBVnaeRp!;i20BwhR0C!s@l0g3NskL*16i9{ZO%UW)JgY2T4YA5d0(FNXKD6r zIuZR$Rwm6Wc%_e;`Zw|#p7X1;6t>F+D9C4ygGl+{qz#I+uCi0Rq1On~%EPmJU%0c8 ztWNSttx}Y;cbm;eH&5Ai>gd*X=aI;YREg%wLp7;OQ5k!u(UG8G=w|mzk@ESGJc9Lr zDom#zh`I;EpWh5mWhsU{t%geK2gy@C)G8aMWuohuP(z3Aq2bH?4I4eToa~|Mz1iHp zF%gvjPlS6TStFze>h=2ogfSYhh_b)f-z;E%1d-1I%3$rc7z=-g-5w)=zo{L#{X!Bs zae#g3dK*BHm~s*`ND&5^?Qsv%!MFF(^~VTbr@8rDs*x;bg+PKc24bs*a}1M9N_=lY zresPN6yLjWl?Ly5gpD*GCemP{O~x=OOfMu8-0XC*y*@b8W|&TkPPga2WYU`8g1jgq94ye@Fw zOOo&Ev}jJ$o1kXRBuo#5cBxkZ-h=LJ2$@^|uPE$Z8-D;i39S#LHY;i}EP++S`3rF{ z5QQmQ&;>;HIt~nfKyBn4-Wqu7E=WOfIO4V4Y$l&Q%gqRWK6v3iTu`qgU=6G}>-cYe z84dq45dIME=n#NV$VbS)vk(5ufpz{-9C%l|sOBw%w-Me&aKzq#qxc;-*n{3s>?vFi zUQt~b;$=1OEsI`ZCdAuK>FvGz7m2r))erT2ErVP(^f0Ndr{~&+b(I^;HCI(OMAt?s tRZPwfH<0hp(LDMwKhqULukh;$ccejFo{sYDJ*0cZs literal 0 HcmV?d00001 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 From de4bc7dbc3994f883cd680271c754de5dcb8028f Mon Sep 17 00:00:00 2001 From: AJ Date: Mon, 9 Jan 2023 11:02:33 -0800 Subject: [PATCH 07/45] fix: LOG_MSG and undefined reference to am::Node --- CMakeLists.txt | 2 +- include/am_super/baby_sitter.h | 2 +- include/am_super/super_node_mediator.h | 1 + include/am_super/super_state_mediator.h | 1 + src/am_super/am_super.cpp | 24 ++++++++++++++---------- 5 files changed, 18 insertions(+), 12 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4cd39a58..855be57e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,6 +17,7 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif() + set(dependencies am_utils brain_box_msgs @@ -28,7 +29,6 @@ set(dependencies rosbag2 rosbag2_cpp std_msgs - rosbag2_cpp ) # find dependencies diff --git a/include/am_super/baby_sitter.h b/include/am_super/baby_sitter.h index 78577a40..cf5d8745 100644 --- a/include/am_super/baby_sitter.h +++ b/include/am_super/baby_sitter.h @@ -352,7 +352,7 @@ void BabySitter::heartbeatCB() 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(log_msg, std::string("/status/super/" + node_name_), "brain_box_msgs/msg/BabySitterStatus", nh_->now(), 1); + LOG_MSG(std::string("/status/super/" + node_name_), log_msg, 1); if (node_state_ == LifeCycleState::ACTIVE) { diff --git a/include/am_super/super_node_mediator.h b/include/am_super/super_node_mediator.h index 17754d8f..df0773b1 100644 --- a/include/am_super/super_node_mediator.h +++ b/include/am_super/super_node_mediator.h @@ -13,6 +13,7 @@ #include #include #include +#include using namespace std; 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/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index 59be6f33..e2708911 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #if CUDA_FLAG #include @@ -277,7 +278,7 @@ class AMSuper : AMLifeCycle rmsg->value, rmsg->process_id, rmsg->header.stamp); // TODO: topic name should come from vb_util_lib::topics.h - LOG_MSG(*rmsg, am_super_topics::LIFECYCLE_STATE, "brain_box_msgs/msg/LifeCycleState", rmsg->header.stamp, SU_LOG_LEVEL); + LOG_MSG(am_super_topics::LIFECYCLE_STATE, *rmsg, SU_LOG_LEVEL); } //void controllerStateCB(const ros::MessageEvent& event) @@ -298,7 +299,7 @@ class AMSuper : AMLifeCycle node_mediator_.setOperatorCommand(supervisor_, (OperatorCommand)rmsg->command); // TODO: topic name should come from vb_util_lib::topics. - LOG_MSG(*rmsg, "/operator/command", "brain_box_msgs/msg/OperatorCommand", nh_->now(), SU_LOG_LEVEL); + LOG_MSG("/operator/command", *rmsg, SU_LOG_LEVEL); } /** * process state @@ -470,7 +471,7 @@ class AMSuper : AMLifeCycle SuperNodeMediator::SuperNodeInfo& nr = (*it).second; status_msg.nodes.push_back(nr.name); } - LOG_MSG(status_msg, "/status/super", "brain_box_msgs/msg/Super2Status", nh_->now(), 1); + LOG_MSG("/status/super", status_msg, 1); if (super_status_pub_->get_subscription_count() > 0) { super_status_pub_->publish(status_msg); @@ -501,7 +502,7 @@ class AMSuper : AMLifeCycle getline(newfile, tp); std_msgs::msg::Int16 msg; msg.data = std::stoi(tp); - LOG_MSG(msg, "/watts", "std_msgs/msg/Int16", nh_->now(), SU_LOG_LEVEL); + LOG_MSG("/watts", msg, SU_LOG_LEVEL); newfile.close(); //close the file object. } @@ -831,12 +832,12 @@ class AMSuper : AMLifeCycle void diagnosticsCB(const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg) { - LOG_MSG(*msg, "/diagnostics", "diagnostic_msgs/msg/DiagnosticArray", nh_->now(), SU_LOG_LEVEL); + LOG_MSG("/diagnostics", *msg, SU_LOG_LEVEL); } void currentENUCB(const nav_msgs::msg::Odometry::SharedPtr msg) { - LOG_MSG(*msg, am_topics::CTRL_VX_VEHICLE_CURRENTENU, "nav_msgs/msg/Odometry", nh_->now(), SU_LOG_LEVEL); + LOG_MSG(am_topics::CTRL_VX_VEHICLE_CURRENTENU, *msg, SU_LOG_LEVEL); } BagLogger::BagLoggerLevel intToLoggerLevel(int level) @@ -879,17 +880,20 @@ class AMSuper : AMLifeCycle #ifdef TESTING #else + +std::shared_ptr am::Node::node; + int main(int argc, char** argv) { rclcpp::init(argc, argv); - std::shared_ptr node = std::make_shared("am_super"); + am::Node::node = std::make_shared("am_super"); - std::shared_ptr am_super_node = std::make_shared(node); + std::shared_ptr am_super_node = std::make_shared(am::Node::node); - RCLCPP_INFO_STREAM(node->get_logger(), node->get_name() << ": running..."); + ROS_INFO_STREAM(am::Node::node->get_name() << ": running..."); - rclcpp::spin(node); + rclcpp::spin(am::Node::node); rclcpp::shutdown(); From c802aaf20ecd34e5ab16a5b7b8c0cba27ea12d6f Mon Sep 17 00:00:00 2001 From: edkoch Date: Wed, 18 Jan 2023 11:47:07 -0800 Subject: [PATCH 08/45] attempted to complete port of am_super --- CMakeLists.txt | 6 +++--- include/am_super/baby_sitter.h | 8 ++++---- src/am_super/am_super.cpp | 13 ++++++------- src/super_lib/am_super_test.cpp | 2 +- 4 files changed, 14 insertions(+), 15 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 855be57e..2dcf581e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -68,9 +68,9 @@ ament_export_targets(super_libTargets HAS_LIBRARY_TARGET) ament_export_include_directories(include) ament_export_libraries(super_lib) -add_executable(am_super ${am_super_cpp_files}) -target_link_libraries(am_super super_lib) -ament_target_dependencies(am_super ${dependencies}) +#add_executable(am_super ${am_super_cpp_files}) +#target_link_libraries(am_super super_lib) +#ament_target_dependencies(am_super ${dependencies}) install(DIRECTORY include/ DESTINATION include/ diff --git a/include/am_super/baby_sitter.h b/include/am_super/baby_sitter.h index cf5d8745..24eb4ad0 100644 --- a/include/am_super/baby_sitter.h +++ b/include/am_super/baby_sitter.h @@ -103,19 +103,19 @@ BabySitter::BabySitter(const rclcpp::Node::SharedPtr nh, BagLogger* logger, c std::string parm = "~" + node_name + "/warn_ms"; - am::getParam(nh_, parm, warn_ms_, 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"; - am::getParam(nh_,parm, error_ms_, 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"; - am::getParam(nh_,parm, warn_count_thresh_, 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"; - am::getParam(nh_,parm, timeout_ms_, timeout_ms); + am::getParam(parm, timeout_ms_, timeout_ms); RCLCPP_INFO_STREAM(nh_->get_logger(), nh_->get_name() << parm << " = " << timeout_ms_); min_ms_ = 1000; diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index e2708911..23d2e93c 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -1,7 +1,7 @@ #include #include -#include +#include #include #include #include @@ -36,7 +36,6 @@ #include #include #include -#include #if CUDA_FLAG #include @@ -131,7 +130,7 @@ class AMSuper : AMLifeCycle { RCLCPP_INFO_STREAM(nh_->get_logger(), nh_->get_name()); - am::getParam(nh_, "node_timeout_s", node_timeout_s_, 2.0); + am::getParam("node_timeout_s", node_timeout_s_, 2.0); RCLCPP_INFO_STREAM(nh_->get_logger(), "node_timeout_s = " << node_timeout_s_); /* @@ -140,7 +139,7 @@ class AMSuper : AMLifeCycle supervisor_.system_state = SuperState::OFF; // strip spaces from manifest param string manifest_param; - am::getParam(nh_, "manifest", manifest_param, ""); + am::getParam("manifest", manifest_param, ""); node_mediator_.parseManifest(supervisor_, manifest_param); @@ -878,8 +877,8 @@ class AMSuper : AMLifeCycle }; }; -#ifdef TESTING -#else +// #ifdef TESTING +// #else std::shared_ptr am::Node::node; @@ -899,4 +898,4 @@ int main(int argc, char** argv) return 0; } -#endif +// #endif diff --git a/src/super_lib/am_super_test.cpp b/src/super_lib/am_super_test.cpp index e5b6d7d3..786eb7a4 100644 --- a/src/super_lib/am_super_test.cpp +++ b/src/super_lib/am_super_test.cpp @@ -19,7 +19,7 @@ AMSuperTest::AMSuperTest() : nh_(std::make_shared("am_super_test") { createPubsSubs(); - am::getParam(nh_, "~target_node_name", target_node_name_, nh_->get_name()); + am::getParam("~target_node_name", target_node_name_, nh_->get_name()); if(target_node_name_[0] != '/') { target_node_name_= '/' + target_node_name_; From 3ae783a55697fb310be352556dd5ef2bb5169112 Mon Sep 17 00:00:00 2001 From: danhennage Date: Mon, 23 Jan 2023 16:30:55 -0800 Subject: [PATCH 09/45] feat: lifecycle inherits from node --- CMakeLists.txt | 6 +- include/super_lib/am_life_cycle.h | 31 ++- launch/am_super.yaml | 4 + src/am_super/am_super.cpp | 405 ++++++++++++++++-------------- src/super_lib/am_life_cycle.cpp | 106 ++++---- src/super_lib/am_super_test.cpp | 2 + 6 files changed, 311 insertions(+), 243 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2dcf581e..855be57e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -68,9 +68,9 @@ ament_export_targets(super_libTargets HAS_LIBRARY_TARGET) ament_export_include_directories(include) ament_export_libraries(super_lib) -#add_executable(am_super ${am_super_cpp_files}) -#target_link_libraries(am_super super_lib) -#ament_target_dependencies(am_super ${dependencies}) +add_executable(am_super ${am_super_cpp_files}) +target_link_libraries(am_super super_lib) +ament_target_dependencies(am_super ${dependencies}) install(DIRECTORY include/ DESTINATION include/ diff --git a/include/super_lib/am_life_cycle.h b/include/super_lib/am_life_cycle.h index e8940afc..1f03101e 100644 --- a/include/super_lib/am_life_cycle.h +++ b/include/super_lib/am_life_cycle.h @@ -3,7 +3,7 @@ #include -#include +#include #include @@ -27,7 +27,7 @@ namespace am * https://automodality.atlassian.net/wiki/spaces/AMROS/pages/901546330/AM+Node+LifeCycle * */ -class AMLifeCycle +class AMLifeCycle : public rclcpp::Node { public: static constexpr std::string_view BROADCAST_NODE_NAME = ""; @@ -55,6 +55,10 @@ class AMLifeCycle * max_configure_seconds_ to allow startup error tolerance.*/ rclcpp::Time configure_start_time_; + /** initialization - called by constructors. + */ + void initialize(); + void setState(const LifeCycleState state); /* if status is valid, then set this status to status */ @@ -76,35 +80,38 @@ class AMLifeCycle void error(std::string message, std::string error_code, bool forced = false); void configureStat(AMStat& stat, std::string name, std::string category=""); - protected: + public: std::string node_name_; /**Maximum time errors will be ignored during configuration. */ int configure_tolerance_s; + // std::shared_ptr updater_; diagnostic_updater::Updater updater_; AMStatList stats_list_; - rclcpp::Node::SharedPtr node_; rclcpp::TimerBase::SharedPtr heartbeat_timer_; rclcpp::Publisher::SharedPtr state_pub_; rclcpp::Subscription::SharedPtr lifecycle_sub_; - /** - * @brief Default constructor - */ - AMLifeCycle(rclcpp::Node::SharedPtr node); + AMLifeCycle( + const std::string & node_name, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + AMLifeCycle( + const std::string & node_name, + const std::string & namespace_, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** * @brief Virtual destructor */ virtual ~AMLifeCycle(); + /** Exactly like ros::param, but ROS_INFO's level showing the actual value assigned. + */ 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; + bool param(const std::string& param_name, T& param_val, const T& default_val); //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 diff --git a/launch/am_super.yaml b/launch/am_super.yaml index e69de29b..255fa3c5 100644 --- a/launch/am_super.yaml +++ b/launch/am_super.yaml @@ -0,0 +1,4 @@ +am_super: + ros__parameters: + platform: + actual: farm-ng_amiga_mockup diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index 23d2e93c..51c5c2a4 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -50,20 +50,19 @@ namespace am * * uses BabySitter instances to generate state and health for nodes that don't publish brain_box_msgs::LifeCycleState */ -class AMSuper : AMLifeCycle +class AMSuper { + friend class AMSuperNode; + private: + shared_ptr life_cycle_node_; + /** * heartbeat log output period */ const int LOG_THROTTLE_S = 10; - /** - * the ros node handle - */ - rclcpp::Node::SharedPtr nh_; - - /* + /* * see constructor for details */ rclcpp::Publisher::SharedPtr lifecycle_pub_; @@ -78,7 +77,6 @@ class AMSuper : AMLifeCycle rclcpp::Subscription::SharedPtr controller_state_sub; rclcpp::Subscription::SharedPtr diagnostics_sub; rclcpp::Subscription::SharedPtr current_enu_sub; - rclcpp::TimerBase::SharedPtr heartbeat_timer_; rclcpp::Subscription::SharedPtr log_control_sub_; BagLogger::BagLoggerLevel log_level_; @@ -126,12 +124,14 @@ class AMSuper : AMLifeCycle #endif public: - AMSuper(rclcpp::Node::SharedPtr nh) : nh_(nh), AMLifeCycle(nh), node_mediator_(nh, SuperNodeMediator::nodeNameStripped(nh->get_name())) + AMSuper() : node_mediator_(am::Node::node, SuperNodeMediator::nodeNameStripped(am::Node::node->get_name())) { - RCLCPP_INFO_STREAM(nh_->get_logger(), nh_->get_name()); + ROS_INFO_STREAM( am::Node::node->get_name()); + + life_cycle_node_ = std::static_pointer_cast(am::Node::node); am::getParam("node_timeout_s", node_timeout_s_, 2.0); - RCLCPP_INFO_STREAM(nh_->get_logger(), "node_timeout_s = " << node_timeout_s_); + ROS_INFO_STREAM( "node_timeout_s = " << node_timeout_s_); /* * create initial node list from manifest and create babysitters as needed @@ -149,13 +149,13 @@ class AMSuper : AMLifeCycle // if a manifest has been specified if (!supervisor_.manifest.empty()) { - RCLCPP_INFO_STREAM(nh_->get_logger(), "configuring nodes from manifest: " << manifest_param); + ROS_INFO_STREAM( "configuring nodes from manifest: " << manifest_param); for (string& name : supervisor_.manifest) { // create a new node in the list for each name in manifest SuperNodeMediator::SuperNodeInfo nr = node_mediator_.initializeManifestedNode(name); supervisor_.nodes.insert(pair(name, nr)); - RCLCPP_INFO_STREAM(nh_->get_logger(), " " << name); + ROS_INFO_STREAM( " " << name); // create babysitters based on hard coded node names if (!name.compare(NODE_BS_ALTIMETER)) @@ -163,20 +163,20 @@ class AMSuper : AMLifeCycle int altimeter_warn_ms, altimeter_error_ms; calcBSTiming(ALTIMETER_HZ, altimeter_warn_ms, altimeter_error_ms); altimeter_bs_ = new am::BabySitter( - nh_, BagLogger::instance(), name, ALTIMETER_BS_TOPIC, altimeter_warn_ms, altimeter_error_ms); + am::Node::node, BagLogger::instance(), name, ALTIMETER_BS_TOPIC, altimeter_warn_ms, altimeter_error_ms); } else if (!name.compare(NODE_BS_DJI)) { int dji_warn_ms, dji_error_ms; calcBSTiming(DJI_HZ, dji_warn_ms, dji_error_ms); - dji_bs_ = new am::BabySitter(nh_, BagLogger::instance(), name, DJI_BS_TOPIC, dji_warn_ms, + dji_bs_ = new am::BabySitter(am::Node::node, BagLogger::instance(), name, DJI_BS_TOPIC, dji_warn_ms, dji_error_ms); } } } else { - RCLCPP_WARN_STREAM(nh_->get_logger(), "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(); @@ -189,22 +189,22 @@ class AMSuper : AMLifeCycle /** * system status pub */ - vstate_summary_pub_ = nh_->create_publisher(am_super_topics::SUPER_STATE, 1000); - system_state_pub_ = nh_->create_publisher(am_topics::SYSTEM_STATE, 1000); + vstate_summary_pub_ = am::Node::node->create_publisher(am_super_topics::SUPER_STATE, 1000); + system_state_pub_ = am::Node::node->create_publisher(am_topics::SYSTEM_STATE, 1000); /**Super * node lifecycle state pub. used to tell nodes to change their lifecycle state. */ - lifecycle_pub_ = nh_->create_publisher(am_super_topics::NODE_LIFECYCLE, 100); + lifecycle_pub_ = am::Node::node->create_publisher(am_super_topics::NODE_LIFECYCLE, 100); /** * led control pub */ - led_pub_ = nh_->create_publisher(am::am_topics::LED_BLINK, 1000); + led_pub_ = am::Node::node->create_publisher(am::am_topics::LED_BLINK, 1000); /** * super status contains online naode list for gcs_comms */ - super_status_pub_ = nh_->create_publisher(am_super_topics::SUPER_STATUS, 1000); + super_status_pub_ = am::Node::node->create_publisher(am_super_topics::SUPER_STATUS, 1000); - flight_plan_deactivation_pub_ = nh_->create_publisher(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, 1000); + flight_plan_deactivation_pub_ = am::Node::node->create_publisher(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, 1000); supervisor_.system_state = SuperState::BOOTING; supervisor_.flt_ctrl_state = SuperNodeMediator::SuperFltCtrlState::INIT; @@ -212,11 +212,11 @@ class AMSuper : AMLifeCycle /** * amros log control */ - log_control_sub_ = nh_->create_subscription(am::am_topics::CTRL_LOG_CONTROL, 10, + log_control_sub_ = am::Node::node->create_subscription(am::am_topics::CTRL_LOG_CONTROL, 10, std::bind(&AMSuper::logControlCB, this, std::placeholders::_1)); // startup bagfile - gets closed after frist log control command - RCLCPP_INFO_STREAM(nh_->get_logger(), "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); @@ -225,26 +225,24 @@ class AMSuper : AMLifeCycle /** * node status via LifeCycle */ - node_state_sub_ = nh_->create_subscription(am_super_topics::LIFECYCLE_STATE, 100, + node_state_sub_ = am::Node::node->create_subscription(am_super_topics::LIFECYCLE_STATE, 100, std::bind(&AMSuper::nodeStateCB, this, std::placeholders::_1)); /** * commands from operator */ - operator_command_sub_ = nh_->create_subscription(am_super_topics::OPERATOR_COMMAND, 100, + operator_command_sub_ = am::Node::node->create_subscription(am_super_topics::OPERATOR_COMMAND, 100, std::bind(&AMSuper::operatorCommandCB, this, std::placeholders::_1)); - controller_state_sub = nh_->create_subscription(am_super_topics::CONTROLLER_STATE, 100, + controller_state_sub = am::Node::node->create_subscription(am_super_topics::CONTROLLER_STATE, 100, std::bind(&AMSuper::controllerStateCB, this, std::placeholders::_1)); - diagnostics_sub = nh_->create_subscription("/diagnostics", 100, + diagnostics_sub = am::Node::node->create_subscription("/diagnostics", 100, std::bind(&AMSuper::diagnosticsCB, this, std::placeholders::_1)); - current_enu_sub = nh_->create_subscription(am_topics::CTRL_VX_VEHICLE_CURRENTENU, 100, + current_enu_sub = am::Node::node->create_subscription(am_topics::CTRL_VX_VEHICLE_CURRENTENU, 100, std::bind(&AMSuper::currentENUCB, this, std::placeholders::_1)); - - heartbeat_timer_ = nh_->create_wall_timer(am::toDuration(1.0), std::bind(&AMSuper::heartbeatCB, this)); - } + } ~AMSuper() { @@ -285,7 +283,7 @@ class AMSuper : AMLifeCycle { //const brain_box_msgs::ControllerState::ConstPtr& rmsg = event.getMessage(); - RCLCPP_INFO(nh_->get_logger(), "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); } @@ -294,7 +292,7 @@ class AMSuper : AMLifeCycle { //const brain_box_msgs::OperatorCommand::ConstPtr& rmsg = event.getMessage(); - RCLCPP_INFO(nh_->get_logger(), "Received Operator Command: %s sent '%i'",rmsg->node_name.c_str(),rmsg->command ); + RCLCPP_INFO(am::Node::node->get_logger(), "Received Operator Command: %s sent '%i'",rmsg->node_name.c_str(),rmsg->command ); node_mediator_.setOperatorCommand(supervisor_, (OperatorCommand)rmsg->command); // TODO: topic name should come from vb_util_lib::topics. @@ -327,25 +325,25 @@ class AMSuper : AMLifeCycle SuperNodeMediator::SuperNodeInfo& nr = it->second; if (!nr.online) { - RCLCPP_INFO_STREAM(nh_->get_logger(), "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) { - RCLCPP_INFO_STREAM(nh_->get_logger(), 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) { - RCLCPP_INFO_STREAM(nh_->get_logger(), node_name << " changed status to = " << life_cycle_mediator_.statusToString(status) << " [09SI]"); + ROS_INFO_STREAM( node_name << " changed status to = " << life_cycle_mediator_.statusToString(status) << " [09SI]"); nr.status = status; nodes_changed = true; if(nr.manifested && nr.status == LifeCycleStatus::ERROR) { supervisor_.status_error = true; - RCLCPP_INFO_STREAM(nh_->get_logger(), "Manifested node " << nr.name << " changed status to ERROR. Shutting down nodes... [JHRE]"); + ROS_INFO_STREAM( "Manifested node " << nr.name << " changed status to ERROR. Shutting down nodes... [JHRE]"); stopFlightPlan(); } } @@ -354,11 +352,11 @@ class AMSuper : AMLifeCycle //process id = 0 observed to be a node coming online. -1 appears to be offline if(pid == 0) { - RCLCPP_INFO_STREAM(nh_->get_logger(), node_name << " process is alive [UIRE]"); + ROS_INFO_STREAM( node_name << " process is alive [UIRE]"); } else { - RCLCPP_INFO_STREAM(nh_->get_logger(), 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; @@ -368,7 +366,7 @@ class AMSuper : AMLifeCycle else { // if we get here, the node is not in the manifest and we've never heard from it before - RCLCPP_WARN_STREAM(nh_->get_logger(), "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; @@ -404,110 +402,13 @@ class AMSuper : AMLifeCycle } if (flt_ctrl_state_changed) { - RCLCPP_INFO_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 1.0, "flight status: " << value); + RCLCPP_INFO_STREAM_THROTTLE(am::Node::node->get_logger(), *am::Node::node->get_clock(), 1.0, "flight status: " << value); checkForSystemStateTransition(); } } } - /** - * called once per second. - * - * times out nodes that haven't been heard from recently. reports on status to bag and trace logs. - */ - void heartbeatCB() override - { -#if CUDA_FLAG - gpu_info_->display(); -#endif - - //publish deprecated topic - { - brain_box_msgs::msg::VxState state_msg; - state_msg.state = (uint8_t)supervisor_.system_state; - vstate_summary_pub_->publish(state_msg); - } - - //publish the system state - { - brain_box_msgs::msg::SystemState system_state_msg; - system_state_msg.state = (uint8_t)supervisor_.system_state; - system_state_msg.state_string = state_mediator_.stateToString(supervisor_.system_state); - system_state_pub_->publish(system_state_msg); - } - - // cycle thru all the nodes in the list to look for a timeout - rclcpp::Time now = nh_->now(); - map::iterator it; - for (it = supervisor_.nodes.begin(); it != supervisor_.nodes.end(); it++) - { - SuperNodeMediator::SuperNodeInfo& nr = (*it).second; - if (nr.online) - { - rclcpp::Duration time_since_contact = (now - nr.last_contact); - rclcpp::Duration timeout_dur(am::toDuration(node_timeout_s_)); - if (time_since_contact > timeout_dur) - { - nr.online = false; - RCLCPP_ERROR_STREAM(nh_->get_logger(),"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::msg::Super2Status status_msg; - status_msg.man = supervisor_.manifest.size(); - status_msg.man_run = num_manifest_nodes_online; - status_msg.run = node_mediator_.nodesOnlineCount(supervisor_); - - 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_->get_subscription_count() > 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 - RCLCPP_ERROR_STREAM(nh_->get_logger(),ss.str()); - RCLCPP_ERROR_STREAM(nh_->get_logger(),"not online: " << node_mediator_.manifestedNodesNotOnlineNamesList(supervisor_)); - } - else - { - // if all manifested nodes are running, report as info - RCLCPP_INFO_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), LOG_THROTTLE_S, ss.str()); - } - - // log stats - fstream newfile; - newfile.open("/sys/bus/i2c/devices/7-0040/iio_device/in_power0_input",ios::in); //open a file to perform read operation using file object - if (newfile.is_open()) - { //checking whether the file is open - string tp; - getline(newfile, tp); - std_msgs::msg::Int16 msg; - msg.data = std::stoi(tp); - LOG_MSG("/watts", msg, SU_LOG_LEVEL); - newfile.close(); //close the file object. - } - - AMLifeCycle::heartbeatCB(); - } - + /** * update stream with system state and status */ @@ -527,7 +428,7 @@ class AMSuper : AMLifeCycle { std::stringstream ss; genSystemState(ss); - RCLCPP_INFO_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), LOG_THROTTLE_S, ss.str()); + ROS_INFO_STREAM_THROTTLE(LOG_THROTTLE_S, ss.str()); } /** @@ -539,7 +440,7 @@ class AMSuper : AMLifeCycle */ void sendLifeCycleCommand(const std::string_view& node_name, const LifeCycleCommand command) { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "sending command: " << life_cycle_mediator_.commandToString(command) << " to " << node_name << " lifecycle"); + ROS_DEBUG_STREAM("sending command: " << life_cycle_mediator_.commandToString(command) << " to " << node_name << " lifecycle"); brain_box_msgs::msg::LifeCycleCommand msg; msg.node_name = node_name; msg.command = (brain_box_msgs::msg::LifeCycleCommand::_command_type)command; @@ -565,7 +466,7 @@ class AMSuper : AMLifeCycle { for (const auto & [ node_name, error_message ] : result.second) { - RCLCPP_WARN_STREAM(nh_->get_logger(),error_message); + ROS_WARN_STREAM(error_message); } } return success; @@ -578,7 +479,7 @@ class AMSuper : AMLifeCycle std_msgs::msg::Bool msg; msg.data = false; //false means deactivate flight_plan_deactivation_pub_->publish(msg); - RCLCPP_ERROR_STREAM(nh_->get_logger(), "Sending flight plan kill command."); + ROS_ERROR_STREAM( "Sending flight plan kill command."); } /** @@ -589,7 +490,7 @@ class AMSuper : AMLifeCycle */ void checkForSystemStateTransition() { - if(getState() == LifeCycleState::INACTIVE && supervisor_.system_state == SuperState::READY) //if super lifecycle is currently inactive + if(life_cycle_node_->getState() == LifeCycleState::INACTIVE && supervisor_.system_state == SuperState::READY) //if super lifecycle is currently inactive { sendLifeCycleCommand(node_mediator_.getNodeName(), LifeCycleCommand::ACTIVATE); } @@ -607,7 +508,7 @@ class AMSuper : AMLifeCycle LifeCycleCommand command = transition_instructions.life_cycle_command; std::string failed_nodes_string = boost::algorithm::join(transition_instructions.failed_nodes, ", "); std::string failed_nodes_reasons_string = boost::algorithm::join(transition_instructions.failed_nodes_reasons, ", "); - RCLCPP_INFO_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 5,state_mediator_.stateToString(supervisor_.system_state) + 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); @@ -628,7 +529,7 @@ class AMSuper : AMLifeCycle */ void setSystemState(SuperState state) { - RCLCPP_INFO_STREAM(nh_->get_logger(),state_mediator_.stateToString(supervisor_.system_state) << " --> " + ROS_INFO_STREAM(state_mediator_.stateToString(supervisor_.system_state) << " --> " << state_mediator_.stateToString(state)); bool legal = true; if(!node_mediator_.forceTransition(state)) @@ -636,7 +537,7 @@ class AMSuper : AMLifeCycle if (!legal) { - RCLCPP_ERROR_STREAM(nh_->get_logger(),"illegal state transition from " << state_mediator_.stateToString(supervisor_.system_state) + ROS_ERROR_STREAM("illegal state transition from " << state_mediator_.stateToString(supervisor_.system_state) << " to " << state_mediator_.stateToString(state)); } else @@ -655,33 +556,6 @@ class AMSuper : AMLifeCycle } } - /** - * 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); - RCLCPP_WARN_STREAM(nh_->get_logger(),"required" << required_platform.maker); - RCLCPP_WARN_STREAM(nh_->get_logger(),"actual" << actual_platform.maker); - if(!node_mediator_.isCorrectPlatform(required_platform,actual_platform)) - { - std::stringstream message; - message << "Platform required: `" - << node_mediator_.platformVariantToConfig(required_platform) - << "` actual: `" - << node_mediator_.platformVariantToConfig(actual_platform) - ; - errorTerminal(message.str(),"NSK2"); //force failure since this is not recoverable - } - else - { - AMLifeCycle::onConfigure(); - } - } /** load the platform configurations from the launch file and populate the variants provided. */ @@ -691,10 +565,10 @@ class AMSuper : AMLifeCycle //actual platform is required or we fail std::string not_provided = "none"; std::string actual_platform_param; - param("platform/actual",actual_platform_param,not_provided); + life_cycle_node_->param("platform.actual",actual_platform_param,not_provided); if(actual_platform_param == not_provided) { - errorTerminal("param `/am_super/platform/actual` must provide the platform running","NNS9"); + life_cycle_node_->errorTerminal("param `am_super.platform.actual` must provide the platform running","NNS9"); return; } node_mediator_.platformConfigToVariant(actual_platform_param,actual_platform); @@ -702,8 +576,8 @@ class AMSuper : AMLifeCycle //compare actual platform to required platform, if provided std::string required_platform_param; std::string platform_app_required_param; - param("platform/required",required_platform_param,not_provided); - param("platform/app/required",platform_app_required_param,not_provided); + life_cycle_node_->param("platform/required",required_platform_param,not_provided); + life_cycle_node_->param("platform/app/required",platform_app_required_param,not_provided); if(required_platform_param != not_provided) { node_mediator_.platformConfigToVariant(required_platform_param,required_platform); @@ -714,7 +588,7 @@ class AMSuper : AMLifeCycle } else { - RCLCPP_WARN(nh_->get_logger(),"platform requirements not set"); + ROS_WARN("platform requirements not set"); } } @@ -865,30 +739,191 @@ class AMSuper : AMLifeCycle { if (msg->enable) { - RCLCPP_INFO_STREAM(nh_->get_logger(),"stop logging"); + ROS_INFO_STREAM("stop logging"); BagLogger::instance()->stopLogging(); - RCLCPP_INFO_STREAM(nh_->get_logger(),"start logging to SU, level " << SU_LOG_LEVEL); + ROS_INFO_STREAM("start logging to SU, level " << SU_LOG_LEVEL); BagLogger::instance()->startLogging("SU", SU_LOG_LEVEL); log_level_ = intToLoggerLevel (SU_LOG_LEVEL); } } }; + +class AMSuperNode : public AMLifeCycle +{ +private: + shared_ptr am_super_; + +public: + AMSuperNode(const std::string & node_name) : AMLifeCycle(node_name) + { + } + + ~AMSuperNode() + { + } + + void setAMSuper(shared_ptr am_super) + { + am_super_= am_super; + } + +/** + * Verify the basic requirements are being met: + * - platform required matches actual platform + */ + void onConfigure() override + { + if(am_super_ == nullptr) + { + AMLifeCycle::onConfigure(); + return; + } + + SuperNodeMediator::PlatformVariant required_platform; + SuperNodeMediator::PlatformVariant actual_platform; + am_super_->configurePlatformRequirements(required_platform, actual_platform); + ROS_WARN_STREAM("required" << required_platform.maker); + ROS_WARN_STREAM("actual" << actual_platform.maker); + if(!am_super_->node_mediator_.isCorrectPlatform(required_platform,actual_platform)) + { + std::stringstream message; + message << "Platform required: `" + << am_super_->node_mediator_.platformVariantToConfig(required_platform) + << "` actual: `" + << am_super_->node_mediator_.platformVariantToConfig(actual_platform) + ; + errorTerminal(message.str(),"NSK2"); //force failure since this is not recoverable + } + else + { + AMLifeCycle::onConfigure(); + } + } + + /** + * called once per second. + * + * times out nodes that haven't been heard from recently. reports on status to bag and trace logs. + */ + void heartbeatCB() override + { + if(am_super_ == nullptr) + { + AMLifeCycle::heartbeatCB(); + return; + } + +#if CUDA_FLAG + gpu_info_->display(); +#endif + + //publish deprecated topic + { + brain_box_msgs::msg::VxState state_msg; + state_msg.state = (uint8_t)am_super_->supervisor_.system_state; + am_super_->vstate_summary_pub_->publish(state_msg); + } + + //publish the system state + { + brain_box_msgs::msg::SystemState system_state_msg; + system_state_msg.state = (uint8_t)am_super_->supervisor_.system_state; + system_state_msg.state_string = am_super_->state_mediator_.stateToString(am_super_->supervisor_.system_state); + am_super_->system_state_pub_->publish(system_state_msg); + } + + // cycle thru all the nodes in the list to look for a timeout + rclcpp::Time now = am::Node::node->now(); + map::iterator it; + for (it = am_super_->supervisor_.nodes.begin(); it != am_super_->supervisor_.nodes.end(); it++) + { + SuperNodeMediator::SuperNodeInfo& nr = (*it).second; + if (nr.online) + { + rclcpp::Duration time_since_contact = (now - nr.last_contact); + rclcpp::Duration timeout_dur(am::toDuration(am_super_->node_timeout_s_)); + if (time_since_contact > timeout_dur) + { + nr.online = false; + ROS_ERROR_STREAM("node timed out:" << nr.name); + am_super_->reportSystemState(); + } + } + } + + // check for state transition due to timeouts or anything else that changed since last heartbeat + am_super_->checkForSystemStateTransition(); + + int num_manifest_nodes_online = am_super_->node_mediator_.manifestedNodesOnlineCount(am_super_->supervisor_); + // publish and bag log super status message + brain_box_msgs::msg::Super2Status status_msg; + status_msg.man = am_super_->supervisor_.manifest.size(); + status_msg.man_run = num_manifest_nodes_online; + status_msg.run = am_super_->node_mediator_.nodesOnlineCount(am_super_->supervisor_); + + for (it = am_super_->supervisor_.nodes.begin(); it != am_super_->supervisor_.nodes.end(); it++) + { + SuperNodeMediator::SuperNodeInfo& nr = (*it).second; + status_msg.nodes.push_back(nr.name); + } + LOG_MSG("/status/super", status_msg, 1); + if (am_super_->super_status_pub_->get_subscription_count() > 0) + { + am_super_->super_status_pub_->publish(status_msg); + } + + // report current status to trace log + std::stringstream ss; + am_super_->genSystemState(ss); + + if (am_super_->supervisor_.manifest.size() != num_manifest_nodes_online) + { + // if all manifested nodes aren't running, report as error + ROS_ERROR_STREAM(ss.str()); + ROS_ERROR_STREAM("not online: " << am_super_->node_mediator_.manifestedNodesNotOnlineNamesList(am_super_->supervisor_)); + } + else + { + // if all manifested nodes are running, report as info + ROS_INFO_STREAM_THROTTLE(am_super_->LOG_THROTTLE_S, ss.str()); + } + + // log stats + fstream newfile; + newfile.open("/sys/bus/i2c/devices/7-0040/iio_device/in_power0_input",ios::in); //open a file to perform read operation using file object + if (newfile.is_open()) + { //checking whether the file is open + string tp; + getline(newfile, tp); + std_msgs::msg::Int16 msg; + msg.data = std::stoi(tp); + LOG_MSG("/watts", msg, am_super_->SU_LOG_LEVEL); + newfile.close(); //close the file object. + } + + AMLifeCycle::heartbeatCB(); + } + }; +}; // namespace + // #ifdef TESTING // #else -std::shared_ptr am::Node::node; +shared_ptr am::Node::node; int main(int argc, char** argv) { rclcpp::init(argc, argv); - am::Node::node = std::make_shared("am_super"); + shared_ptr am_super_node = make_shared("am_super"); + am::Node::node = am_super_node; - std::shared_ptr am_super_node = std::make_shared(am::Node::node); + std::shared_ptr am_super = make_shared(); + am_super_node->setAMSuper(am_super); ROS_INFO_STREAM(am::Node::node->get_name() << ": running..."); diff --git a/src/super_lib/am_life_cycle.cpp b/src/super_lib/am_life_cycle.cpp index 4058bf09..62efb351 100644 --- a/src/super_lib/am_life_cycle.cpp +++ b/src/super_lib/am_life_cycle.cpp @@ -11,7 +11,23 @@ namespace am // static constexpr std::string AMLifeCycle::STATE_INVALID_STRING; // static constexpr std::string AMLifeCycle::STATE_UNCONFIGURED_STRING; -AMLifeCycle::AMLifeCycle(rclcpp::Node::SharedPtr node) : node_(node), updater_(node) +AMLifeCycle::AMLifeCycle(const std::string & node_name, const rclcpp::NodeOptions & options ) : + rclcpp::Node(node_name, options), updater_(this) +{ + initialize(); +}; + +AMLifeCycle::AMLifeCycle(const std::string & node_name, const std::string & namespace_, const rclcpp::NodeOptions & options ) : + rclcpp::Node(node_name, namespace_, options), updater_(this) +{ + initialize(); +} + +AMLifeCycle::~AMLifeCycle() +{ +} + +void AMLifeCycle::initialize() { std::string init_state_str; //FIXME: This string should come from the enum @@ -38,22 +54,23 @@ AMLifeCycle::AMLifeCycle(rclcpp::Node::SharedPtr node) : node_(node), updater_(n life_cycle_info_.state = LifeCycleState::ACTIVE; } life_cycle_info_.status = LifeCycleStatus::OK; - state_pub_ = node_->create_publisher("/node_state", 100); + state_pub_ = create_publisher("/node_state", 100); + // updater_ = std::make_shared(this); updater_.setHardwareID("none"); updater_.broadcast(0, "Initializing node"); updater_.add("diagnostics", this, &AMLifeCycle::addStatistics); - updater_.force_update(); + // updater_.force_update(); // strip leading '/' if it is there // TODO: this might always be there so just strip it without checking? - if (std::string(node_->get_name()).at(0) == '/') + if (std::string(get_name()).at(0) == '/') { - node_name_ = std::string(node_->get_name()).substr(1); + node_name_ = std::string(get_name()).substr(1); } else { - node_name_ = node_->get_name(); + node_name_ = get_name(); } @@ -62,30 +79,33 @@ AMLifeCycle::AMLifeCycle(rclcpp::Node::SharedPtr node) : node_(node), updater_(n /** * node status via LifeCycle */ - lifecycle_sub_ = node_->create_subscription("/node_lifecycle", 100, + lifecycle_sub_ = create_subscription("/node_lifecycle", 100, std::bind(&AMLifeCycle::lifecycleCB, this, std::placeholders::_1)); - heartbeat_timer_ = node_->create_wall_timer(am::toDuration(1.0), std::bind(&AMLifeCycle::heartbeatCB, this)); - -} + heartbeat_timer_ = create_wall_timer(am::toDuration(1.0), std::bind(&AMLifeCycle::heartbeatCB, this)); -AMLifeCycle::~AMLifeCycle() -{ } template -bool AMLifeCycle::param(const std::string& param_name, T& param_val, const T& default_val) const +bool AMLifeCycle::param(const std::string& param_name, T& param_val, const T& default_val) { - //todo: fix the parameter - //bool result = nh_.param(param_name, param_val, default_val); - bool result = true; - RCLCPP_INFO_STREAM(node_->get_logger(), param_name << " = " << param_val); + // can't use am:Node calls here because we are in constructor + try + { + declare_parameter(param_name, default_val); + } + catch(rclcpp::exceptions::ParameterAlreadyDeclaredException &e) + { + ; + } + bool result = get_parameter_or(param_name, param_val, default_val); + RCLCPP_INFO_STREAM(get_logger(), param_name << " = " << param_val); return result; } void AMLifeCycle::lifecycleCB(const brain_box_msgs::msg::LifeCycleCommand::SharedPtr msg) { - RCLCPP_DEBUG_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 1.0, life_cycle_mediator_.commandToString((LifeCycleCommand)msg->command)); + 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_)) { @@ -104,7 +124,7 @@ void AMLifeCycle::lifecycleCB(const brain_box_msgs::msg::LifeCycleCommand::Share configure(); break; case LifeCycleCommand::CREATE: - RCLCPP_WARN_STREAM(node_->get_logger(),"illegal command " << life_cycle_mediator_.commandToString(LifeCycleCommand::CREATE) << " [7YT8]"); + ROS_WARN_STREAM("illegal command " << life_cycle_mediator_.commandToString(LifeCycleCommand::CREATE) << " [7YT8]"); break; case LifeCycleCommand::DEACTIVATE: transition("deactivate", LifeCycleState::ACTIVE, LifeCycleState::DEACTIVATING, LifeCycleState::INACTIVE, @@ -125,17 +145,17 @@ void AMLifeCycle::transition(std::string transition_name, LifeCycleState initial { if (life_cycle_info_.state == initial_state) { - RCLCPP_INFO_STREAM(node_->get_logger(), transition_name << ", current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [ASWU]"); + 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) { - RCLCPP_DEBUG_STREAM(node_->get_logger(),"ignoring redundant " << transition_name << " [0393]"); + ROS_DEBUG_STREAM("ignoring redundant " << transition_name << " [0393]"); } else { - RCLCPP_WARN_STREAM(node_->get_logger(),"received illegal " << transition_name << " in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [JGV5]"); + ROS_WARN_STREAM("received illegal " << transition_name << " in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [JGV5]"); } } @@ -145,12 +165,12 @@ void AMLifeCycle::doTransition(std::string transition_name, bool success, LifeCy logState(); if (success) { - RCLCPP_INFO_STREAM(node_->get_logger(), transition_name << " succeeded"); + ROS_INFO_STREAM(transition_name << " succeeded"); setState(success_state); } else { - RCLCPP_INFO_STREAM(node_->get_logger(),transition_name << " failed"); + ROS_INFO_STREAM(transition_name << " failed"); setState(failure_state); } } @@ -181,7 +201,7 @@ void AMLifeCycle::doCleanup(bool success) void AMLifeCycle::onConfigure() { - RCLCPP_INFO(node_->get_logger(),"onConfigure called [POMH]"); + ROS_INFO("onConfigure called [POMH]"); if(stats_list_.hasStats()) { LifeCycleStatus status = stats_list_.process(throttle_info_.warn_throttle_s, throttle_info_.error_throttle_s); @@ -191,7 +211,7 @@ void AMLifeCycle::onConfigure() } else if (!withinConfigureTolerance()) { - RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(),5, stats_list_.getStatsStr() << " blocked by stats past configure tolerance with status " << life_cycle_mediator_.statusToString(status) ); + 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 @@ -215,7 +235,7 @@ void AMLifeCycle::onDeactivate() void AMLifeCycle::logState() { - RCLCPP_INFO_STREAM(node_->get_logger(),"LifeCycle: " << life_cycle_mediator_.stateToString(life_cycle_info_.state)); + ROS_INFO_STREAM("LifeCycle: " << life_cycle_mediator_.stateToString(life_cycle_info_.state)); } void AMLifeCycle::doDeactivate(bool success) @@ -228,7 +248,7 @@ void AMLifeCycle::configure() //mark the configuration start time once if(getState() != LifeCycleState::CONFIGURING) { - configure_start_time_=node_->now(); + configure_start_time_= am::ClockNow(); } transition("configure", LifeCycleState::UNCONFIGURED, LifeCycleState::CONFIGURING, LifeCycleState::INACTIVE, std::bind(&AMLifeCycle::onConfigure, this)); @@ -238,12 +258,12 @@ void AMLifeCycle::destroy() { if (life_cycle_mediator_.illegalDestroy(life_cycle_info_)) { - RCLCPP_INFO_STREAM(node_->get_logger(),"received illegal activate in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [45RT]"); + 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 { - RCLCPP_INFO_STREAM(node_->get_logger(),"current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [RE45]"); + ROS_INFO_STREAM("current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [RE45]"); onDestroy(); } } @@ -266,7 +286,7 @@ bool AMLifeCycle::withinConfigureTolerance() //outside of configuring, we have no tolerance if(life_cycle_mediator_.unconfigured(life_cycle_info_)) { - rclcpp::Duration duration_since_configure = node_->now() - configure_start_time_; + rclcpp::Duration duration_since_configure = am::ClockNow() - configure_start_time_; if (life_cycle_info_.state == LifeCycleState::UNCONFIGURED || duration_since_configure <= rclcpp::Duration(am::toDuration(configure_tolerance_s)) ) { tolerated = true; @@ -285,7 +305,7 @@ void AMLifeCycle::error(std::string message, std::string error_code, bool forced std::string error_code_message = "Error[" + error_code + "] "; if(withinConfigureTolerance() && !forced) { - RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(),throttle_info_.warn_throttle_s,"Ignoring tolerant error for (" << configure_tolerance_s << "s) `" << message << "` " << error_code_message << "[GFRT]"); + ROS_WARN_STREAM_THROTTLE(throttle_info_.warn_throttle_s,"Ignoring tolerant error for (" << configure_tolerance_s << "s) `" << message << "` " << error_code_message << "[GFRT]"); } else { @@ -303,7 +323,7 @@ void AMLifeCycle::error(std::string message, std::string error_code, bool forced repeat_prefix = "Repeated "; } std::string error_explanation=forced_prefix + repeat_prefix + error_code_message; - RCLCPP_ERROR_STREAM(node_->get_logger(), message << " -> " << error_explanation << " [R45Y]" ); + ROS_ERROR_STREAM(message << " -> " << error_explanation << " [R45Y]" ); } } @@ -342,17 +362,17 @@ void AMLifeCycle::shutdown() { if (life_cycle_mediator_.shutdown(life_cycle_info_)) { - RCLCPP_INFO_STREAM(node_->get_logger(),"current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state)); + 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_)) { - RCLCPP_DEBUG_STREAM(node_->get_logger(),"ignoring redundant shutdown"); + ROS_DEBUG_STREAM("ignoring redundant shutdown"); } else { - RCLCPP_INFO_STREAM(node_->get_logger(),"received illegal activate in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state)); + ROS_INFO_STREAM("received illegal activate in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state)); } } @@ -469,8 +489,8 @@ AMStatReset& AMLifeCycle::configureHzStats(AMStatReset& stats) void AMLifeCycle::sendNodeUpdate() { brain_box_msgs::msg::LifeCycleState msg; - msg.header.stamp = node_->now(); - msg.node_name = node_->get_name(); + msg.header.stamp = am::ClockNow(); + msg.node_name = get_name(); msg.process_id = 0; msg.state = (uint8_t)life_cycle_info_.state; msg.status = (uint8_t)life_cycle_info_.status; @@ -489,7 +509,7 @@ void AMLifeCycle::heartbeatCB() << stats_list_.getStatsStrShort(); double throttle_s = getThrottle(); - RCLCPP_INFO_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), throttle_s, "LifeCycle heartbeat: " << ss.str()); + ROS_INFO_STREAM_THROTTLE( throttle_s, "LifeCycle heartbeat: " << ss.str()); stats_list_.reset(); @@ -512,12 +532,12 @@ void AMLifeCycle::setState(const LifeCycleState state) if (life_cycle_mediator_.setState(state, life_cycle_info_)) { - RCLCPP_INFO_STREAM(node_->get_logger(), "changing state from " << life_cycle_mediator_.stateToString(initial_state) << " to " << life_cycle_mediator_.stateToString(state)); + ROS_INFO_STREAM("changing state from " << life_cycle_mediator_.stateToString(initial_state) << " to " << life_cycle_mediator_.stateToString(state)); sendNodeUpdate(); } else { - RCLCPP_ERROR_STREAM(node_->get_logger(), "illegal state: " << (int)state); + ROS_ERROR_STREAM("illegal state: " << (int)state); } } @@ -531,7 +551,7 @@ bool AMLifeCycle::setStatus(const LifeCycleStatus status) //if we are in error and want to leave it if(life_cycle_info_.status == LifeCycleStatus::ERROR && status != LifeCycleStatus::ERROR) { - RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), getThrottle(), "requested to change status from ERROR to " << life_cycle_mediator_.statusToString(status) << " [DFRE]"); + 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_)) { @@ -540,7 +560,7 @@ bool AMLifeCycle::setStatus(const LifeCycleStatus status) else { - RCLCPP_ERROR_STREAM(node_->get_logger(), "illegal status: " << life_cycle_mediator_.statusToString(status)); + ROS_ERROR_STREAM("illegal status: " << life_cycle_mediator_.statusToString(status)); } return true; diff --git a/src/super_lib/am_super_test.cpp b/src/super_lib/am_super_test.cpp index 786eb7a4..1f3b735b 100644 --- a/src/super_lib/am_super_test.cpp +++ b/src/super_lib/am_super_test.cpp @@ -3,6 +3,8 @@ #include +std::shared_ptr am::Node::node; + AMSuperTest::AMSuperTest(string target_node_name) : nh_(std::make_shared(target_node_name)) { createPubsSubs(); From ce8dca83d4524a5966a0bd9daeec4e0242550d3e Mon Sep 17 00:00:00 2001 From: danhennage Date: Tue, 24 Jan 2023 13:31:29 -0800 Subject: [PATCH 10/45] feat: convert am::Node to am::AMLifeCycle --- CMakeLists.txt | 32 +- include/super_lib/am_life_cycle.h | 252 --------- include/super_lib/am_life_cycle_mediator.h | 250 --------- include/super_lib/am_life_cycle_types.h | 56 -- include/super_lib/am_stat.h | 298 ----------- include/super_lib/am_stat_ave.h | 234 -------- include/super_lib/am_stat_list.h | 85 --- include/super_lib/am_stat_reset.h | 60 --- include/super_lib/am_super_test.h | 160 ------ include/super_lib/am_super_topics.h | 31 -- src/am_super/am_super.cpp | 8 +- src/super_lib/am_life_cycle.cpp | 588 --------------------- src/super_lib/am_life_cycle_mediator.cpp | 237 --------- src/super_lib/am_super_test.cpp | 229 -------- 14 files changed, 7 insertions(+), 2513 deletions(-) delete mode 100644 include/super_lib/am_life_cycle.h delete mode 100644 include/super_lib/am_life_cycle_mediator.h delete mode 100644 include/super_lib/am_life_cycle_types.h delete mode 100644 include/super_lib/am_stat.h delete mode 100644 include/super_lib/am_stat_ave.h delete mode 100644 include/super_lib/am_stat_list.h delete mode 100644 include/super_lib/am_stat_reset.h delete mode 100644 include/super_lib/am_super_test.h delete mode 100644 include/super_lib/am_super_topics.h delete mode 100644 src/super_lib/am_life_cycle.cpp delete mode 100644 src/super_lib/am_life_cycle_mediator.cpp delete mode 100644 src/super_lib/am_super_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 855be57e..c5334b07 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,13 +17,10 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif() - set(dependencies am_utils brain_box_msgs builtin_interfaces - diagnostic_msgs - diagnostic_updater rclcpp rclpy rosbag2 @@ -42,10 +39,6 @@ include_directories( include ) -file(GLOB super_lib_cpp_files - src/super_lib/*.cpp -) - file(GLOB cuda_cpp_files src/cuda/*.cpp ) @@ -58,33 +51,13 @@ file(GLOB SUPER_MEDIATOR_FILES src/am_super/*_mediator.cpp ) -add_library(super_lib - ${super_lib_cpp_files} -) -target_link_libraries(super_lib) - -ament_target_dependencies(super_lib ${dependencies}) -ament_export_targets(super_libTargets HAS_LIBRARY_TARGET) -ament_export_include_directories(include) -ament_export_libraries(super_lib) - add_executable(am_super ${am_super_cpp_files}) -target_link_libraries(am_super super_lib) ament_target_dependencies(am_super ${dependencies}) install(DIRECTORY include/ DESTINATION include/ ) -install( - TARGETS super_lib - EXPORT super_libTargets - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include -) - install(TARGETS am_super DESTINATION lib/${PROJECT_NAME} @@ -115,12 +88,11 @@ if(BUILD_TESTING) super_test/abort_to_disarming/abort_to_disarming_rostest.cpp ) - target_link_libraries(abort_to_disarming super_lib) - ament_target_dependencies(abort_to_disarming am_utils ${dependencies}) + 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_libraries(super_lib) ament_export_dependencies(${dependencies}) ament_package() diff --git a/include/super_lib/am_life_cycle.h b/include/super_lib/am_life_cycle.h deleted file mode 100644 index 1f03101e..00000000 --- a/include/super_lib/am_life_cycle.h +++ /dev/null @@ -1,252 +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 rclcpp::Node -{ - 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.*/ - rclcpp::Time configure_start_time_; - - /** initialization - called by constructors. - */ - void initialize(); - - 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=""); - - public: - std::string node_name_; - - /**Maximum time errors will be ignored during configuration. */ - int configure_tolerance_s; - - // std::shared_ptr updater_; - diagnostic_updater::Updater updater_; - AMStatList stats_list_; - - rclcpp::TimerBase::SharedPtr heartbeat_timer_; - rclcpp::Publisher::SharedPtr state_pub_; - rclcpp::Subscription::SharedPtr lifecycle_sub_; - - AMLifeCycle( - const std::string & node_name, - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - - AMLifeCycle( - const std::string & node_name, - const std::string & namespace_, - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - - /** - * @brief Virtual destructor - */ - virtual ~AMLifeCycle(); - - /** Exactly like ros::param, but ROS_INFO's level showing the actual value assigned. - */ - template - bool param(const std::string& param_name, T& param_val, const T& default_val); - - //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(); - - void lifecycleCB(const brain_box_msgs::msg::LifeCycleCommand::SharedPtr 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 7d006d77..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::msg::LifeCycleState::STATE_INVALID, - UNCONFIGURED = brain_box_msgs::msg::LifeCycleState::STATE_UNCONFIGURED, - INACTIVE = brain_box_msgs::msg::LifeCycleState::STATE_INACTIVE, - ACTIVE = brain_box_msgs::msg::LifeCycleState::STATE_ACTIVE, - FINALIZED = brain_box_msgs::msg::LifeCycleState::STATE_FINALIZED, - CONFIGURING = brain_box_msgs::msg::LifeCycleState::STATE_CONFIGURING, - CLEANING_UP = brain_box_msgs::msg::LifeCycleState::STATE_CLEANING_UP, - SHUTTING_DOWN = brain_box_msgs::msg::LifeCycleState::STATE_SHUTTING_DOWN, - ACTIVATING = brain_box_msgs::msg::LifeCycleState::STATE_ACTIVATING, - DEACTIVATING = brain_box_msgs::msg::LifeCycleState::STATE_DEACTIVATING, - ERROR_PROCESSING = brain_box_msgs::msg::LifeCycleState::STATE_ERROR_PROCESSING -}; - -/** - * status of the functionality of the node (i.e. is it operating to spec) - */ -enum class LifeCycleStatus : std::uint8_t -{ - OK = brain_box_msgs::msg::LifeCycleState::STATUS_OK, - WARN = brain_box_msgs::msg::LifeCycleState::STATUS_WARN, - ERROR = brain_box_msgs::msg::LifeCycleState::STATUS_ERROR -}; - -/** - * lifecycle commands to nodes to change state - */ -enum class LifeCycleCommand : std::uint8_t -{ - CREATE = brain_box_msgs::msg::LifeCycleCommand::COMMAND_CREATE, - CONFIGURE = brain_box_msgs::msg::LifeCycleCommand::COMMAND_CONFIGURE, - CLEANUP = brain_box_msgs::msg::LifeCycleCommand::COMMAND_CLEANUP, - ACTIVATE = brain_box_msgs::msg::LifeCycleCommand::COMMAND_ACTIVATE, - DEACTIVATE = brain_box_msgs::msg::LifeCycleCommand::COMMAND_DEACTIVATE, - SHUTDOWN = brain_box_msgs::msg::LifeCycleCommand::COMMAND_SHUTDOWN, - DESTROY = brain_box_msgs::msg::LifeCycleCommand::COMMAND_DESTROY, - - //FIXME: there should be no last command - LAST_COMMAND = brain_box_msgs::msg::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 1260f81d..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; - rclcpp::Node::SharedPtr node_; -private: - AMStat(); -public: - AMStat(rclcpp::Node::SharedPtr node, const std::string& short_name, const std::string& long_name): node_(node) - { - short_name_ = short_name; - long_name_ = long_name; - } - - AMStat(rclcpp::Node::SharedPtr node, const std::string& short_name, const std::string& long_name, uint32_t max_warn, uint32_t max_error) - : AMStat(node, short_name, long_name) - { - setMaxWarn(max_warn); - setMaxError(max_error); - } - - AMStat(rclcpp::Node::SharedPtr node, const std::string& short_name, const std::string& long_name, uint32_t min_error, uint32_t min_warn, - uint32_t max_warn, uint32_t max_error) - : AMStat(node, 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_) - { - RCLCPP_ERROR_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(),error_throttle_s, long_name_ << " exceeding max_error: " << value_ - << " (max:" << max_error_ << ") [TF5R]"); - compoundStatus(status, LifeCycleStatus::ERROR); - } - else if (value_ > max_warn_) - { - RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), warn_throttle_s, long_name_ << " exceeding max_warn: " << value_ << " (max:" << max_warn_ - << ") [PO9P]"); - compoundStatus(status, LifeCycleStatus::WARN); - } - } - - if(validate_min_) - { - if (value_ < min_error_) - { - RCLCPP_ERROR_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), error_throttle_s, long_name_ << " exceeding min_error: " << value_ - << " (min:" << min_error_ << ") [K08K]"); - compoundStatus(status, LifeCycleStatus::ERROR); - } - else if (value_ < min_warn_) - { - RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), warn_throttle_s, long_name_ << " exceeding min_warn: " << value_ << " (min:" << min_warn_ - << ") [H9H8]"); - compoundStatus(status, LifeCycleStatus::WARN); - } - } - - if(!validate_max_ && !validate_min_) - { - //report this warning once during first validation - RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 9999, long_name_ << " lacks validation since min/max is not set [TRB5]"); - } - } - else - { - //sample is required and not yet received - status = LifeCycleStatus::ERROR; - RCLCPP_ERROR_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 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 86c8d621..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 dea8827e..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(rclcpp::Node::SharedPtr node, const std::string& short_name, const std::string& long_name) : AMStat(node, short_name, long_name) - { - init(); - } - - AMStatReset(rclcpp::Node::SharedPtr node, const std::string& short_name, const std::string& long_name, uint32_t max_warn, uint32_t max_error) - : AMStat(node, short_name, long_name, max_warn, max_error) - { - init(); - } - - AMStatReset(rclcpp::Node::SharedPtr node, const std::string& short_name, const std::string& long_name, uint32_t min_error, uint32_t min_warn, - uint32_t max_warn, uint32_t max_error) - : AMStat(node, 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_test.h b/include/super_lib/am_super_test.h deleted file mode 100644 index ffd6a346..00000000 --- a/include/super_lib/am_super_test.h +++ /dev/null @@ -1,160 +0,0 @@ -#ifndef _AM_SUPER_SUPER_LIB_AM_SUPER_TEST_H_ -#define _AM_SUPER_SUPER_LIB_AM_SUPER_TEST_H_ - -#include - -#include - -#include // googletest header file - -#include // to be armed, launch for state transitions -#include -#include // msg for status -#include // msg for status -#include -#include -#include -#include -#include - -using namespace std; -using namespace am; - -/** - * Base class for testing the LifeCycle transitions of a target node along with the - * state transition of the supervisor node. - */ -class AMSuperTest : public ::testing::Test -{ -private: - rclcpp::Subscription::SharedPtr nodeLifeCycleStateSubscription_; - rclcpp::Subscription::SharedPtr missionStateSubscription_; - rclcpp::Publisher::SharedPtr operatorCommandPublisher_; - rclcpp::Publisher::SharedPtr controllerStatePublisher_; - std::multimap node_states_; - std::vector mission_states_; - - /** Act as the operator (typically via a remote or ground station) to send one - * signal to transition to proceed, cancel, abort, etc. - * - * @param command one of brain_box_msgs::OperatorCommand::ARM; - * - * @see arm() - * @see launch() - */ - void publishOperatorCommand(uint8_t command); - - /** Acting as the controller, this publishes the controller state - * to signal transitions due to autonomous processing - * @param state one of the brain_box_msgs::ControllerState state enums - * @see landed() - * */ - void publishControllerState(uint8_t state); - -public: - rclcpp::Node::SharedPtr nh_; - string target_node_name_; - - AMSuperTest(); - AMSuperTest(string target_node_name); - - /** when configured and Inactive, arm is sent to prepare all nodes for takeoff*/ - void arm(); - - /** when armed, signals for the props to spin and takeoff */ - void launch(); - - /** to transition super into disarming, flight must be completed by landing or canceled */ - void landed(); - - /** to transition super into disarming from armed */ - void cancel(); - - /** to transition super into semi-auto from auto */ - void pause(); - - /** to transition super into auto from semi-auto */ - void resume(); - - /** to transition super into abort while in flight mode */ - void abort(); - - /** to transition super into manual from an auto mode*/ - void manual(); - - /** to shut super down. Must be in BOOTING or READY */ - void shutdown(); - - - /** searches the node states matching the lifecycle given. - * - * @return true if the state is found for the node given, false otherwise - */ - bool nodeStateReceived(string node_name,LifeCycleState state); - - /** - * @return true if the desired state is anywhere in the list, regardless of order - */ - bool missionStateReceived(uint8_t mission_state); - - /** - * Callback sniffing the state of nodes, as if it were am_super, to see - * if the target node is transitioning as expected. - * - * Simply registers states in multimap for later inspection. - */ - void nodeLifeCycleStateCallback(const brain_box_msgs::msg::LifeCycleState& msg); - - void missionStateCallback(const brain_box_msgs::msg::VxState& msg); - - /** Loop until a am_super is broadcasting the desired state or until - * ros says its time to quit. - * FIXME: export SuperState into a library and use instead of the untyped messages. - * @param log_code to corrleate the log output to the source code - */ - void waitUntilMissionState(const uint8_t mssion_state, std::string log_code, float sleep = 1); - - [[deprecated("use waitUntilMissionState(status,log_code,sleep)")]] - void waitUntilMissionState(const uint8_t mssion_state, float sleep = 1); - - /** - * spin until the desired state is found or until the test times out. - */ - void waitUntil(const LifeCycleState state, float sleep = 1); - - /** - * @param log_code makes it easy to find the source of the log messages - */ - void waitUntil(const LifeCycleState state, const std::string log_code, float sleep = 1); - - - /** - * Wait until status is received or the test times out. - */ - [[deprecated("use waitUntil(status,log_code,sleep)")]] - void waitUntilStatus(const LifeCycleStatus& status, float sleep = 1); - - /** - * @brief look for status periodically, based on the sleep, or timeout based on the test time limit. - * @param log_code makes it easy to find the source of the log messages - */ - void waitUntil(const LifeCycleStatus& status, const std::string log_code,float sleep = 1); - - /** - * Looks to see if the given status has ever been received. - */ - bool nodeStatusReceived(string node_name, LifeCycleStatus status); - - /** - * Generate publishers and subscriptions. - */ - void createPubsSubs(); - -#define TEST_LOG( args, code) \ - do \ - { \ - RCLCPP_WARN_STREAM_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 1.0, args << " [" << code << "]"); \ - } while (0) -}; - -#endif \ No newline at end of file diff --git a/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/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index 51c5c2a4..2e37d718 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -54,6 +54,10 @@ class AMSuper { friend class AMSuperNode; +protected: + /* manage logic for LifeCycle */ + AMLifeCycleMediator life_cycle_mediator_; + private: shared_ptr life_cycle_node_; @@ -84,8 +88,6 @@ class AMSuper /** manage logic for SuperState transitions */ SuperStateMediator state_mediator_; - /* manage logic for LifeCycle */ - AMLifeCycleMediator life_cycle_mediator_; /** Node behavior management.*/ SuperNodeMediator node_mediator_; @@ -913,7 +915,7 @@ class AMSuperNode : public AMLifeCycle // #ifdef TESTING // #else -shared_ptr am::Node::node; +shared_ptr am::Node::node; int main(int argc, char** argv) { diff --git a/src/super_lib/am_life_cycle.cpp b/src/super_lib/am_life_cycle.cpp deleted file mode 100644 index 62efb351..00000000 --- a/src/super_lib/am_life_cycle.cpp +++ /dev/null @@ -1,588 +0,0 @@ -#include -#include -#include -#include -#include - -namespace am -{ - - -// static constexpr std::string AMLifeCycle::STATE_INVALID_STRING; -// static constexpr std::string AMLifeCycle::STATE_UNCONFIGURED_STRING; - -AMLifeCycle::AMLifeCycle(const std::string & node_name, const rclcpp::NodeOptions & options ) : - rclcpp::Node(node_name, options), updater_(this) -{ - initialize(); -}; - -AMLifeCycle::AMLifeCycle(const std::string & node_name, const std::string & namespace_, const rclcpp::NodeOptions & options ) : - rclcpp::Node(node_name, namespace_, options), updater_(this) -{ - initialize(); -} - -AMLifeCycle::~AMLifeCycle() -{ -} - -void AMLifeCycle::initialize() -{ - 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_ = create_publisher("/node_state", 100); - - // updater_ = std::make_shared(this); - 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 (std::string(get_name()).at(0) == '/') - { - node_name_ = std::string(get_name()).substr(1); - } - else - { - node_name_ = get_name(); - } - - - - // subs should always come at the end - /** - * node status via LifeCycle - */ - lifecycle_sub_ = create_subscription("/node_lifecycle", 100, - std::bind(&AMLifeCycle::lifecycleCB, this, std::placeholders::_1)); - - heartbeat_timer_ = create_wall_timer(am::toDuration(1.0), std::bind(&AMLifeCycle::heartbeatCB, this)); - -} - -template -bool AMLifeCycle::param(const std::string& param_name, T& param_val, const T& default_val) -{ - // can't use am:Node calls here because we are in constructor - try - { - declare_parameter(param_name, default_val); - } - catch(rclcpp::exceptions::ParameterAlreadyDeclaredException &e) - { - ; - } - bool result = get_parameter_or(param_name, param_val, default_val); - RCLCPP_INFO_STREAM(get_logger(), param_name << " = " << param_val); - return result; -} - -void AMLifeCycle::lifecycleCB(const brain_box_msgs::msg::LifeCycleCommand::SharedPtr 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_= am::ClockNow(); - } - 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_)) - { - rclcpp::Duration duration_since_configure = am::ClockNow() - configure_start_time_; - if (life_cycle_info_.state == LifeCycleState::UNCONFIGURED || duration_since_configure <= rclcpp::Duration(am::toDuration(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"); - - return stat; - -} - -AMStatReset& AMLifeCycle::configureHzStats(AMStatReset& stats) -{ - configureStat(stats, stats.getShortName(), "hz"); - - return stats; -} - -void AMLifeCycle::sendNodeUpdate() -{ - brain_box_msgs::msg::LifeCycleState msg; - msg.header.stamp = am::ClockNow(); - msg.node_name = get_name(); - msg.process_id = 0; - msg.state = (uint8_t)life_cycle_info_.state; - msg.status = (uint8_t)life_cycle_info_.status; - msg.subsystem = ""; - msg.value = ""; - state_pub_->publish(msg); -} - - -void AMLifeCycle::heartbeatCB() -{ - 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)); - } - - return true; -} - -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/src/super_lib/am_super_test.cpp b/src/super_lib/am_super_test.cpp deleted file mode 100644 index 1f3b735b..00000000 --- a/src/super_lib/am_super_test.cpp +++ /dev/null @@ -1,229 +0,0 @@ -#include -#include - -#include - -std::shared_ptr am::Node::node; - -AMSuperTest::AMSuperTest(string target_node_name) : nh_(std::make_shared(target_node_name)) -{ - createPubsSubs(); - - target_node_name_= target_node_name; - if(target_node_name_[0] != '/') - { - target_node_name_= '/' + target_node_name_; - } - TEST_LOG("Target node name:" << target_node_name_,"AXSO"); -} - -AMSuperTest::AMSuperTest() : nh_(std::make_shared("am_super_test")) -{ - createPubsSubs(); - - am::getParam("~target_node_name", target_node_name_, nh_->get_name()); - if(target_node_name_[0] != '/') - { - target_node_name_= '/' + target_node_name_; - } - - TEST_LOG("Target node name:" << target_node_name_, "ANNQ"); -} - -void AMSuperTest::createPubsSubs() -{ - nodeLifeCycleStateSubscription_ = nh_->create_subscription - (am_super_topics::LIFECYCLE_STATE, 1000, - std::bind(&AMSuperTest::nodeLifeCycleStateCallback, this, std::placeholders::_1)); - missionStateSubscription_ = nh_->create_subscription - (am_super_topics::SUPER_STATE, 1000, - std::bind(&AMSuperTest::missionStateCallback, this, std::placeholders::_1)); - operatorCommandPublisher_ = nh_->create_publisher - (am_super_topics::OPERATOR_COMMAND,100); - controllerStatePublisher_ = nh_->create_publisher - (am_super_topics::CONTROLLER_STATE, 100); -} - -void AMSuperTest::publishOperatorCommand(uint8_t command) -{ - brain_box_msgs::msg::OperatorCommand msg; - msg.node_name = nh_->get_name(); - msg.command = command; - operatorCommandPublisher_->publish(msg); -} - -void AMSuperTest::publishControllerState(uint8_t state) -{ - brain_box_msgs::msg::ControllerState msg; - msg.node_name = nh_->get_name(); - msg.state = state; - controllerStatePublisher_->publish(msg); -} - -/** when configured and Inactive, arm is sent to prepare all nodes for takeoff*/ -void AMSuperTest::arm() -{ - TEST_LOG("Operator sending ARM command","NQNA"); - publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::ARM); -} - -/** when armed, signals for the props to spin and takeoff */ -void AMSuperTest::launch() -{ - TEST_LOG("Operator sending LAUNCH command","ANQP"); - publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::LAUNCH); -} - -void AMSuperTest::landed() -{ - TEST_LOG("Controller sending LANDED state","BSJO"); - publishControllerState(brain_box_msgs::msg::ControllerState::COMPLETED); -} - -void AMSuperTest::cancel() -{ - TEST_LOG("Operator sending CANCEL command","LPOQ"); - publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::CANCEL); -} - -void AMSuperTest::pause() -{ - TEST_LOG("Operator sending PAUSE command","WOEB"); - publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::PAUSE); -} - -void AMSuperTest::resume() -{ - TEST_LOG("Operator sending RESUME command","ANQE"); - publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::RESUME); -} - -void AMSuperTest::abort() -{ - TEST_LOG("Operator sending ABORT command","EEEN"); - publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::ABORT); -} - -void AMSuperTest::manual() -{ - TEST_LOG("Operator sending MANUAL command","NQAY"); - publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::MANUAL); -} - -void AMSuperTest::shutdown() -{ - TEST_LOG("Operator sending SHUTDOWN command","VKSP"); - publishOperatorCommand(brain_box_msgs::msg::OperatorCommand::SHUTDOWN); -} - -bool AMSuperTest::nodeStateReceived(string node_name,LifeCycleState state) -{ - if(node_states_.count(node_name)){ - int key = 2; - auto lower_it = node_states_.lower_bound(node_name); - auto upper_it = node_states_.upper_bound(node_name); - - while (lower_it != upper_it) - { - if (lower_it -> first == node_name) { - brain_box_msgs::msg::LifeCycleState state_msg = lower_it -> second; - if((LifeCycleState)state_msg.state == state){ - return true; - } - } - lower_it++; - } - } - return false; -} - -bool AMSuperTest::missionStateReceived(uint8_t mission_state) -{ - for (auto state : mission_states_) - { - if(state == mission_state) - { - //Erase-remove idiom for removing all occurences from list - mission_states_.erase(remove(mission_states_.begin(), mission_states_.end(), state), mission_states_.end()); - return true; - } - } - return false; -} - -void AMSuperTest::nodeLifeCycleStateCallback(const brain_box_msgs::msg::LifeCycleState& msg) -{ - node_states_.emplace(msg.node_name,msg); -} - -void AMSuperTest::missionStateCallback(const brain_box_msgs::msg::VxState& msg) -{ - mission_states_.insert(mission_states_.end(),msg.state); -} - -void AMSuperTest::waitUntilMissionState(const uint8_t mission_state, float sleep) -{ - waitUntilMissionState(mission_state,"NAWU",sleep); -} - -void AMSuperTest::waitUntilMissionState(const uint8_t mission_state, std::string error_code, float sleep) -{ - rclcpp::Rate loop_rate(sleep); - while (!missionStateReceived(mission_state) && rclcpp::ok() ) - { - rclcpp::spin_some(nh_); - loop_rate.sleep(); - TEST_LOG("[" << error_code << "] waiting to receive mission state: " << mission_state, "08JU"); - } -} - -void AMSuperTest::waitUntil(const LifeCycleState state, float sleep){ - waitUntil(state,"XS32",sleep); -} - -void AMSuperTest::waitUntil(const LifeCycleState state, const std::string log_code, float sleep){ - rclcpp::Rate loop_rate(sleep); - while (!nodeStateReceived(nh_->get_name(),state) && rclcpp::ok() ) - { - rclcpp::spin_some(nh_); - loop_rate.sleep(); - TEST_LOG("[" << log_code << "]" << " waiting to receive node state: " << (int)state,"NBDC"); - } -} - -void AMSuperTest::waitUntilStatus(const LifeCycleStatus& status, float sleep) -{ - waitUntil(status,"XWSQ",sleep); - -} -void AMSuperTest::waitUntil(const LifeCycleStatus& status, const std::string log_code, float sleep) -{ - rclcpp::Rate loop_rate(sleep); - while (!nodeStatusReceived(nh_->get_name(),status) && rclcpp::ok() ) - { - rclcpp::spin_some(nh_); - loop_rate.sleep(); - TEST_LOG("[" << log_code << "]" << " waiting to receive node status: " << (int)status, "YTNJ" ); - } -} - -bool AMSuperTest::nodeStatusReceived(string node_name, LifeCycleStatus status) -{ - if(node_states_.count(node_name)){ - int key = 2; - auto lower_it = node_states_.lower_bound(node_name); - auto upper_it = node_states_.upper_bound(node_name); - - while (lower_it != upper_it) - { - if (lower_it -> first == node_name) { - brain_box_msgs::msg::LifeCycleState state_msg = lower_it -> second; - if((LifeCycleStatus)state_msg.status == status){ - return true; - } - } - lower_it++; - } - } - return false; -} \ No newline at end of file From dc4f4613018bbb9ff717d499859a1aead0265ea7 Mon Sep 17 00:00:00 2001 From: danhennage Date: Sun, 29 Jan 2023 16:11:35 -0800 Subject: [PATCH 11/45] fix: changed scope of life_cycle_mediator_ --- src/am_super/am_super.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index 2e37d718..be82e0f0 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -54,10 +54,6 @@ class AMSuper { friend class AMSuperNode; -protected: - /* manage logic for LifeCycle */ - AMLifeCycleMediator life_cycle_mediator_; - private: shared_ptr life_cycle_node_; @@ -88,6 +84,8 @@ class AMSuper /** manage logic for SuperState transitions */ SuperStateMediator state_mediator_; + /* manage logic for LifeCycle */ + AMLifeCycleMediator life_cycle_mediator_; /** Node behavior management.*/ SuperNodeMediator node_mediator_; From 062edf6990f98755bda4b84467b4fce82daf3483 Mon Sep 17 00:00:00 2001 From: Ian McMurray Date: Thu, 8 Jun 2023 16:03:27 -0700 Subject: [PATCH 12/45] feat: redefine ros_info_stream in am_super to print blue --- src/am_super/am_super.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index be82e0f0..65fed5f3 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -41,6 +41,27 @@ #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) + + using namespace std; namespace am From 55f5aa0ab3ee5f2bb862d93629402af5cd37656c Mon Sep 17 00:00:00 2001 From: Ian McMurray Date: Thu, 8 Jun 2023 17:32:30 -0700 Subject: [PATCH 13/45] feat: add am_ll2f to manifest, print when super configures self --- launch/am_super.yaml | 1 + src/am_super/am_super.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/launch/am_super.yaml b/launch/am_super.yaml index 255fa3c5..01a1a746 100644 --- a/launch/am_super.yaml +++ b/launch/am_super.yaml @@ -2,3 +2,4 @@ am_super: ros__parameters: platform: actual: farm-ng_amiga_mockup + manifest: am_latlong2frame diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index 65fed5f3..9770a995 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -513,6 +513,7 @@ class AMSuper { 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 From bfc066bea4cc1f0c557eb1cc94608534a03362d2 Mon Sep 17 00:00:00 2001 From: Ian McMurray Date: Tue, 13 Jun 2023 12:31:22 -0700 Subject: [PATCH 14/45] add am_mavros_bs to manifest --- launch/am_super.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/am_super.yaml b/launch/am_super.yaml index 01a1a746..a689e36e 100644 --- a/launch/am_super.yaml +++ b/launch/am_super.yaml @@ -2,4 +2,4 @@ am_super: ros__parameters: platform: actual: farm-ng_amiga_mockup - manifest: am_latlong2frame + manifest: am_latlong2frame, am_mavros_bs From f1077d31ccba8074f446c3f147710fb12e10192e Mon Sep 17 00:00:00 2001 From: Ian McMurray Date: Wed, 14 Jun 2023 19:12:05 -0700 Subject: [PATCH 15/45] change manifest to include am_gps instead of ll2f --- launch/am_super.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/am_super.yaml b/launch/am_super.yaml index a689e36e..56ad0813 100644 --- a/launch/am_super.yaml +++ b/launch/am_super.yaml @@ -2,4 +2,4 @@ am_super: ros__parameters: platform: actual: farm-ng_amiga_mockup - manifest: am_latlong2frame, am_mavros_bs + manifest: am_gps, am_mavros_bs From 1ab20a2355ea9af4d8664d7e6fd003ed63b507fd Mon Sep 17 00:00:00 2001 From: FarmXDev Date: Sat, 2 Mar 2024 12:08:47 -0800 Subject: [PATCH 16/45] feat: config and cleanup --- launch/am_super.yaml | 2 +- src/am_super/am_super.cpp | 29 ++++++++++++++--------------- 2 files changed, 15 insertions(+), 16 deletions(-) diff --git a/launch/am_super.yaml b/launch/am_super.yaml index 56ad0813..c3164dc3 100644 --- a/launch/am_super.yaml +++ b/launch/am_super.yaml @@ -2,4 +2,4 @@ am_super: ros__parameters: platform: actual: farm-ng_amiga_mockup - manifest: am_gps, am_mavros_bs + manifest: am_gps, am_mavros_bs, y3space_imu, logger, am_locator, amiga, am_ouster, am_pilot, ground_detector, vineyard, pcl_yolo_node_front_right, pcl_yolo_node_rear_right, pcl_yolo_node_rear_left, pcl_yolo_node_front_left diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index 9770a995..fe620282 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -210,22 +210,22 @@ class AMSuper /** * system status pub */ - vstate_summary_pub_ = am::Node::node->create_publisher(am_super_topics::SUPER_STATE, 1000); - system_state_pub_ = am::Node::node->create_publisher(am_topics::SYSTEM_STATE, 1000); + vstate_summary_pub_ = am::Node::node->create_publisher(am_super_topics::SUPER_STATE, am::getSensorQoS(10)); + system_state_pub_ = am::Node::node->create_publisher(am_topics::SYSTEM_STATE, am::getSensorQoS(10)); /**Super * node lifecycle state pub. used to tell nodes to change their lifecycle state. */ - lifecycle_pub_ = am::Node::node->create_publisher(am_super_topics::NODE_LIFECYCLE, 100); + lifecycle_pub_ = am::Node::node->create_publisher(am_super_topics::NODE_LIFECYCLE, am::getSensorQoS(1)); /** * led control pub */ - led_pub_ = am::Node::node->create_publisher(am::am_topics::LED_BLINK, 1000); + led_pub_ = am::Node::node->create_publisher(am::am_topics::LED_BLINK, am::getSensorQoS(1)); /** * super status contains online naode list for gcs_comms */ - super_status_pub_ = am::Node::node->create_publisher(am_super_topics::SUPER_STATUS, 1000); + super_status_pub_ = am::Node::node->create_publisher(am_super_topics::SUPER_STATUS, am::getSensorQoS(10)); - flight_plan_deactivation_pub_ = am::Node::node->create_publisher(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, 1000); + flight_plan_deactivation_pub_ = am::Node::node->create_publisher(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, am::getSensorQoS(1)); supervisor_.system_state = SuperState::BOOTING; supervisor_.flt_ctrl_state = SuperNodeMediator::SuperFltCtrlState::INIT; @@ -246,7 +246,7 @@ class AMSuper /** * node status via LifeCycle */ - node_state_sub_ = am::Node::node->create_subscription(am_super_topics::LIFECYCLE_STATE, 100, + node_state_sub_ = am::Node::node->create_subscription(am_super_topics::LIFECYCLE_STATE, am::getSensorQoS(1), std::bind(&AMSuper::nodeStateCB, this, std::placeholders::_1)); /** @@ -261,7 +261,7 @@ class AMSuper diagnostics_sub = am::Node::node->create_subscription("/diagnostics", 100, std::bind(&AMSuper::diagnosticsCB, this, std::placeholders::_1)); - current_enu_sub = am::Node::node->create_subscription(am_topics::CTRL_VX_VEHICLE_CURRENTENU, 100, + 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)); } @@ -795,12 +795,11 @@ class AMSuperNode : public AMLifeCycle * Verify the basic requirements are being met: * - platform required matches actual platform */ - void onConfigure() override + bool onConfigure() override { if(am_super_ == nullptr) { - AMLifeCycle::onConfigure(); - return; + return AMLifeCycle::onConfigure(); } SuperNodeMediator::PlatformVariant required_platform; @@ -817,11 +816,11 @@ class AMSuperNode : public AMLifeCycle << am_super_->node_mediator_.platformVariantToConfig(actual_platform) ; errorTerminal(message.str(),"NSK2"); //force failure since this is not recoverable + //ROS_ERROR_STREAM(message.str()); + return false; + } - else - { - AMLifeCycle::onConfigure(); - } + return AMLifeCycle::onConfigure(); } /** From e0513139aa25640450e05af6ece9a2ac9e94fc72 Mon Sep 17 00:00:00 2001 From: FarmXDev Date: Sun, 31 Mar 2024 14:30:48 -0700 Subject: [PATCH 17/45] fix: treat BOOTING special --- src/am_super/am_super.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index fe620282..8ad70681 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -361,7 +361,7 @@ class AMSuper ROS_INFO_STREAM( node_name << " changed status to = " << life_cycle_mediator_.statusToString(status) << " [09SI]"); nr.status = status; nodes_changed = true; - if(nr.manifested && nr.status == LifeCycleStatus::ERROR) + if(nr.manifested && nr.status == LifeCycleStatus::ERROR && supervisor_.system_state != SuperState::BOOTING) { supervisor_.status_error = true; ROS_INFO_STREAM( "Manifested node " << nr.name << " changed status to ERROR. Shutting down nodes... [JHRE]"); @@ -511,10 +511,12 @@ class AMSuper */ void checkForSystemStateTransition() { - if(life_cycle_node_->getState() == LifeCycleState::INACTIVE && supervisor_.system_state == SuperState::READY) //if super lifecycle is currently inactive + if(life_cycle_node_->getState() == LifeCycleState::INACTIVE && supervisor_.system_state == SuperState::READY) { - ROS_INFO_STREAM("Automatically activating am_super"); - sendLifeCycleCommand(node_mediator_.getNodeName(), LifeCycleCommand::ACTIVATE); + // this is legacy code to ACTIVATE as soon as eeryone is online. this shouldn't happen until the operator starts the mission. + ROS_INFO_STREAM_THROTTLE(10, "Automatic activation disabled"); + // ROS_INFO_STREAM("Automatically activating am_super"); + // sendLifeCycleCommand(node_mediator_.getNodeName(), LifeCycleCommand::ACTIVATE); } else { From c153da550d3cb65374d1b662f80b8bacf735e677 Mon Sep 17 00:00:00 2001 From: FarmXDev Date: Tue, 2 Apr 2024 14:12:30 -0700 Subject: [PATCH 18/45] fix: removed comment --- src/am_super/am_super.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index 8ad70681..5a1888b9 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -818,7 +818,6 @@ class AMSuperNode : public AMLifeCycle << am_super_->node_mediator_.platformVariantToConfig(actual_platform) ; errorTerminal(message.str(),"NSK2"); //force failure since this is not recoverable - //ROS_ERROR_STREAM(message.str()); return false; } From c132cac295418d3f62d224b7cf7b8ce6456101ce Mon Sep 17 00:00:00 2001 From: danhennage Date: Wed, 31 Jul 2024 15:22:41 -0700 Subject: [PATCH 19/45] feat: AMStatStatus --- include/am_super/super_node_mediator.h | 1 + launch/am_super.yaml | 2 +- src/am_super/am_super.cpp | 220 ++++++++++++------------- src/am_super/super_node_mediator.cpp | 94 ++++++----- 4 files changed, 161 insertions(+), 156 deletions(-) diff --git a/include/am_super/super_node_mediator.h b/include/am_super/super_node_mediator.h index df0773b1..b4f2d1a2 100644 --- a/include/am_super/super_node_mediator.h +++ b/include/am_super/super_node_mediator.h @@ -101,6 +101,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; diff --git a/launch/am_super.yaml b/launch/am_super.yaml index c3164dc3..b4984717 100644 --- a/launch/am_super.yaml +++ b/launch/am_super.yaml @@ -2,4 +2,4 @@ am_super: ros__parameters: platform: actual: farm-ng_amiga_mockup - manifest: am_gps, am_mavros_bs, y3space_imu, logger, am_locator, amiga, am_ouster, am_pilot, ground_detector, vineyard, pcl_yolo_node_front_right, pcl_yolo_node_rear_right, pcl_yolo_node_rear_left, pcl_yolo_node_front_left + manifest: am_node_template diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index 5a1888b9..a325ea25 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -124,22 +124,6 @@ class AMSuper */ 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::msg::StampedAltimeter altimeter_bs_msg_type; - am::BabySitter* altimeter_bs_; - const std::string ALTIMETER_BS_TOPIC = "/sensor/distance/agl_lw"; // TODO: replace with system global const - const int ALTIMETER_HZ = 20; - - const std::string NODE_BS_DJI = "dji_sdk"; // TODO: replace with system global const - typedef sensor_msgs::msg::Joy dji_bs_msg_type; - am::BabySitter* dji_bs_; - const std::string DJI_BS_TOPIC = "/dji_sdk/rc"; // TODO: replace with system global const - const int DJI_HZ = 50; - #if CUDA_FLAG std::shared_ptr gpu_info_; #endif @@ -155,7 +139,7 @@ class AMSuper ROS_INFO_STREAM( "node_timeout_s = " << node_timeout_s_); /* - * 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 @@ -170,7 +154,7 @@ class AMSuper // 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 @@ -178,21 +162,6 @@ class AMSuper supervisor_.nodes.insert(pair(name, nr)); 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( - am::Node::node, BagLogger::instance(), name, ALTIMETER_BS_TOPIC, altimeter_warn_ms, altimeter_error_ms); - } - else if (!name.compare(NODE_BS_DJI)) - { - int dji_warn_ms, dji_error_ms; - calcBSTiming(DJI_HZ, dji_warn_ms, dji_error_ms); - dji_bs_ = new am::BabySitter(am::Node::node, BagLogger::instance(), name, DJI_BS_TOPIC, dji_warn_ms, - dji_error_ms); - } } } else @@ -207,35 +176,28 @@ class AMSuper gpu_info_ = std::make_shared(nh_); #endif - /** - * system status pub - */ + // 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_pub_ = am::Node::node->create_publisher(am_topics::SYSTEM_STATE, am::getSensorQoS(10)); - /**Super - * node lifecycle state pub. used to tell nodes to change their lifecycle state. - */ + + // 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 control pub - */ + + // 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 status contains online naode list for gcs_comms - */ + + // super state: manifest info super_status_pub_ = am::Node::node->create_publisher(am_super_topics::SUPER_STATUS, am::getSensorQoS(10)); + // ??? flight_plan_deactivation_pub_ = am::Node::node->create_publisher(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, am::getSensorQoS(1)); supervisor_.system_state = SuperState::BOOTING; supervisor_.flt_ctrl_state = SuperNodeMediator::SuperFltCtrlState::INIT; - /** - * 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)); - // startup bagfile - gets closed after frist log control command ROS_INFO_STREAM( "start logging to ST, level " << SU_LOG_LEVEL); BagLogger::instance()->startLogging("ST", SU_LOG_LEVEL); @@ -243,24 +205,27 @@ class AMSuper // subs should always come at the end - /** - * node status via LifeCycle - */ + // 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)); + + // 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)); - /** - * commands from operator - */ + // 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)); + // 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)); } @@ -313,14 +278,15 @@ class AMSuper { //const brain_box_msgs::OperatorCommand::ConstPtr& rmsg = event.getMessage(); - RCLCPP_INFO(am::Node::node->get_logger(), "Received Operator Command: %s sent '%i'",rmsg->node_name.c_str(),rmsg->command ); + RCLCPP_INFO(am::Node::node->get_logger(), "Received Operator Command: %s sent '%i'",rmsg->node_name.c_str(),rmsg->command ); node_mediator_.setOperatorCommand(supervisor_, (OperatorCommand)rmsg->command); // TODO: topic name should come from vb_util_lib::topics. LOG_MSG("/operator/command", *rmsg, SU_LOG_LEVEL); } + /** - * process state + * process state from lifecycle nodes * @param node_name_in * @param state * @param status @@ -352,22 +318,24 @@ class AMSuper } if (nr.state != state) { - ROS_INFO_STREAM( node_name << " changed state to = " << life_cycle_mediator_.stateToString(state) << " [38S8]"); + ROS_INFO_STREAM( node_name << " changed state to = " << life_cycle_mediator_.stateToString(state) << " [38S8]"); nr.state = state; nodes_changed = true; } if (nr.status != status) { - ROS_INFO_STREAM( node_name << " changed status to = " << life_cycle_mediator_.statusToString(status) << " [09SI]"); + ROS_INFO_STREAM( node_name << " changed status to = " << life_cycle_mediator_.statusToString(status) << " [09SI]"); nr.status = status; nodes_changed = true; if(nr.manifested && nr.status == LifeCycleStatus::ERROR && supervisor_.system_state != SuperState::BOOTING) { supervisor_.status_error = true; ROS_INFO_STREAM( "Manifested node " << nr.name << " changed status to ERROR. Shutting down nodes... [JHRE]"); - stopFlightPlan(); + // TODO: put this back in somehow - need to rethink how am_super influsenes control + // stopFlightPlan(); } } + // 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 @@ -407,26 +375,27 @@ class AMSuper 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) - { - RCLCPP_INFO_STREAM_THROTTLE(am::Node::node->get_logger(), *am::Node::node->get_clock(), 1.0, "flight status: " << value); - checkForSystemStateTransition(); - } - } + // 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(); + // } + // } } @@ -468,30 +437,30 @@ class AMSuper 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. */ @@ -511,15 +480,14 @@ class AMSuper */ void checkForSystemStateTransition() { - if(life_cycle_node_->getState() == LifeCycleState::INACTIVE && supervisor_.system_state == SuperState::READY) - { - // this is legacy code to ACTIVATE as soon as eeryone is online. this shouldn't happen until the operator starts the mission. - ROS_INFO_STREAM_THROTTLE(10, "Automatic activation disabled"); - // ROS_INFO_STREAM("Automatically activating am_super"); - // 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_); @@ -541,7 +509,7 @@ class AMSuper sendLifeCycleCommand(failed_node_name, command); } } - } + // } } /** @@ -888,7 +856,25 @@ class AMSuperNode : public AMLifeCycle for (it = am_super_->supervisor_.nodes.begin(); it != am_super_->supervisor_.nodes.end(); it++) { SuperNodeMediator::SuperNodeInfo& nr = (*it).second; - status_msg.nodes.push_back(nr.name); + 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("/status/super", status_msg, 1); if (am_super_->super_status_pub_->get_subscription_count() > 0) diff --git a/src/am_super/super_node_mediator.cpp b/src/am_super/super_node_mediator.cpp index 4a854874..206dc046 100644 --- a/src/am_super/super_node_mediator.cpp +++ b/src/am_super/super_node_mediator.cpp @@ -12,39 +12,55 @@ namespace am 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 + }}, + + // { 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}}, + // }} + }) { } @@ -151,14 +167,16 @@ SuperNodeMediator::TransitionInstructions SuperNodeMediator::transitionReady(Sup { 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) + // { + // transition = getErrorTransition(); + // } + // 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) From a433cc765559440e643ce32f572a4ee225943684 Mon Sep 17 00:00:00 2001 From: danhennage Date: Thu, 1 Aug 2024 09:56:01 -0700 Subject: [PATCH 20/45] fix: added time to msg header --- src/am_super/am_super.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index a325ea25..962ddf0a 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -372,6 +372,7 @@ class AMSuper if (nodes_changed) { reportSystemState(); + ROS_INFO("processSTate"); checkForSystemStateTransition(); } @@ -432,6 +433,7 @@ class AMSuper { ROS_DEBUG_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::msg::LifeCycleCommand::_command_type)command; lifecycle_pub_->publish(msg); From 5e5197c862fb573eadbd27921734fdce016c0616 Mon Sep 17 00:00:00 2001 From: AJ Date: Mon, 16 Sep 2024 10:49:18 -0700 Subject: [PATCH 21/45] Investigating am_super functionality --- src/am_super/am_super.cpp | 7 ++++- src/am_super/super_node_mediator.cpp | 39 +++++++++++++++++++++------- 2 files changed, 36 insertions(+), 10 deletions(-) diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index 962ddf0a..d2f6cee5 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -332,7 +332,7 @@ class AMSuper supervisor_.status_error = true; ROS_INFO_STREAM( "Manifested node " << nr.name << " changed status to ERROR. Shutting down nodes... [JHRE]"); // TODO: put this back in somehow - need to rethink how am_super influsenes control - // stopFlightPlan(); + stopFlightPlan(); } } // TODO: need to test the pid stuff - not sure if it is working @@ -493,6 +493,11 @@ class AMSuper SuperNodeMediator::TransitionInstructions transition_instructions = node_mediator_.transitionReady(supervisor_); + ROS_WARN_STREAM("Transition Instructions: ready_for_transition=" << transition_instructions.ready_for_transition << + " new_state=" << (std::uint8_t)transition_instructions.new_state << + " resend_life_cycle_command= " << transition_instructions.resend_life_cycle_command << + "life_cycle_command=" << (std::uint8_t)transition_instructions.life_cycle_command); + if (transition_instructions.ready_for_transition) { setSystemState(transition_instructions.new_state); diff --git a/src/am_super/super_node_mediator.cpp b/src/am_super/super_node_mediator.cpp index 206dc046..e87662c4 100644 --- a/src/am_super/super_node_mediator.cpp +++ b/src/am_super/super_node_mediator.cpp @@ -20,21 +20,42 @@ SuperNodeMediator::SuperNodeMediator(rclcpp::Node::SharedPtr node, const std::st { SuperState::BOOTING, { // from state {SuperState::READY, {SuperState::READY, SuperNodeMediator::checkReadyToArm, LifeCycleCommand::CONFIGURE}} // to state }}, + + + // FROM HARDIK SIDE: - // { SuperState::READY, { - // {SuperState::ARMING, {SuperState::ARMING, SuperNodeMediator::checkReadyToArm, StateTransition:: - // , OperatorCommand::ARM}}, - // {SuperState::SHUTDOWN, {SuperState::SHUTDOWN, SuperNodeMediator::checkNodesShuttingDownOrFinalized, LifeCycleCommand::SHUTDOWN, OperatorCommand::SHUTDOWN}} - // }}, + // { SuperState::READY, { // from state + // {SuperState::AUTO, {SuperState::AUTO, StateTransition::NO_LIFECYCLE_COMMAND, OperatorCommand::LAUNCH}} // to state + // }}, + + + // { SuperState::AUTO, { + // READY -- everything worked, stop flight plan + // ABORT -- something went wrong. stop flight plan + + + // {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}} + // }}, + + // 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::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}}, From 7dc49e47e1ec8d0b43b8bf3658a1447f65238a11 Mon Sep 17 00:00:00 2001 From: HardikSingh97 Date: Fri, 20 Sep 2024 18:33:27 -0700 Subject: [PATCH 22/45] feat: wholesale changes to get super working with health monitoring and demo state transitions --- include/am_super/super_node_mediator.h | 12 +- src/am_super/am_super.cpp | 195 ++++++++++++++++- src/am_super/super_node_mediator.cpp | 287 ++++++++++++++++++++++--- src/am_super/super_state_mediator.cpp | 27 ++- 4 files changed, 470 insertions(+), 51 deletions(-) diff --git a/include/am_super/super_node_mediator.h b/include/am_super/super_node_mediator.h index b4f2d1a2..2a191a14 100644 --- a/include/am_super/super_node_mediator.h +++ b/include/am_super/super_node_mediator.h @@ -161,7 +161,7 @@ class SuperNodeMediator /** 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 */ @@ -243,6 +243,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 @@ -267,7 +275,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*/ diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index d2f6cee5..435e35b4 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -31,12 +31,15 @@ #include #include #include +#include #include #include #include #include +#include + #if CUDA_FLAG #include #endif @@ -62,10 +65,114 @@ #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. * @@ -95,6 +202,7 @@ class AMSuper rclcpp::Publisher::SharedPtr flight_plan_deactivation_pub_; rclcpp::Subscription::SharedPtr node_state_sub_; rclcpp::Subscription::SharedPtr operator_command_sub_; + rclcpp::Subscription::SharedPtr fake_operator_command_sub_; rclcpp::Subscription::SharedPtr controller_state_sub; rclcpp::Subscription::SharedPtr diagnostics_sub; rclcpp::Subscription::SharedPtr current_enu_sub; @@ -135,7 +243,7 @@ class AMSuper life_cycle_node_ = std::static_pointer_cast(am::Node::node); - am::getParam("node_timeout_s", node_timeout_s_, 2.0); + am::getParam("node_timeout_s", node_timeout_s_, 3.1); ROS_INFO_STREAM( "node_timeout_s = " << node_timeout_s_); /* @@ -217,6 +325,9 @@ class AMSuper operator_command_sub_ = am::Node::node->create_subscription(am_super_topics::OPERATOR_COMMAND, 100, std::bind(&AMSuper::operatorCommandCB, this, std::placeholders::_1)); + fake_operator_command_sub_ = am::Node::node->create_subscription(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, 100, + std::bind(&AMSuper::fakeOperatorCommandCB, this, std::placeholders::_1)); + // controller state: ??? controller_state_sub = am::Node::node->create_subscription(am_super_topics::CONTROLLER_STATE, 100, std::bind(&AMSuper::controllerStateCB, this, std::placeholders::_1)); @@ -228,6 +339,7 @@ class AMSuper // 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() @@ -280,9 +392,25 @@ class AMSuper RCLCPP_INFO(am::Node::node->get_logger(), "Received Operator Command: %s sent '%i'",rmsg->node_name.c_str(),rmsg->command ); - node_mediator_.setOperatorCommand(supervisor_, (OperatorCommand)rmsg->command); - // TODO: topic name should come from vb_util_lib::topics. - LOG_MSG("/operator/command", *rmsg, SU_LOG_LEVEL); + // 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) + { + RCLCPP_INFO(am::Node::node->get_logger(), "Received FAKE Operator Command (actually flight_controller command): %i", msg->data); + if (msg->data == true) + { + node_mediator_.setOperatorCommand(supervisor_, OperatorCommand::LAUNCH); + } + else + { + node_mediator_.setOperatorCommand(supervisor_, OperatorCommand::CANCEL); + } + + LOG_MSG(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, *msg, SU_LOG_LEVEL); } /** @@ -431,7 +559,7 @@ class AMSuper */ 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"); + 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; @@ -494,9 +622,9 @@ class AMSuper SuperNodeMediator::TransitionInstructions transition_instructions = node_mediator_.transitionReady(supervisor_); ROS_WARN_STREAM("Transition Instructions: ready_for_transition=" << transition_instructions.ready_for_transition << - " new_state=" << (std::uint8_t)transition_instructions.new_state << - " resend_life_cycle_command= " << transition_instructions.resend_life_cycle_command << - "life_cycle_command=" << (std::uint8_t)transition_instructions.life_cycle_command); + " 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) { @@ -506,14 +634,22 @@ class AMSuper { 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++; } } // } @@ -749,13 +885,30 @@ class AMSuper }; + + +class AMSuperNodeStats +{ + friend class AmSuperNode; + + +public: + AMStatStatus statStatus = AMStatStatus("ss", "AMStatStatus"); + AMSuperNodeStats(AMStatList &stat_list) + { + stat_list.add(&statStatus); + } +}; + + class AMSuperNode : public AMLifeCycle { private: shared_ptr am_super_; + std::shared_ptr stats_; public: - AMSuperNode(const std::string & node_name) : AMLifeCycle(node_name) + AMSuperNode(const std::string & node_name) : AMLifeCycle(node_name), stats_(std::make_shared(stats_list_)) { } @@ -845,6 +998,14 @@ class AMSuperNode : public AMLifeCycle { 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(); } } @@ -898,11 +1059,23 @@ class AMSuperNode : public AMLifeCycle // 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) + { + stats_->statStatus = 2; + 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()); + if (am_super_->supervisor_.system_state == SuperState::BOOTING) + { + stats_->statStatus=0; + } } // log stats diff --git a/src/am_super/super_node_mediator.cpp b/src/am_super/super_node_mediator.cpp index e87662c4..7a8c9284 100644 --- a/src/am_super/super_node_mediator.cpp +++ b/src/am_super/super_node_mediator.cpp @@ -9,6 +9,81 @@ namespace am /** * The state of the system as the supervisor sees it.*/ + +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_({ @@ -24,21 +99,33 @@ SuperNodeMediator::SuperNodeMediator(rclcpp::Node::SharedPtr node, const std::st // FROM HARDIK SIDE: - // { SuperState::READY, { // from state - // {SuperState::AUTO, {SuperState::AUTO, StateTransition::NO_LIFECYCLE_COMMAND, OperatorCommand::LAUNCH}} // to state + { 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::AUTO, { - // READY -- everything worked, stop flight plan - // ABORT -- something went wrong. stop flight plan - // {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:} + // FROM BEFORE SIDE: @@ -46,7 +133,7 @@ SuperNodeMediator::SuperNodeMediator(rclcpp::Node::SharedPtr node, const std::st // {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}} @@ -57,12 +144,12 @@ SuperNodeMediator::SuperNodeMediator(rclcpp::Node::SharedPtr node, const std::st // {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::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}}, @@ -110,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) @@ -135,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 @@ -143,6 +231,7 @@ 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; } } @@ -150,20 +239,30 @@ SuperNodeMediator::StateTransition SuperNodeMediator::getStateTransition(const S { 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() @@ -183,6 +282,13 @@ SuperNodeMediator::TransitionInstructions SuperNodeMediator::transitionReady(Sup transition_instructions.ready_for_transition = false; transition_instructions.resend_life_cycle_command = 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; + } // only check those states registered with state_transitions if (state_transitions_.count(supervisor.system_state)) { @@ -190,9 +296,6 @@ SuperNodeMediator::TransitionInstructions SuperNodeMediator::transitionReady(Sup // TODO: put htis back in when we figure out how errors are going to work // if(supervisor.status_error) - // { - // transition = getErrorTransition(); - // } // else { transition = getStateTransition(supervisor); @@ -205,7 +308,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; @@ -219,7 +322,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; @@ -250,11 +354,13 @@ bool SuperNodeMediator::lifeCycleNotYetImplemented(string node_name) bool SuperNodeMediator::checkReadyToArm(SuperNodeMediator::SuperNodeInfo& nr, SuperNodeMediator& node_mediator) { + ROS_INFO_STREAM("checkReadyToArm: " << nr.name << " is " << printLifeCycleState(nr.state)); return nr.state == LifeCycleState::INACTIVE || (nr.state == LifeCycleState::ACTIVE && node_mediator.nodeNameIsSuper(nr.name)); } bool SuperNodeMediator::checkArmed(SuperNodeMediator::SuperNodeInfo& nr, SuperNodeMediator& node_mediator) { + ROS_INFO_STREAM("checkArmed: " << nr.name << " is " << printLifeCycleState(nr.state)); return nr.state == LifeCycleState::ACTIVE; } @@ -268,54 +374,181 @@ 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::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; + -pair> SuperNodeMediator::allManifestedNodesCheck( + // 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; + + +} + +// 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); 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 { 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 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, " "); diff --git a/src/am_super/super_state_mediator.cpp b/src/am_super/super_state_mediator.cpp index ba1f40ab..7a8bd139 100644 --- a/src/am_super/super_state_mediator.cpp +++ b/src/am_super/super_state_mediator.cpp @@ -29,18 +29,23 @@ struct SuperStateInfo 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::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() From d1d6b3a8eb157ea1b751360bc4cff0b97b62a026 Mon Sep 17 00:00:00 2001 From: HardikSingh97 Date: Wed, 25 Sep 2024 15:46:40 -0700 Subject: [PATCH 23/45] tweak: make sure am_super also goes inactive --- src/am_super/super_node_mediator.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/am_super/super_node_mediator.cpp b/src/am_super/super_node_mediator.cpp index 7a8c9284..719f5ec6 100644 --- a/src/am_super/super_node_mediator.cpp +++ b/src/am_super/super_node_mediator.cpp @@ -355,7 +355,8 @@ bool SuperNodeMediator::lifeCycleNotYetImplemented(string node_name) bool SuperNodeMediator::checkReadyToArm(SuperNodeMediator::SuperNodeInfo& nr, SuperNodeMediator& node_mediator) { 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.state == LifeCycleState::ACTIVE && node_mediator.nodeNameIsSuper(nr.name)); + return nr.state == LifeCycleState::INACTIVE; } bool SuperNodeMediator::checkArmed(SuperNodeMediator::SuperNodeInfo& nr, SuperNodeMediator& node_mediator) From 2098684999a5996832ef092e73556029f060b8c0 Mon Sep 17 00:00:00 2001 From: HardikSingh97 Date: Mon, 30 Sep 2024 12:46:18 -0700 Subject: [PATCH 24/45] tweak: in field changes allowed to improve demo --- src/am_super/am_super.cpp | 5 +- src/am_super/super_node_mediator.cpp | 77 +++++++++++++++++++++++++-- src/am_super/super_state_mediator.cpp | 2 +- 3 files changed, 77 insertions(+), 7 deletions(-) diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index 435e35b4..11050894 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -301,7 +301,9 @@ class AMSuper super_status_pub_ = am::Node::node->create_publisher(am_super_topics::SUPER_STATUS, am::getSensorQoS(10)); // ??? - flight_plan_deactivation_pub_ = am::Node::node->create_publisher(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, am::getSensorQoS(1)); + 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); supervisor_.system_state = SuperState::BOOTING; supervisor_.flt_ctrl_state = SuperNodeMediator::SuperFltCtrlState::INIT; @@ -462,6 +464,7 @@ class AMSuper // TODO: put this back in somehow - need to rethink how am_super influsenes control stopFlightPlan(); } + // if (nr.manifested && nr.status == LifeCycleStatus::ACTIVE) } // TODO: need to test the pid stuff - not sure if it is working if (nr.pid != pid) diff --git a/src/am_super/super_node_mediator.cpp b/src/am_super/super_node_mediator.cpp index 719f5ec6..c32aa5c8 100644 --- a/src/am_super/super_node_mediator.cpp +++ b/src/am_super/super_node_mediator.cpp @@ -283,12 +283,78 @@ SuperNodeMediator::TransitionInstructions SuperNodeMediator::transitionReady(Sup transition_instructions.resend_life_cycle_command = false; // Hardik: shortcircuit if we have am_super in error - if (supervisor.nodes.at(SUPER_NODE_NAME).status == LifeCycleStatus::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; + // } + + + bool are_in_error = false; + for (pair nodePair : supervisor.nodes) { - transition_instructions.ready_for_transition = true; - transition_instructions.new_state = getErrorTransition().to_state; + SuperNodeInfo node = nodePair.second; + if (node.status == LifeCycleStatus::ERROR) + { + are_in_error = true; + transition_instructions.ready_for_transition = 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)) { @@ -356,13 +422,13 @@ bool SuperNodeMediator::checkReadyToArm(SuperNodeMediator::SuperNodeInfo& nr, Su { 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; + return nr.state == LifeCycleState::INACTIVE && nr.status != LifeCycleStatus::ERROR; } bool SuperNodeMediator::checkArmed(SuperNodeMediator::SuperNodeInfo& nr, SuperNodeMediator& node_mediator) { ROS_INFO_STREAM("checkArmed: " << nr.name << " is " << printLifeCycleState(nr.state)); - return nr.state == LifeCycleState::ACTIVE; + return nr.state == LifeCycleState::ACTIVE && nr.status != LifeCycleStatus::ERROR; } bool SuperNodeMediator::checkNodesActiveOrInactive(SuperNodeMediator::SuperNodeInfo& nr, SuperNodeMediator& node_mediator) @@ -471,6 +537,7 @@ pair>> SuperNodeMediator::allManifestedNode // success = false; need_lifecycle_resend = false; // TODO from Hardik: Should this be flipped? } + // else if (node.status == LifeCycleState::ACTIVE) } else { diff --git a/src/am_super/super_state_mediator.cpp b/src/am_super/super_state_mediator.cpp index 7a8bd139..b8b5bed7 100644 --- a/src/am_super/super_state_mediator.cpp +++ b/src/am_super/super_state_mediator.cpp @@ -28,7 +28,7 @@ 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::BOOTING, { "BOOTING", { SuperState::BOOTING, SuperState::READY, SuperState::SHUTDOWN } } }, { SuperState::READY, { "READY", { SuperState::AUTO, SuperState::BOOTING} } }, { SuperState::AUTO, { "AUTO", { SuperState::READY, SuperState::BOOTING } } }, From a35244ad1bcda391364243a7e9adcb71f3b90bef Mon Sep 17 00:00:00 2001 From: HardikSingh97 Date: Mon, 7 Oct 2024 21:39:49 -0700 Subject: [PATCH 25/45] feat: close to work with new state machine --- include/am_super/super_node_mediator.h | 13 +- src/am_super/am_super.cpp | 301 ++++++++++++++++++++++++- src/am_super/super_node_mediator.cpp | 169 +++++++++----- 3 files changed, 417 insertions(+), 66 deletions(-) diff --git a/include/am_super/super_node_mediator.h b/include/am_super/super_node_mediator.h index 2a191a14..ad51abc4 100644 --- a/include/am_super/super_node_mediator.h +++ b/include/am_super/super_node_mediator.h @@ -73,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. @@ -93,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 @@ -155,6 +163,9 @@ 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; @@ -199,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 diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index 11050894..f22a9ec3 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -8,6 +8,7 @@ #include #include #include +#include "std_msgs/msg/int32_multi_array.hpp" #include #include @@ -20,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -197,12 +199,15 @@ class AMSuper 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 */ 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; @@ -226,6 +231,13 @@ class AMSuper * 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_required_ = false; + bool restart_required_ = false; + bool offline_restart_required_ = false; + bool first_time_booted_ = false; /** * baglogger level @@ -246,6 +258,9 @@ class AMSuper 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 */ @@ -287,7 +302,7 @@ class AMSuper // 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: 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 @@ -300,11 +315,18 @@ class AMSuper // 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); + // 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; @@ -330,6 +352,10 @@ class AMSuper fake_operator_command_sub_ = am::Node::node->create_subscription(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, 100, std::bind(&AMSuper::fakeOperatorCommandCB, this, std::placeholders::_1)); + ui_operator_command_sub_ = am::Node::node->create_subscription("/ui_operator_cmd", 100, + std::bind(&AMSuper::uiOperatorCommandCB, this, std::placeholders::_1)); + + // controller state: ??? controller_state_sub = am::Node::node->create_subscription(am_super_topics::CONTROLLER_STATE, 100, std::bind(&AMSuper::controllerStateCB, this, std::placeholders::_1)); @@ -402,8 +428,13 @@ class AMSuper void fakeOperatorCommandCB(const std_msgs::msg::Bool::SharedPtr msg) { + 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) + 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); } @@ -415,6 +446,25 @@ class AMSuper 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 from lifecycle nodes * @param node_name_in @@ -435,7 +485,7 @@ class AMSuper // 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 @@ -455,15 +505,34 @@ class AMSuper if (nr.status != status) { 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); + } + nr.status = status; nodes_changed = true; - if(nr.manifested && nr.status == LifeCycleStatus::ERROR && supervisor_.system_state != SuperState::BOOTING) + // 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; + } supervisor_.status_error = true; - ROS_INFO_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(); } + + // if (nr.manifested && nr.status == LifeCycleStatus::ACTIVE) } // TODO: need to test the pid stuff - not sure if it is working @@ -481,6 +550,8 @@ class AMSuper nr.pid = pid; nodes_changed = true; } + + nr.last_contact = last_contact; } else @@ -605,6 +676,15 @@ class AMSuper 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."); + } + /** * check for state transition based upon current state and values of member fields. * Will call to modify the system state if transition is necessary. Will also call @@ -654,8 +734,30 @@ class AMSuper } 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); + } + + + } + + } } /** @@ -686,11 +788,21 @@ class AMSuper reportSystemState(); - sendLEDMessage(); + // sendLEDMessage(); + sendTractorLightMessage(); brain_box_msgs::msg::VxState state_msg; state_msg.state = (uint8_t)supervisor_.system_state; vstate_summary_pub_->publish(state_msg); + + if (state == SuperState::AUTO && supervisor_.start_fp_from_super_) + { + startFlightPlan(); + } + if (state == SuperState::READY && !first_time_booted_) + { + first_time_booted_ = true; + } } } @@ -829,6 +941,29 @@ class AMSuper 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 @@ -984,6 +1119,35 @@ class AMSuperNode : public AMLifeCycle 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); + 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"; + } + else + { + 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"; + } + else if (am_super_->reset_required_) + { + system_state_msg.state_string = "RESET"; + } + } am_super_->system_state_pub_->publish(system_state_msg); } @@ -1047,7 +1211,7 @@ class AMSuperNode : public AMLifeCycle } } } - LOG_MSG("/status/super", status_msg, 1); + 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); @@ -1064,8 +1228,14 @@ class AMSuperNode : public AMLifeCycle 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 (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_->stopFlightPlan(); // This will keep sending until the all nodes are back, and that is ok for now } @@ -1075,12 +1245,123 @@ class AMSuperNode : public AMLifeCycle { // 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; + } + 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") + { + am_super_->reset_required_ = true; + super_errored_msg.reset_nodes.push_back(node.first); + } + else + { + am_super_->restart_required_ = true; + super_errored_msg.restart_nodes.push_back(node.first); + } + } + + } + 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 diff --git a/src/am_super/super_node_mediator.cpp b/src/am_super/super_node_mediator.cpp index c32aa5c8..57f56af5 100644 --- a/src/am_super/super_node_mediator.cpp +++ b/src/am_super/super_node_mediator.cpp @@ -234,7 +234,13 @@ SuperNodeMediator::StateTransition SuperNodeMediator::getStateTransition(const S 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) @@ -275,12 +281,13 @@ 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) @@ -290,68 +297,104 @@ SuperNodeMediator::TransitionInstructions SuperNodeMediator::transitionReady(Sup // 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; + // } - bool are_in_error = false; - for (pair nodePair : supervisor.nodes) - { - SuperNodeInfo node = nodePair.second; - if (node.status == LifeCycleStatus::ERROR) - { - are_in_error = true; - transition_instructions.ready_for_transition = 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) + // 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) { - bool active_true = false; - bool inactive_true = false; - for (pair nodePair : supervisor.nodes) + if (supervisor.errored_nodes_.size()==0 && allManifestedNodesCheck(supervisor, getErrorTransition().check).first) { - 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; + // 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 { - // This technically should not happen....? - transition_instructions.life_cycle_command = LifeCycleCommand::CONFIGURE; + // 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; } - 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; + + // } @@ -443,7 +486,8 @@ bool SuperNodeMediator::checkNodesShuttingDownOrFinalized(SuperNodeMediator::Sup bool SuperNodeMediator::checkErrorTransition(SuperNodeMediator::SuperNodeInfo& nr, SuperNodeMediator& node_mediator) { - return nr.state != LifeCycleState::ACTIVE; + return nr.state == LifeCycleState::INACTIVE; + // return nr.state != LifeCycleState::ACTIVE; // return nr.state == LifeCycleState::UNCONFIGURED | nr.state == LifeCycleState::INACTIVE; } @@ -490,6 +534,20 @@ bool SuperNodeMediator::checkSuperError(SuperNodeMediator::SuperNodeInfo& nr, Su } + +// bool SuperNodeMediator::allManifestedBackToInactiveCheck(Supervisor& supervisor) +// { +// for (auto nodePair : supervisor.nodes) +// { +// SuperNodeInfo node = nodePair.second; + +// if (node.manifested) +// { + +// } +// } +// } + // This does the check function on all manifested nodes. pair>> SuperNodeMediator::allManifestedNodesCheck( Supervisor& supervisor, std::function check) @@ -526,6 +584,7 @@ pair>> SuperNodeMediator::allManifestedNode 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; } From bdcc697b7b7225b97ae921fa3196c650c931edf9 Mon Sep 17 00:00:00 2001 From: HardikSingh97 Date: Sat, 12 Oct 2024 14:56:28 -0700 Subject: [PATCH 26/45] Final super changes before autostart image --- src/am_super/am_super.cpp | 87 ++++++++++++++++++++++++++++++++++++--- 1 file changed, 81 insertions(+), 6 deletions(-) diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index f22a9ec3..2fc61c87 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -234,10 +234,16 @@ class AMSuper 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 @@ -428,6 +434,9 @@ class AMSuper 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; @@ -514,6 +523,10 @@ class AMSuper } // 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; @@ -526,10 +539,19 @@ class AMSuper // 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_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); } @@ -683,6 +705,7 @@ class AMSuper msg.data = true; //false means deactivate flight_plan_deactivation_pub_->publish(msg); ROS_ERROR_STREAM( "Sending flight plan start command."); + } /** @@ -1119,6 +1142,7 @@ class AMSuperNode : public AMLifeCycle 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_) @@ -1126,10 +1150,22 @@ class AMSuperNode : public AMLifeCycle 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 { - system_state_msg.state_string = "BOOTING"; + if (am_super_->flight_plan_running_) + { + system_state_msg.state_string = "RESET"; + } + else // flight plan is NOT running + { + system_state_msg.state_string = "BOOTING"; + } } } @@ -1142,6 +1178,7 @@ class AMSuperNode : public AMLifeCycle 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_) { @@ -1237,7 +1274,8 @@ class AMSuperNode : public AMLifeCycle am_super_->offline_timer_running_ = true; } stats_->statStatus = 2; - am_super_->stopFlightPlan(); // This will keep sending until the all nodes are back, and that is ok for now + 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 } } @@ -1266,9 +1304,12 @@ class AMSuperNode : public AMLifeCycle 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("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 { @@ -1331,15 +1372,49 @@ class AMSuperNode : public AMLifeCycle 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" || node.first == "am_pilot") + if (node.first == "am_locator") { - am_super_->reset_required_ = true; - super_errored_msg.reset_nodes.push_back(node.first); + // 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(); } } From ae0ac61e853360ca18ae5443ad323f4c63d9a77e Mon Sep 17 00:00:00 2001 From: AJ Date: Mon, 25 Nov 2024 12:53:47 -0800 Subject: [PATCH 27/45] feat: update --- CMakeLists.txt | 4 +- include/am_super/system_status_class.h | 95 ++++++++ include/am_super/transform_status_class.h | 26 ++ src/am_super/am_super.cpp | 19 ++ src/am_super/system_status_class.cpp | 282 ++++++++++++++++++++++ 5 files changed, 425 insertions(+), 1 deletion(-) create mode 100644 include/am_super/system_status_class.h create mode 100644 include/am_super/transform_status_class.h create mode 100644 src/am_super/system_status_class.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index c5334b07..77111190 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,6 +30,7 @@ set(dependencies # find dependencies find_package(ament_cmake REQUIRED) +find_library(PROCPS_LIBRARY procps REQUIRED) foreach(Dependency IN ITEMS ${dependencies}) find_package(${Dependency} REQUIRED) @@ -51,7 +52,8 @@ file(GLOB SUPER_MEDIATOR_FILES src/am_super/*_mediator.cpp ) -add_executable(am_super ${am_super_cpp_files}) +add_executable(am_super ${am_super_cpp_files} src/am_super/system_status_class.cpp) +target_link_libraries(am_super ${PROCPS_LIBRARY}) ament_target_dependencies(am_super ${dependencies}) install(DIRECTORY include/ diff --git a/include/am_super/system_status_class.h b/include/am_super/system_status_class.h new file mode 100644 index 00000000..90034f0d --- /dev/null +++ b/include/am_super/system_status_class.h @@ -0,0 +1,95 @@ +#ifndef AM_SUPER_INCLUDE_SYSTEM_STATUS_CLASS_H_ +#define AM_SUPER_INCLUDE_SYSTEM_STATUS_CLASS_H_ + +#include +#include + +#include +#include +#include +#include +#include + +namespace am +{ + +struct MemoryInfo +{ + unsigned long total; + unsigned long free; + unsigned long used; + unsigned long available; +}; + +struct GpuInfo +{ + std::string gpu_name; + int temp; + int mem_used; + int mem_free; + int util_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; +}; + +class SystemStatus +{ +public: + SystemStatus(); + + ~SystemStatus(); + + am::MemoryInfo& getMemoryInfo(); + + am::CpuInfo getCPUInfo(); + + void getGPUInfo(std::vector &gpu_infos); + + void getCPUInfo(std::vector &infos); + + double calculateCpuLoad(const am::CpuInfo &ci, const am::CpuInfo &ci_old); + + double getUpTime(); + + void updateInfos(); + + void print(); + +private: + + int getCPUCoresCount(); + + am::CpuInfo parseCpuLine(const std::string &line); + + int cpu_cnt_= -1; + + double cpu_usage_; + + double uptime_seconds_; + + bool is_first_time_ {true}; + + std::vector cpu_loads_; + + am::MemoryInfo mi; + + std::vector cpu_infos_; + + std::vector cpu_infos_old_; + + std::vector gpu_infos_; +}; +} + +#endif /*AM_SUPER_INCLUDE_SYSTEM_STATUS_CLASS_H_*/ \ No newline at end of file diff --git a/include/am_super/transform_status_class.h b/include/am_super/transform_status_class.h new file mode 100644 index 00000000..9493d4f3 --- /dev/null +++ b/include/am_super/transform_status_class.h @@ -0,0 +1,26 @@ +#ifndef AM_SUPER_INCLUDE_TRANSFORM_STATUS_CLASS_H_ +#define AM_SUPER_INCLUDE_TRANSFORM_STATUS_CLASS_H_ + +#include + +namespace am +{ + +class TransformStatus +{ +public: + + TransformStatus(); + + ~TransformStatus(); + + +private: + rclcpp::TimerBase::SharedPtr check_timer_; + + void checkTimerCB(); +}; + +} + +#endif /*AM_SUPER_INCLUDE_TRANSFORM_STATUS_CLASS_H_*/ diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index 11050894..ccc18ecc 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #include #include @@ -207,6 +208,9 @@ class AMSuper rclcpp::Subscription::SharedPtr diagnostics_sub; rclcpp::Subscription::SharedPtr current_enu_sub; + rclcpp::TimerBase::SharedPtr system_check_timer_; + + rclcpp::Subscription::SharedPtr log_control_sub_; BagLogger::BagLoggerLevel log_level_; @@ -222,6 +226,8 @@ class AMSuper /** The current state of the system. */ SuperNodeMediator::Supervisor supervisor_; + std::shared_ptr system_status_; + /** * amount of time in seconds without hearing from a node that will cause it to go offline */ @@ -241,6 +247,8 @@ class AMSuper { ROS_INFO_STREAM( am::Node::node->get_name()); + system_status_ = std::make_shared(); + life_cycle_node_ = std::static_pointer_cast(am::Node::node); am::getParam("node_timeout_s", node_timeout_s_, 3.1); @@ -342,6 +350,9 @@ class AMSuper 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)); + + system_check_timer_ = am::Node::node->create_wall_timer(am::toDuration(1.0), std::bind(&AMSuper::statusTimerCB, this)); + } ~AMSuper() @@ -353,6 +364,14 @@ class AMSuper } private: + + void statusTimerCB() + { + system_status_->updateInfos(); + system_status_->print(); + } + + /** * process LifeCycleState messages from nodes * diff --git a/src/am_super/system_status_class.cpp b/src/am_super/system_status_class.cpp new file mode 100644 index 00000000..e76b8744 --- /dev/null +++ b/src/am_super/system_status_class.cpp @@ -0,0 +1,282 @@ +#include + +namespace am +{ +SystemStatus::SystemStatus() +{ + cpu_cnt_ = getCPUCoresCount(); +} + +SystemStatus::~SystemStatus() +{ + +} + +int SystemStatus::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 SystemStatus::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 SystemStatus::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; + } + + for(int i = 0; i < cpu_infos_.size(); i++) + { + cpu_loads_[i] = calculateCpuLoad(cpu_infos_[i], cpu_infos_old_[i]); + } + + uptime_seconds_ = getUpTime(); + + cpu_infos_old_ = cpu_infos_; + + getGPUInfo(gpu_infos_); +} + + +am::MemoryInfo& SystemStatus::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; + file.close(); + return mi; +} + +void SystemStatus::getGPUInfo(std::vector &gpu_infos) +{ + gpu_infos.clear(); + // 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; + } + + 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); + am::GpuInfo gpu_info; + // 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.util_percent = gpuUtilization; + gpu_info.temp = gpuTemperature; + gpu_info.mem_free = memoryFree; + gpu_info.mem_used = memoryUsed; + + gpu_infos.push_back(gpu_info); + } +} + + +void SystemStatus::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(); +} + +am::CpuInfo SystemStatus::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 SystemStatus::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 SystemStatus::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 SystemStatus::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 = ""; + for(int i = 0; i < gpu_infos_.size(); i++) + { + msg += gpu_infos_[i].gpu_name + ": Temp[C] = " + std::to_string(gpu_infos_[i].temp) + ", Used[%]: " + std::to_string(gpu_infos_[i].util_percent); + } + + ROS_INFO("%s", msg.c_str()); +} +} \ No newline at end of file From 48919bb3fdc95043abd7034d5a0c3919168608d3 Mon Sep 17 00:00:00 2001 From: AJ Date: Mon, 25 Nov 2024 13:52:06 -0800 Subject: [PATCH 28/45] feat: renamed --- CMakeLists.txt | 2 +- ...status_class.h => resource_status_class.h} | 12 ++++---- src/am_super/am_super.cpp | 10 +++---- ...us_class.cpp => resource_status_class.cpp} | 28 +++++++++---------- 4 files changed, 26 insertions(+), 26 deletions(-) rename include/am_super/{system_status_class.h => resource_status_class.h} (86%) rename src/am_super/{system_status_class.cpp => resource_status_class.cpp} (90%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 77111190..73ad0113 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,7 +52,7 @@ file(GLOB SUPER_MEDIATOR_FILES src/am_super/*_mediator.cpp ) -add_executable(am_super ${am_super_cpp_files} src/am_super/system_status_class.cpp) +add_executable(am_super ${am_super_cpp_files} src/am_super/resource_status_class.cpp) target_link_libraries(am_super ${PROCPS_LIBRARY}) ament_target_dependencies(am_super ${dependencies}) diff --git a/include/am_super/system_status_class.h b/include/am_super/resource_status_class.h similarity index 86% rename from include/am_super/system_status_class.h rename to include/am_super/resource_status_class.h index 90034f0d..49f09ecf 100644 --- a/include/am_super/system_status_class.h +++ b/include/am_super/resource_status_class.h @@ -1,5 +1,5 @@ -#ifndef AM_SUPER_INCLUDE_SYSTEM_STATUS_CLASS_H_ -#define AM_SUPER_INCLUDE_SYSTEM_STATUS_CLASS_H_ +#ifndef AM_SUPER_INCLUDE_RESOURCE_STATUS_CLASS_H_ +#define AM_SUPER_INCLUDE_RESOURCE_STATUS_CLASS_H_ #include #include @@ -43,12 +43,12 @@ struct CpuInfo unsigned long long total; }; -class SystemStatus +class ResourceStatus { public: - SystemStatus(); + ResourceStatus(); - ~SystemStatus(); + ~ResourceStatus(); am::MemoryInfo& getMemoryInfo(); @@ -92,4 +92,4 @@ class SystemStatus }; } -#endif /*AM_SUPER_INCLUDE_SYSTEM_STATUS_CLASS_H_*/ \ No newline at end of file +#endif /*AM_SUPER_INCLUDE_RESOURCE_STATUS_CLASS_H_*/ \ No newline at end of file diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index ccc18ecc..de59c47a 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -13,7 +13,7 @@ #include #include #include -#include +#include #include #include @@ -226,7 +226,7 @@ class AMSuper /** The current state of the system. */ SuperNodeMediator::Supervisor supervisor_; - std::shared_ptr system_status_; + std::shared_ptr resource_status_; /** * amount of time in seconds without hearing from a node that will cause it to go offline @@ -247,7 +247,7 @@ class AMSuper { ROS_INFO_STREAM( am::Node::node->get_name()); - system_status_ = std::make_shared(); + resource_status_ = std::make_shared(); life_cycle_node_ = std::static_pointer_cast(am::Node::node); @@ -367,8 +367,8 @@ class AMSuper void statusTimerCB() { - system_status_->updateInfos(); - system_status_->print(); + resource_status_->updateInfos(); + resource_status_->print(); } diff --git a/src/am_super/system_status_class.cpp b/src/am_super/resource_status_class.cpp similarity index 90% rename from src/am_super/system_status_class.cpp rename to src/am_super/resource_status_class.cpp index e76b8744..dac57f23 100644 --- a/src/am_super/system_status_class.cpp +++ b/src/am_super/resource_status_class.cpp @@ -1,18 +1,18 @@ -#include +#include namespace am { -SystemStatus::SystemStatus() +ResourceStatus::ResourceStatus() { cpu_cnt_ = getCPUCoresCount(); } -SystemStatus::~SystemStatus() +ResourceStatus::~ResourceStatus() { } -int SystemStatus::getCPUCoresCount() +int ResourceStatus::getCPUCoresCount() { std::ifstream file("/proc/stat"); if (!file.is_open()) { @@ -34,7 +34,7 @@ int SystemStatus::getCPUCoresCount() return coreCount; } -am::CpuInfo SystemStatus::parseCpuLine(const std::string& line) +am::CpuInfo ResourceStatus::parseCpuLine(const std::string& line) { am::CpuInfo info = {0, 0, 0, 0, 0, 0, 0, 0, 0}; std::istringstream iss(line); @@ -47,7 +47,7 @@ am::CpuInfo SystemStatus::parseCpuLine(const std::string& line) return info; } -void SystemStatus::updateInfos() +void ResourceStatus::updateInfos() { getMemoryInfo(); if(cpu_cnt_ < 0) @@ -85,7 +85,7 @@ void SystemStatus::updateInfos() } -am::MemoryInfo& SystemStatus::getMemoryInfo() +am::MemoryInfo& ResourceStatus::getMemoryInfo() { mi = {0, 0, 0}; std::ifstream file("/proc/meminfo"); @@ -121,7 +121,7 @@ am::MemoryInfo& SystemStatus::getMemoryInfo() return mi; } -void SystemStatus::getGPUInfo(std::vector &gpu_infos) +void ResourceStatus::getGPUInfo(std::vector &gpu_infos) { gpu_infos.clear(); // Execute the nvidia-smi command and read the output directly @@ -146,7 +146,7 @@ void SystemStatus::getGPUInfo(std::vector &gpu_infos) while (std::getline(iss, line)) { - ROS_INFO(GREEN "%s" COLOR_RESET, line.c_str()); + //ROS_INFO(GREEN "%s" COLOR_RESET, line.c_str()); std::istringstream lineStream(line); am::GpuInfo gpu_info; // Parse memory used and free values @@ -176,7 +176,7 @@ void SystemStatus::getGPUInfo(std::vector &gpu_infos) } -void SystemStatus::getCPUInfo(std::vector &infos) +void ResourceStatus::getCPUInfo(std::vector &infos) { std::ifstream file("/proc/stat"); if (!file.is_open()) { @@ -199,7 +199,7 @@ void SystemStatus::getCPUInfo(std::vector &infos) file.close(); } -am::CpuInfo SystemStatus::getCPUInfo() +am::CpuInfo ResourceStatus::getCPUInfo() { if(cpu_cnt_ < 0) { @@ -231,7 +231,7 @@ am::CpuInfo SystemStatus::getCPUInfo() return cpu_info; } -double SystemStatus::calculateCpuLoad(const am::CpuInfo &ci, const am::CpuInfo &ci_old) +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); @@ -241,7 +241,7 @@ double SystemStatus::calculateCpuLoad(const am::CpuInfo &ci, const am::CpuInfo & return cpu_usage_; } -double SystemStatus::getUpTime() +double ResourceStatus::getUpTime() { std::ifstream file("/proc/uptime"); if (!file.is_open()) { @@ -257,7 +257,7 @@ double SystemStatus::getUpTime() return uptime_seconds_; } -void SystemStatus::print() +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)); From d1357e275e2fda598ce7cee837986553300c0ba2 Mon Sep 17 00:00:00 2001 From: AJ Date: Mon, 25 Nov 2024 13:53:07 -0800 Subject: [PATCH 29/45] feat: update --- include/am_super/transform_status_class.h | 26 ----------------------- 1 file changed, 26 deletions(-) delete mode 100644 include/am_super/transform_status_class.h diff --git a/include/am_super/transform_status_class.h b/include/am_super/transform_status_class.h deleted file mode 100644 index 9493d4f3..00000000 --- a/include/am_super/transform_status_class.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef AM_SUPER_INCLUDE_TRANSFORM_STATUS_CLASS_H_ -#define AM_SUPER_INCLUDE_TRANSFORM_STATUS_CLASS_H_ - -#include - -namespace am -{ - -class TransformStatus -{ -public: - - TransformStatus(); - - ~TransformStatus(); - - -private: - rclcpp::TimerBase::SharedPtr check_timer_; - - void checkTimerCB(); -}; - -} - -#endif /*AM_SUPER_INCLUDE_TRANSFORM_STATUS_CLASS_H_*/ From 119118a8ae893ecadc59e8450b5591e26f8015ec Mon Sep 17 00:00:00 2001 From: AJ Date: Mon, 25 Nov 2024 14:36:51 -0800 Subject: [PATCH 30/45] feat: update --- include/am_super/resource_status_class.h | 9 +++++++ src/am_super/resource_status_class.cpp | 34 ++++++++++++++++++++++++ 2 files changed, 43 insertions(+) diff --git a/include/am_super/resource_status_class.h b/include/am_super/resource_status_class.h index 49f09ecf..8d8ac061 100644 --- a/include/am_super/resource_status_class.h +++ b/include/am_super/resource_status_class.h @@ -9,6 +9,8 @@ #include #include #include +#include + namespace am { @@ -89,6 +91,13 @@ class ResourceStatus std::vector cpu_infos_old_; std::vector gpu_infos_; + + + + /*ROS Infrastructure Checking tools*/ + std::shared_ptr transformer_; + rclcpp::TimerBase::SharedPtr timer_; + void timerCB(); }; } diff --git a/src/am_super/resource_status_class.cpp b/src/am_super/resource_status_class.cpp index dac57f23..4d3b1c61 100644 --- a/src/am_super/resource_status_class.cpp +++ b/src/am_super/resource_status_class.cpp @@ -1,9 +1,15 @@ + + #include namespace am { ResourceStatus::ResourceStatus() { + transformer_ = std::make_shared(); + + timer_ = am::Node::node->create_wall_timer(am::toDuration(1.0), std::bind(&ResourceStatus::timerCB, this)); + cpu_cnt_ = getCPUCoresCount(); } @@ -279,4 +285,32 @@ void ResourceStatus::print() ROS_INFO("%s", msg.c_str()); } + + + +void ResourceStatus::timerCB() +{ + 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) + { + string_count[str]++; + } + + // Collect strings that appear more than once + for (const auto& [str, count] : string_count) + { + if (count > 1) + { + ROS_ERROR("Found a duplicate Node: %s", str.c_str()); + } + } + + +} } \ No newline at end of file From fcd1a62ff66dbaa8108c65c6f021a1fca9c8f13a Mon Sep 17 00:00:00 2001 From: AJ Date: Mon, 25 Nov 2024 14:48:42 -0800 Subject: [PATCH 31/45] feat: udpate --- include/am_super/resource_status_class.h | 3 +-- src/am_super/resource_status_class.cpp | 14 ++++++++++++++ 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/include/am_super/resource_status_class.h b/include/am_super/resource_status_class.h index 8d8ac061..52d31199 100644 --- a/include/am_super/resource_status_class.h +++ b/include/am_super/resource_status_class.h @@ -92,10 +92,9 @@ class ResourceStatus std::vector gpu_infos_; - - /*ROS Infrastructure Checking tools*/ std::shared_ptr transformer_; + std::vector> transform_list_; rclcpp::TimerBase::SharedPtr timer_; void timerCB(); }; diff --git a/src/am_super/resource_status_class.cpp b/src/am_super/resource_status_class.cpp index 4d3b1c61..c46f0cb0 100644 --- a/src/am_super/resource_status_class.cpp +++ b/src/am_super/resource_status_class.cpp @@ -8,6 +8,10 @@ ResourceStatus::ResourceStatus() { transformer_ = std::make_shared(); + transform_list_.push_back(std::make_pair("base_link","Asset_Frame")); + transform_list_.push_back(std::make_pair("base_link","ouster_FLU")); + transform_list_.push_back(std::make_pair("base_link","Asset_ENU")); + timer_ = am::Node::node->create_wall_timer(am::toDuration(1.0), std::bind(&ResourceStatus::timerCB, this)); cpu_cnt_ = getCPUCoresCount(); @@ -311,6 +315,16 @@ void ResourceStatus::timerCB() } } + + //Transform check + 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()); + } + } } } \ No newline at end of file From 10efa3c344385e1b7d789816e02f122507229cad Mon Sep 17 00:00:00 2001 From: AJ Date: Tue, 26 Nov 2024 07:13:53 -0800 Subject: [PATCH 32/45] feat: code refactored --- CMakeLists.txt | 11 +++- .../resource_monitor/resource_monitor_node.h | 31 +++++++++ .../resource_monitor/resource_monitor_stats.h | 38 +++++++++++ .../resource_status_class.h | 26 ++++++-- src/am_super/am_super.cpp | 14 ---- .../resource_monitor_main.cpp | 28 ++++++++ .../resource_monitor_node.cpp | 65 ++++++++++++++++++ .../resource_status_class.cpp | 66 +++++++++++++++++-- 8 files changed, 253 insertions(+), 26 deletions(-) create mode 100644 include/resource_monitor/resource_monitor_node.h create mode 100644 include/resource_monitor/resource_monitor_stats.h rename include/{am_super => resource_monitor}/resource_status_class.h (67%) create mode 100644 src/resource_monitor/resource_monitor_main.cpp create mode 100644 src/resource_monitor/resource_monitor_node.cpp rename src/{am_super => resource_monitor}/resource_status_class.cpp (82%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 73ad0113..1d88066a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,7 +30,6 @@ set(dependencies # find dependencies find_package(ament_cmake REQUIRED) -find_library(PROCPS_LIBRARY procps REQUIRED) foreach(Dependency IN ITEMS ${dependencies}) find_package(${Dependency} REQUIRED) @@ -52,16 +51,22 @@ file(GLOB SUPER_MEDIATOR_FILES src/am_super/*_mediator.cpp ) -add_executable(am_super ${am_super_cpp_files} src/am_super/resource_status_class.cpp) -target_link_libraries(am_super ${PROCPS_LIBRARY}) +add_executable(am_super ${am_super_cpp_files}) +target_link_libraries(am_super) ament_target_dependencies(am_super ${dependencies}) +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}) + install(DIRECTORY include/ DESTINATION include/ ) install(TARGETS am_super + resource_monitor DESTINATION lib/${PROJECT_NAME} ) 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..8dbbff3b --- /dev/null +++ b/include/resource_monitor/resource_monitor_stats.h @@ -0,0 +1,38 @@ +#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, 100); + AMStat node_stats = AMStat("n_s", "Nodes Stats", 1, 2, 80, 100); + AMStat cpu_stats = AMStat("cpu_s", "CPU Stats", 1, 2, 80, 100); + AMStat gpu_stats = AMStat("gpu_s", "GPU Stats", 1, 2, 80, 100); + AMStat ram_stats = AMStat("ram_s", "RAM Stats", 1, 2, 80, 100); + + 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); + } +}; + +}; // namespace + +#endif /* AM_LIDAR_BS_AM_LIDAR_BS_STATS_H_ */ diff --git a/include/am_super/resource_status_class.h b/include/resource_monitor/resource_status_class.h similarity index 67% rename from include/am_super/resource_status_class.h rename to include/resource_monitor/resource_status_class.h index 52d31199..0044f614 100644 --- a/include/am_super/resource_status_class.h +++ b/include/resource_monitor/resource_status_class.h @@ -1,5 +1,5 @@ -#ifndef AM_SUPER_INCLUDE_RESOURCE_STATUS_CLASS_H_ -#define AM_SUPER_INCLUDE_RESOURCE_STATUS_CLASS_H_ +#ifndef AM_SUPER_INCLUDE_RESOURCE_MONITOR_RESOURCE_STATUS_CLASS_H_ +#define AM_SUPER_INCLUDE_RESOURCE_MONITOR_RESOURCE_STATUS_CLASS_H_ #include #include @@ -10,6 +10,8 @@ #include #include #include +#include +#include namespace am @@ -21,6 +23,7 @@ struct MemoryInfo unsigned long free; unsigned long used; unsigned long available; + int used_percent; }; struct GpuInfo @@ -48,7 +51,7 @@ struct CpuInfo class ResourceStatus { public: - ResourceStatus(); + ResourceStatus(std::shared_ptr stats); ~ResourceStatus(); @@ -68,8 +71,23 @@ class ResourceStatus void print(); + std::shared_ptr getStats(); + + // 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(); am::CpuInfo parseCpuLine(const std::string &line); @@ -100,4 +118,4 @@ class ResourceStatus }; } -#endif /*AM_SUPER_INCLUDE_RESOURCE_STATUS_CLASS_H_*/ \ No newline at end of file +#endif /*AM_SUPER_INCLUDE_RESOURCE_MONITOR_RESOURCE_STATUS_CLASS_H_*/ \ No newline at end of file diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index de59c47a..0c975cb9 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -13,7 +13,6 @@ #include #include #include -#include #include #include @@ -208,8 +207,6 @@ class AMSuper rclcpp::Subscription::SharedPtr diagnostics_sub; rclcpp::Subscription::SharedPtr current_enu_sub; - rclcpp::TimerBase::SharedPtr system_check_timer_; - rclcpp::Subscription::SharedPtr log_control_sub_; BagLogger::BagLoggerLevel log_level_; @@ -226,8 +223,6 @@ class AMSuper /** The current state of the system. */ SuperNodeMediator::Supervisor supervisor_; - std::shared_ptr resource_status_; - /** * amount of time in seconds without hearing from a node that will cause it to go offline */ @@ -247,8 +242,6 @@ class AMSuper { ROS_INFO_STREAM( am::Node::node->get_name()); - resource_status_ = std::make_shared(); - life_cycle_node_ = std::static_pointer_cast(am::Node::node); am::getParam("node_timeout_s", node_timeout_s_, 3.1); @@ -351,7 +344,6 @@ class AMSuper std::bind(&AMSuper::currentENUCB, this, std::placeholders::_1)); - system_check_timer_ = am::Node::node->create_wall_timer(am::toDuration(1.0), std::bind(&AMSuper::statusTimerCB, this)); } @@ -365,12 +357,6 @@ class AMSuper private: - void statusTimerCB() - { - resource_status_->updateInfos(); - resource_status_->print(); - } - /** * process LifeCycleState messages from nodes diff --git a/src/resource_monitor/resource_monitor_main.cpp b/src/resource_monitor/resource_monitor_main.cpp new file mode 100644 index 00000000..95068749 --- /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_minitor"); + 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/am_super/resource_status_class.cpp b/src/resource_monitor/resource_status_class.cpp similarity index 82% rename from src/am_super/resource_status_class.cpp rename to src/resource_monitor/resource_status_class.cpp index c46f0cb0..6d615475 100644 --- a/src/am_super/resource_status_class.cpp +++ b/src/resource_monitor/resource_status_class.cpp @@ -1,10 +1,10 @@ -#include +#include namespace am { -ResourceStatus::ResourceStatus() +ResourceStatus::ResourceStatus(std::shared_ptr stats) : stats_(stats) { transformer_ = std::make_shared(); @@ -22,6 +22,41 @@ ResourceStatus::~ResourceStatus() } +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"); @@ -81,11 +116,20 @@ void ResourceStatus::updateInfos() getCPUInfo(cpu_infos_old_); is_first_time_ = false; } - + double avg_load = 0.0; for(int i = 0; i < cpu_infos_.size(); i++) { - cpu_loads_[i] = calculateCpuLoad(cpu_infos_[i], cpu_infos_old_[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(); @@ -127,6 +171,8 @@ am::MemoryInfo& ResourceStatus::getMemoryInfo() // 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; } @@ -180,6 +226,7 @@ void ResourceStatus::getGPUInfo(std::vector &gpu_infos) gpu_info.temp = gpuTemperature; gpu_info.mem_free = memoryFree; gpu_info.mem_used = memoryUsed; + stats_->gpu_stats = (gpu_info.util_percent>90?100:50); gpu_infos.push_back(gpu_info); } @@ -307,24 +354,33 @@ void ResourceStatus::timerCB() } // 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); //Transform check + 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()); + //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); + + //Resource Check + updateInfos(); } } \ No newline at end of file From d19ee87ff50bb74576d88a542e5da18e6b14f713 Mon Sep 17 00:00:00 2001 From: AJ Date: Tue, 26 Nov 2024 09:16:45 -0800 Subject: [PATCH 33/45] feat: update --- include/resource_monitor/resource_monitor_stats.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/include/resource_monitor/resource_monitor_stats.h b/include/resource_monitor/resource_monitor_stats.h index 8dbbff3b..74ab0a4c 100644 --- a/include/resource_monitor/resource_monitor_stats.h +++ b/include/resource_monitor/resource_monitor_stats.h @@ -16,11 +16,11 @@ class ResourceMonitorStats AMStatStatus statStatus = AMStatStatus("ss", "AMStatStatus"); - AMStat tf_stats = AMStat("tf_s", "Transform Stats", 1, 2, 80, 100); - AMStat node_stats = AMStat("n_s", "Nodes Stats", 1, 2, 80, 100); - AMStat cpu_stats = AMStat("cpu_s", "CPU Stats", 1, 2, 80, 100); - AMStat gpu_stats = AMStat("gpu_s", "GPU Stats", 1, 2, 80, 100); - AMStat ram_stats = AMStat("ram_s", "RAM Stats", 1, 2, 80, 100); + 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); ResourceMonitorStats(AMStatList &stat_list) { From 8e06d57a8c7057b979cb4a72cf6f9bf2e6d9b06e Mon Sep 17 00:00:00 2001 From: AJ Date: Thu, 5 Dec 2024 09:09:09 -0800 Subject: [PATCH 34/45] feat: ip checks --- .../resource_monitor/resource_monitor_stats.h | 10 ++ .../resource_monitor/resource_status_class.h | 4 + .../resource_status_class.cpp | 101 ++++++++++++++++++ 3 files changed, 115 insertions(+) diff --git a/include/resource_monitor/resource_monitor_stats.h b/include/resource_monitor/resource_monitor_stats.h index 74ab0a4c..2b2519e7 100644 --- a/include/resource_monitor/resource_monitor_stats.h +++ b/include/resource_monitor/resource_monitor_stats.h @@ -21,6 +21,11 @@ class ResourceMonitorStats 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 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) { @@ -30,6 +35,11 @@ class ResourceMonitorStats stat_list.add(&gpu_stats); stat_list.add(&cpu_stats); stat_list.add(&ram_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); } }; diff --git a/include/resource_monitor/resource_status_class.h b/include/resource_monitor/resource_status_class.h index 0044f614..5e745005 100644 --- a/include/resource_monitor/resource_status_class.h +++ b/include/resource_monitor/resource_status_class.h @@ -71,6 +71,8 @@ class ResourceStatus void print(); + bool isReachable(const std::string &ipAddress, int port = 80, int timeoutSec = 1); + std::shared_ptr getStats(); // AMLifeCycle passthrus @@ -110,6 +112,8 @@ class ResourceStatus std::vector gpu_infos_; + std::map ip_addresses_; //IPAddress, Name + /*ROS Infrastructure Checking tools*/ std::shared_ptr transformer_; std::vector> transform_list_; diff --git a/src/resource_monitor/resource_status_class.cpp b/src/resource_monitor/resource_status_class.cpp index 6d615475..e91267f3 100644 --- a/src/resource_monitor/resource_status_class.cpp +++ b/src/resource_monitor/resource_status_class.cpp @@ -1,6 +1,11 @@ #include +#include +#include // For memset +#include // For socket functions +#include // For inet_addr and sockaddr_in +#include // For close namespace am { @@ -12,6 +17,15 @@ ResourceStatus::ResourceStatus(std::shared_ptr stats) transform_list_.push_back(std::make_pair("base_link","ouster_FLU")); transform_list_.push_back(std::make_pair("base_link","Asset_ENU")); + ip_addresses_["192.168.1.55"] = std::string("lidar"); + ip_addresses_["192.168.1.10"] = std::string("front_left"); + ip_addresses_["192.168.1.20"] = std::string("front_right"); + ip_addresses_["192.168.1.30"] = std::string("rear_right"); + ip_addresses_["192.168.1.40"] = std::string("rear_left"); + + + + timer_ = am::Node::node->create_wall_timer(am::toDuration(1.0), std::bind(&ResourceStatus::timerCB, this)); cpu_cnt_ = getCPUCoresCount(); @@ -256,6 +270,46 @@ void ResourceStatus::getCPUInfo(std::vector &infos) file.close(); } +bool ResourceStatus::isReachable(const std::string &ip_address, int port, int timeoutSec) +{ + /*std::string command = std::string("ping -c 1 ") + ip_address + std::string(" >/dev/null 2>&1"); + + int result = std::system(command.c_str()); + + return result == 0;*/ + + int sockfd; + struct sockaddr_in serverAddr; + + // Create a socket + sockfd = socket(AF_INET, SOCK_STREAM, 0); + if (sockfd < 0) { + std::cerr << "Error: Cannot create socket\n"; + return false; + } + + // Set socket timeout + struct timeval timeout; + timeout.tv_sec = timeoutSec; + timeout.tv_usec = 0; + setsockopt(sockfd, SOL_SOCKET, SO_RCVTIMEO, (const char*)&timeout, sizeof(timeout)); + setsockopt(sockfd, SOL_SOCKET, SO_SNDTIMEO, (const char*)&timeout, sizeof(timeout)); + + // Set up the server address struct + memset(&serverAddr, 0, sizeof(serverAddr)); + serverAddr.sin_family = AF_INET; + serverAddr.sin_port = htons(port); + serverAddr.sin_addr.s_addr = inet_addr(ip_address.c_str()); + + // Attempt to connect + bool reachable = (connect(sockfd, (struct sockaddr*)&serverAddr, sizeof(serverAddr)) == 0); + + // Close the socket + close(sockfd); + + return reachable; +} + am::CpuInfo ResourceStatus::getCPUInfo() { if(cpu_cnt_ < 0) @@ -350,6 +404,10 @@ void ResourceStatus::timerCB() // Count occurrences of each string for (const std::string& str : running_nodes) { + if(str.find("plugin_name") != std::string::npos) + { + continue; + } string_count[str]++; } @@ -379,6 +437,49 @@ void ResourceStatus::timerCB() } stats_->tf_stats = (tf_check?50:100); + + //IP Address Check + stats_->lidar_ip = 50; + stats_->fl_ip = 50; + stats_->fr_ip = 50; + stats_->rl_ip = 50; + stats_->rr_ip = 50; + + 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"); + } + } + } + //Resource Check updateInfos(); From 42d5c3a74559af0329c1384d4d861a3966b95960 Mon Sep 17 00:00:00 2001 From: AJ Date: Thu, 5 Dec 2024 12:26:58 -0800 Subject: [PATCH 35/45] feat: update --- .../resource_monitor/resource_status_class.h | 13 +- .../resource_status_class.cpp | 187 +++++++++++------- 2 files changed, 131 insertions(+), 69 deletions(-) diff --git a/include/resource_monitor/resource_status_class.h b/include/resource_monitor/resource_status_class.h index 5e745005..7b9bc567 100644 --- a/include/resource_monitor/resource_status_class.h +++ b/include/resource_monitor/resource_status_class.h @@ -71,13 +71,19 @@ class ResourceStatus void print(); - bool isReachable(const std::string &ipAddress, int port = 80, int timeoutSec = 1); + bool isReachable(const std::string &ipAddress); + + 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: @@ -85,9 +91,11 @@ class ResourceStatus 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(); @@ -116,8 +124,11 @@ class ResourceStatus /*ROS Infrastructure Checking tools*/ std::shared_ptr transformer_; + std::vector> transform_list_; + rclcpp::TimerBase::SharedPtr timer_; + void timerCB(); }; } diff --git a/src/resource_monitor/resource_status_class.cpp b/src/resource_monitor/resource_status_class.cpp index e91267f3..2ef4db23 100644 --- a/src/resource_monitor/resource_status_class.cpp +++ b/src/resource_monitor/resource_status_class.cpp @@ -2,10 +2,9 @@ #include #include -#include // For memset -#include // For socket functions -#include // For inet_addr and sockaddr_in -#include // For close +#include // For popen and fgets +#include // For std::unique_ptr +#include // For std::regex namespace am { @@ -270,44 +269,13 @@ void ResourceStatus::getCPUInfo(std::vector &infos) file.close(); } -bool ResourceStatus::isReachable(const std::string &ip_address, int port, int timeoutSec) +bool ResourceStatus::isReachable(const std::string &ip_address) { - /*std::string command = std::string("ping -c 1 ") + ip_address + std::string(" >/dev/null 2>&1"); + std::string command = std::string("ping -c 1 ") + ip_address + std::string(" >/dev/null 2>&1"); int result = std::system(command.c_str()); - return result == 0;*/ - - int sockfd; - struct sockaddr_in serverAddr; - - // Create a socket - sockfd = socket(AF_INET, SOCK_STREAM, 0); - if (sockfd < 0) { - std::cerr << "Error: Cannot create socket\n"; - return false; - } - - // Set socket timeout - struct timeval timeout; - timeout.tv_sec = timeoutSec; - timeout.tv_usec = 0; - setsockopt(sockfd, SOL_SOCKET, SO_RCVTIMEO, (const char*)&timeout, sizeof(timeout)); - setsockopt(sockfd, SOL_SOCKET, SO_SNDTIMEO, (const char*)&timeout, sizeof(timeout)); - - // Set up the server address struct - memset(&serverAddr, 0, sizeof(serverAddr)); - serverAddr.sin_family = AF_INET; - serverAddr.sin_port = htons(port); - serverAddr.sin_addr.s_addr = inet_addr(ip_address.c_str()); - - // Attempt to connect - bool reachable = (connect(sockfd, (struct sockaddr*)&serverAddr, sizeof(serverAddr)) == 0); - - // Close the socket - close(sockfd); - - return reachable; + return result == 0; } am::CpuInfo ResourceStatus::getCPUInfo() @@ -438,50 +406,133 @@ void ResourceStatus::timerCB() stats_->tf_stats = (tf_check?50:100); + + //todo: this should be static and checked once or should be passed as argument depending on the architecture: for sim env this is false + bool ips_should_exists = false; + std::vector 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") + { + ips_should_exists = true; + } + } + //IP Address Check stats_->lidar_ip = 50; stats_->fl_ip = 50; stats_->fr_ip = 50; stats_->rl_ip = 50; stats_->rr_ip = 50; - - std::map::iterator it = ip_addresses_.begin(); - for(; it != ip_addresses_.end(); ++it) + //Only if you have the subnet + if(ips_should_exists) { - - if(!isReachable(it->first)) + std::unordered_set available_ips = getActiveIPs(); + std::map::iterator it = ip_addresses_.begin(); + for(; it != ip_addresses_.end(); ++it) { //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") + if(available_ips.find(it->first) == available_ips.end()) { - stats_->rl_ip = 100; - ROS_ERROR("Rear Left Camera is not reachable"); - } + 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"); + } + } } } - //Resource Check updateInfos(); } + + +// 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; +} } \ No newline at end of file From 3520e7c95f7f7fc313787d971888d8614f44298d Mon Sep 17 00:00:00 2001 From: AJ Date: Thu, 5 Dec 2024 13:46:31 -0800 Subject: [PATCH 36/45] feat: update --- .../resource_monitor/resource_status_class.h | 8 ++ .../resource_status_class.cpp | 96 ++++++++++++++----- 2 files changed, 81 insertions(+), 23 deletions(-) diff --git a/include/resource_monitor/resource_status_class.h b/include/resource_monitor/resource_status_class.h index 7b9bc567..5a77e4ef 100644 --- a/include/resource_monitor/resource_status_class.h +++ b/include/resource_monitor/resource_status_class.h @@ -73,6 +73,8 @@ class ResourceStatus 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(); @@ -130,6 +132,12 @@ class ResourceStatus rclcpp::TimerBase::SharedPtr timer_; void timerCB(); + + void checkNodeNames(); + + void checkTransforms(); + + void checkSensorIPs(); }; } diff --git a/src/resource_monitor/resource_status_class.cpp b/src/resource_monitor/resource_status_class.cpp index 2ef4db23..63acecd7 100644 --- a/src/resource_monitor/resource_status_class.cpp +++ b/src/resource_monitor/resource_status_class.cpp @@ -12,18 +12,7 @@ ResourceStatus::ResourceStatus(std::shared_ptr stats) { transformer_ = std::make_shared(); - transform_list_.push_back(std::make_pair("base_link","Asset_Frame")); - transform_list_.push_back(std::make_pair("base_link","ouster_FLU")); - transform_list_.push_back(std::make_pair("base_link","Asset_ENU")); - - ip_addresses_["192.168.1.55"] = std::string("lidar"); - ip_addresses_["192.168.1.10"] = std::string("front_left"); - ip_addresses_["192.168.1.20"] = std::string("front_right"); - ip_addresses_["192.168.1.30"] = std::string("rear_right"); - ip_addresses_["192.168.1.40"] = std::string("rear_left"); - - - + getParams(); timer_ = am::Node::node->create_wall_timer(am::toDuration(1.0), std::bind(&ResourceStatus::timerCB, this)); @@ -35,6 +24,50 @@ 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_; @@ -43,6 +76,7 @@ std::shared_ptr ResourceStatus::getStats() 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; @@ -359,9 +393,7 @@ void ResourceStatus::print() ROS_INFO("%s", msg.c_str()); } - - -void ResourceStatus::timerCB() +void ResourceStatus::checkNodeNames() { rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph = am::Node::node->get_node_graph_interface(); @@ -390,9 +422,10 @@ void ResourceStatus::timerCB() } } stats_->node_stats = (node_check?50:100); +} - - //Transform check +void ResourceStatus::checkTransforms() +{ bool tf_check = true; for(std::pair &tf_str : transform_list_) { @@ -404,15 +437,16 @@ void ResourceStatus::timerCB() } } stats_->tf_stats = (tf_check?50:100); +} - - +void ResourceStatus::checkSensorIPs() +{ //todo: this should be static and checked once or should be passed as argument depending on the architecture: for sim env this is false bool ips_should_exists = false; std::vector sub_nets_add = getInetAddresses(); for(const std::string &ip : sub_nets_add) { - ROS_INFO("subnet: %s", ip.c_str()); + //ROS_INFO("subnet: %s", ip.c_str()); if(ip == "192.168.1.1") { ips_should_exists = true; @@ -463,9 +497,6 @@ void ResourceStatus::timerCB() } } } - //Resource Check - updateInfos(); - } @@ -535,4 +566,23 @@ std::vector ResourceStatus::getInetAddresses() return inetAddresses; } + +/* + 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 From cbf706a9db5cd2bd19da8cb521710459aaefb070 Mon Sep 17 00:00:00 2001 From: AJ Date: Thu, 5 Dec 2024 14:50:42 -0800 Subject: [PATCH 37/45] feat: update --- src/resource_monitor/resource_monitor_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/resource_monitor/resource_monitor_main.cpp b/src/resource_monitor/resource_monitor_main.cpp index 95068749..81a4f729 100644 --- a/src/resource_monitor/resource_monitor_main.cpp +++ b/src/resource_monitor/resource_monitor_main.cpp @@ -10,7 +10,7 @@ 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_minitor"); + 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; From 4e8d59c50f88f764549e1cbc2280cf4e79e702ad Mon Sep 17 00:00:00 2001 From: AJ Date: Fri, 6 Dec 2024 10:23:49 -0800 Subject: [PATCH 38/45] feat: update --- .../resource_monitor/resource_monitor_stats.h | 2 + .../resource_monitor/resource_status_class.h | 21 +++- .../resource_status_class.cpp | 117 +++++++++++++++--- 3 files changed, 118 insertions(+), 22 deletions(-) diff --git a/include/resource_monitor/resource_monitor_stats.h b/include/resource_monitor/resource_monitor_stats.h index 2b2519e7..45885f6c 100644 --- a/include/resource_monitor/resource_monitor_stats.h +++ b/include/resource_monitor/resource_monitor_stats.h @@ -21,6 +21,7 @@ class ResourceMonitorStats 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); @@ -35,6 +36,7 @@ class ResourceMonitorStats 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); diff --git a/include/resource_monitor/resource_status_class.h b/include/resource_monitor/resource_status_class.h index 5a77e4ef..479f221e 100644 --- a/include/resource_monitor/resource_status_class.h +++ b/include/resource_monitor/resource_status_class.h @@ -12,6 +12,8 @@ #include #include #include +#include // For statvfs +#include // For std::setprecision namespace am @@ -30,9 +32,7 @@ struct GpuInfo { std::string gpu_name; int temp; - int mem_used; - int mem_free; - int util_percent; + int load_percent; }; struct CpuInfo @@ -48,6 +48,13 @@ struct CpuInfo 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: @@ -59,10 +66,12 @@ class ResourceStatus am::CpuInfo getCPUInfo(); - void getGPUInfo(std::vector &gpu_infos); + 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(); @@ -102,6 +111,8 @@ class ResourceStatus int getCPUCoresCount(); + std::string readFile(const std::string& path); + am::CpuInfo parseCpuLine(const std::string &line); int cpu_cnt_= -1; @@ -120,7 +131,7 @@ class ResourceStatus std::vector cpu_infos_old_; - std::vector gpu_infos_; + am::GpuInfo gpu_info_; std::map ip_addresses_; //IPAddress, Name diff --git a/src/resource_monitor/resource_status_class.cpp b/src/resource_monitor/resource_status_class.cpp index 63acecd7..36740d32 100644 --- a/src/resource_monitor/resource_status_class.cpp +++ b/src/resource_monitor/resource_status_class.cpp @@ -5,6 +5,7 @@ #include // For popen and fgets #include // For std::unique_ptr #include // For std::regex +#include namespace am { @@ -182,7 +183,26 @@ void ResourceStatus::updateInfos() cpu_infos_old_ = cpu_infos_; - getGPUInfo(gpu_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); + } + } @@ -224,21 +244,69 @@ am::MemoryInfo& ResourceStatus::getMemoryInfo() return mi; } -void ResourceStatus::getGPUInfo(std::vector &gpu_infos) +// Function to read the content of a file +std::string ResourceStatus::readFile(const std::string& path) { - gpu_infos.clear(); + 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"; + + 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; + std::string type = readFile(typePath); + if (type.find("GPU") != std::string::npos) + { // Look for the GPU thermal zone + 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 + } + } catch (...) { + // Ignore errors and continue checking other zones + } + } + throw std::runtime_error("Error: GPU thermal zone not found."); + + 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; + return gpu_info; } char buffer[128]; std::ostringstream result; - while (fgets(buffer, sizeof(buffer), pipe) != nullptr) { + while (fgets(buffer, sizeof(buffer), pipe) != nullptr) + { result << buffer; } pclose(pipe); @@ -248,10 +316,8 @@ void ResourceStatus::getGPUInfo(std::vector &gpu_infos) std::string line; while (std::getline(iss, line)) { - //ROS_INFO(GREEN "%s" COLOR_RESET, line.c_str()); std::istringstream lineStream(line); - am::GpuInfo gpu_info; // Parse memory used and free values std::string gpuName; int gpuUtilization, gpuTemperature, memoryUsed, memoryFree; @@ -269,14 +335,15 @@ void ResourceStatus::getGPUInfo(std::vector &gpu_infos) lineStream >> memoryFree; gpu_info.gpu_name = gpuName; - gpu_info.util_percent = gpuUtilization; + gpu_info.load_percent = gpuUtilization; gpu_info.temp = gpuTemperature; - gpu_info.mem_free = memoryFree; - gpu_info.mem_used = memoryUsed; - stats_->gpu_stats = (gpu_info.util_percent>90?100:50); - - gpu_infos.push_back(gpu_info); + + return gpu_info; } + + + + return gpu_info; } @@ -385,10 +452,7 @@ void ResourceStatus::print() ROS_INFO("UpTime: %f", uptime_seconds_); msg = ""; - for(int i = 0; i < gpu_infos_.size(); i++) - { - msg += gpu_infos_[i].gpu_name + ": Temp[C] = " + std::to_string(gpu_infos_[i].temp) + ", Used[%]: " + std::to_string(gpu_infos_[i].util_percent); - } + msg += "GPU: Temp[C] = " + std::to_string(gpu_info_.temp) + ", Used[%]: " + std::to_string(gpu_info_.load_percent); ROS_INFO("%s", msg.c_str()); } @@ -567,6 +631,25 @@ std::vector ResourceStatus::getInetAddresses() 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 */ From c5a8c2bfbb5f9bf6e242c29f81ec5b4670fd9133 Mon Sep 17 00:00:00 2001 From: AJ Date: Fri, 6 Dec 2024 10:31:39 -0800 Subject: [PATCH 39/45] feat: update --- src/resource_monitor/resource_status_class.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/resource_monitor/resource_status_class.cpp b/src/resource_monitor/resource_status_class.cpp index 36740d32..ee393deb 100644 --- a/src/resource_monitor/resource_status_class.cpp +++ b/src/resource_monitor/resource_status_class.cpp @@ -277,6 +277,7 @@ am::GpuInfo ResourceStatus::getGPUInfo() 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()); std::string type = readFile(typePath); if (type.find("GPU") != std::string::npos) { // Look for the GPU thermal zone From af0fa9b5b4ba99d552fd902fd1237d56e71e4ad7 Mon Sep 17 00:00:00 2001 From: AJ Date: Fri, 6 Dec 2024 10:35:55 -0800 Subject: [PATCH 40/45] feat: update --- .../resource_status_class.cpp | 32 ++++++++++++------- 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/src/resource_monitor/resource_status_class.cpp b/src/resource_monitor/resource_status_class.cpp index ee393deb..f622c615 100644 --- a/src/resource_monitor/resource_status_class.cpp +++ b/src/resource_monitor/resource_status_class.cpp @@ -274,22 +274,30 @@ am::GpuInfo ResourceStatus::getGPUInfo() const std::string typeSuffix = "/type"; const std::string tempSuffix = "/temp"; - for (int i = 0; i < 10; ++i) { // Check up to 10 thermal zones + 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()); - std::string type = readFile(typePath); - if (type.find("GPU") != std::string::npos) - { // Look for the GPU thermal zone - 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 - } - } catch (...) { + 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 } } - throw std::runtime_error("Error: GPU thermal zone not found."); return gpu_info; } From bdbfa046d4cdb149c3670254aa5e6b30859b06fd Mon Sep 17 00:00:00 2001 From: AJ Date: Fri, 6 Dec 2024 10:54:18 -0800 Subject: [PATCH 41/45] feat: update --- include/resource_monitor/resource_status_class.h | 2 ++ src/resource_monitor/resource_status_class.cpp | 6 ++++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/include/resource_monitor/resource_status_class.h b/include/resource_monitor/resource_status_class.h index 479f221e..6f66e6d9 100644 --- a/include/resource_monitor/resource_status_class.h +++ b/include/resource_monitor/resource_status_class.h @@ -133,6 +133,8 @@ class ResourceStatus am::GpuInfo gpu_info_; + std::vector sub_nets_add_; + std::map ip_addresses_; //IPAddress, Name /*ROS Infrastructure Checking tools*/ diff --git a/src/resource_monitor/resource_status_class.cpp b/src/resource_monitor/resource_status_class.cpp index f622c615..8ca3eff1 100644 --- a/src/resource_monitor/resource_status_class.cpp +++ b/src/resource_monitor/resource_status_class.cpp @@ -13,6 +13,8 @@ ResourceStatus::ResourceStatus(std::shared_ptr stats) { transformer_ = std::make_shared(); + sub_nets_add_ = getInetAddresses(); + getParams(); timer_ = am::Node::node->create_wall_timer(am::toDuration(1.0), std::bind(&ResourceStatus::timerCB, this)); @@ -516,8 +518,8 @@ void ResourceStatus::checkSensorIPs() { //todo: this should be static and checked once or should be passed as argument depending on the architecture: for sim env this is false bool ips_should_exists = false; - std::vector sub_nets_add = getInetAddresses(); - for(const std::string &ip : sub_nets_add) + + for(const std::string &ip : sub_nets_add_) { //ROS_INFO("subnet: %s", ip.c_str()); if(ip == "192.168.1.1") From 282fcb7dacb724b38a6071799d5fa3631e81a0aa Mon Sep 17 00:00:00 2001 From: AJ Date: Fri, 6 Dec 2024 10:55:43 -0800 Subject: [PATCH 42/45] feat: update --- src/resource_monitor/resource_status_class.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/resource_monitor/resource_status_class.cpp b/src/resource_monitor/resource_status_class.cpp index 8ca3eff1..b944b441 100644 --- a/src/resource_monitor/resource_status_class.cpp +++ b/src/resource_monitor/resource_status_class.cpp @@ -14,6 +14,7 @@ ResourceStatus::ResourceStatus(std::shared_ptr stats) transformer_ = std::make_shared(); sub_nets_add_ = getInetAddresses(); + getParams(); From 51ded3864088a555ac8d5cf25012e9c09ad88e6c Mon Sep 17 00:00:00 2001 From: AJ Date: Fri, 6 Dec 2024 13:13:36 -0800 Subject: [PATCH 43/45] feat: using the old ping method with better options --- .../resource_monitor/resource_status_class.h | 2 ++ .../resource_status_class.cpp | 35 +++++++++---------- 2 files changed, 18 insertions(+), 19 deletions(-) diff --git a/include/resource_monitor/resource_status_class.h b/include/resource_monitor/resource_status_class.h index 6f66e6d9..0f24eb66 100644 --- a/include/resource_monitor/resource_status_class.h +++ b/include/resource_monitor/resource_status_class.h @@ -121,6 +121,8 @@ class ResourceStatus double uptime_seconds_; + bool ip_check_ {false}; + bool is_first_time_ {true}; std::vector cpu_loads_; diff --git a/src/resource_monitor/resource_status_class.cpp b/src/resource_monitor/resource_status_class.cpp index b944b441..343cda85 100644 --- a/src/resource_monitor/resource_status_class.cpp +++ b/src/resource_monitor/resource_status_class.cpp @@ -14,8 +14,16 @@ ResourceStatus::ResourceStatus(std::shared_ptr 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; + } + } + getParams(); timer_ = am::Node::node->create_wall_timer(am::toDuration(1.0), std::bind(&ResourceStatus::timerCB, this)); @@ -384,7 +392,7 @@ void ResourceStatus::getCPUInfo(std::vector &infos) bool ResourceStatus::isReachable(const std::string &ip_address) { - std::string command = std::string("ping -c 1 ") + ip_address + std::string(" >/dev/null 2>&1"); + 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()); @@ -516,18 +524,7 @@ void ResourceStatus::checkTransforms() } void ResourceStatus::checkSensorIPs() -{ - //todo: this should be static and checked once or should be passed as argument depending on the architecture: for sim env this is false - bool ips_should_exists = false; - - for(const std::string &ip : sub_nets_add_) - { - //ROS_INFO("subnet: %s", ip.c_str()); - if(ip == "192.168.1.1") - { - ips_should_exists = true; - } - } +{ //IP Address Check stats_->lidar_ip = 50; @@ -536,15 +533,15 @@ void ResourceStatus::checkSensorIPs() stats_->rl_ip = 50; stats_->rr_ip = 50; //Only if you have the subnet - if(ips_should_exists) + if(ip_check_) { - std::unordered_set available_ips = getActiveIPs(); std::map::iterator it = ip_addresses_.begin(); for(; it != ip_addresses_.end(); ++it) { - //THE DEVICE CANNOT BE REACHED - if(available_ips.find(it->first) == available_ips.end()) + + if(!isReachable(it->first)) { + //THE DEVICE CANNOT BE REACHED if(it->second == "lidar") { stats_->lidar_ip = 100; @@ -569,7 +566,7 @@ void ResourceStatus::checkSensorIPs() { stats_->rl_ip = 100; ROS_ERROR("Rear Left Camera is not reachable"); - } + } } } } From 6e4373844cde318503e5b7604b5783db239be571 Mon Sep 17 00:00:00 2001 From: AJ Date: Fri, 6 Dec 2024 13:24:01 -0800 Subject: [PATCH 44/45] feat: less trace --- src/resource_monitor/resource_status_class.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/resource_monitor/resource_status_class.cpp b/src/resource_monitor/resource_status_class.cpp index 343cda85..2e94962d 100644 --- a/src/resource_monitor/resource_status_class.cpp +++ b/src/resource_monitor/resource_status_class.cpp @@ -21,6 +21,7 @@ ResourceStatus::ResourceStatus(std::shared_ptr stats) if(ip == "192.168.1.1") { ip_check_ = true; + ROS_INFO("Resource Monitor: looking for sensors on 192.168.1.1"); } } @@ -196,7 +197,7 @@ void ResourceStatus::updateInfos() gpu_info_ = getGPUInfo(); stats_->gpu_stats = 50; - ROS_INFO("GPU Load Percent: %d , Temp: %d", gpu_info_.load_percent, gpu_info_.temp); + //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; @@ -290,7 +291,7 @@ am::GpuInfo ResourceStatus::getGPUInfo() { // 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()); + //ROS_INFO("Type file: %s", typePath.c_str()); if(!boost::filesystem::exists(typePath)) { continue; From 742cb8d5e97781bf21db04530c1812768dd04cc1 Mon Sep 17 00:00:00 2001 From: Peter Chow <57922485+shpydah@users.noreply.github.com> Date: Wed, 12 Mar 2025 10:39:36 -0700 Subject: [PATCH 45/45] Create LICENSE --- LICENSE | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 LICENSE 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.