From fc4f31af7f4ae39438730ab7c7b64a8d3c7a12bb Mon Sep 17 00:00:00 2001 From: AJ Date: Mon, 19 Dec 2022 22:54:32 -0800 Subject: [PATCH 01/11] 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/11] 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/11] 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/11] 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/11] 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/11] 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/11] 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/11] 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/11] 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/11] 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/11] 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_;