From 017517dc6f328ba197b193a2fb3ce0d734fb6fad Mon Sep 17 00:00:00 2001 From: "myriam.lacroix.1@ens.etsmtl.ca" Date: Fri, 21 Oct 2022 12:30:37 -0400 Subject: [PATCH 01/10] Create node to receive and handle heartbeats. --- markhor_heartbeat/CMakeLists.txt | 207 ++++++++++++++++++++++ markhor_heartbeat/launch/heartbeat.launch | 6 + markhor_heartbeat/package.xml | 62 +++++++ markhor_heartbeat/src/heartbeat.cpp | 43 +++++ 4 files changed, 318 insertions(+) create mode 100644 markhor_heartbeat/CMakeLists.txt create mode 100755 markhor_heartbeat/launch/heartbeat.launch create mode 100644 markhor_heartbeat/package.xml create mode 100755 markhor_heartbeat/src/heartbeat.cpp diff --git a/markhor_heartbeat/CMakeLists.txt b/markhor_heartbeat/CMakeLists.txt new file mode 100644 index 0000000..acebc6d --- /dev/null +++ b/markhor_heartbeat/CMakeLists.txt @@ -0,0 +1,207 @@ +cmake_minimum_required(VERSION 3.0.2) +project(markhor_heartbeat) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## 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 COMPONENTS + roscpp +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## 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 ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## 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 # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## 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 markhor_heartbeat + CATKIN_DEPENDS roscpp +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + src + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/markhor_heartbeat.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/markhor_heartbeat_node.cpp) +add_executable(${PROJECT_NAME}_node src/heartbeat.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME}_node + ${catkin_LIBRARIES} +) + +############# +## 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 +# catkin_install_python(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}_node +# 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 ${PROJECT_NAME} +# 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/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_markhor_heartbeat.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/markhor_heartbeat/launch/heartbeat.launch b/markhor_heartbeat/launch/heartbeat.launch new file mode 100755 index 0000000..abd5db6 --- /dev/null +++ b/markhor_heartbeat/launch/heartbeat.launch @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/markhor_heartbeat/package.xml b/markhor_heartbeat/package.xml new file mode 100644 index 0000000..c9be667 --- /dev/null +++ b/markhor_heartbeat/package.xml @@ -0,0 +1,62 @@ + + + markhor_heartbeat + 0.0.0 + The markhor_heartbeat package + + + + + Myriam + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + roscpp + roscpp + + + + + + + + diff --git a/markhor_heartbeat/src/heartbeat.cpp b/markhor_heartbeat/src/heartbeat.cpp new file mode 100755 index 0000000..4740a15 --- /dev/null +++ b/markhor_heartbeat/src/heartbeat.cpp @@ -0,0 +1,43 @@ +#include +#include +#include + +int heartbeat_timeout; +int heartbeat_interval; +std::chrono::_V2::system_clock::time_point last_heartbeat = std::chrono::system_clock::now(); + +void heartbeat(const std_msgs::String message) +{ + last_heartbeat = std::chrono::system_clock::now(); +} + +void check_heartbeat(const ros::TimerEvent& e) { + std::chrono::_V2::system_clock::time_point now = std::chrono::system_clock::now(); + std::chrono::duration since_last_heartbeat = now - last_heartbeat; + + if (since_last_heartbeat.count() > heartbeat_timeout) { + ROS_WARN("Heartbeat timeout! Shutting down."); + // TODO : Estop? + } +} + +int main(int argc, char* argv[]) +{ + ros::init(argc, argv, "markhor_heartbeat_node"); + ros::NodeHandle nh; + + // Get parameters from launch file. + nh.getParam("/markhor/heartbeat/markhor_heartbeat_node/heartbeat_interval", heartbeat_interval); + nh.getParam("/markhor/heartbeat/markhor_heartbeat_node/heartbeat_timeout", heartbeat_timeout); + + ROS_INFO("Heartbeat interval is : %d ms", heartbeat_interval); + ROS_INFO("Heartbeat timeout is : %d ms", heartbeat_timeout); + + // Subscribe to heartbeat topic to receive heartbeats from web ui. + ros::Subscriber heartbeat_subscriber = nh.subscribe("/heartbeat", 5, heartbeat); + + // Create timer to verify that we received a heartbeat. + ros::Timer timer1 = nh.createTimer(ros::Duration(heartbeat_interval/1000), check_heartbeat); + + ros::spin(); +} \ No newline at end of file From 61427e0df0bdb171c62ee7d8bdda002f6958158a Mon Sep 17 00:00:00 2001 From: myriamlacroix Date: Fri, 21 Oct 2022 12:34:22 -0400 Subject: [PATCH 02/10] Create node to receive/handle heartbeats. --- markhor_heartbeat/launch/heartbeat.launch | 0 markhor_heartbeat/src/heartbeat.cpp | 0 2 files changed, 0 insertions(+), 0 deletions(-) mode change 100755 => 100644 markhor_heartbeat/launch/heartbeat.launch mode change 100755 => 100644 markhor_heartbeat/src/heartbeat.cpp diff --git a/markhor_heartbeat/launch/heartbeat.launch b/markhor_heartbeat/launch/heartbeat.launch old mode 100755 new mode 100644 diff --git a/markhor_heartbeat/src/heartbeat.cpp b/markhor_heartbeat/src/heartbeat.cpp old mode 100755 new mode 100644 From 4007c74eb20390ee668d678d08efd466e2c4bb8f Mon Sep 17 00:00:00 2001 From: myriamlacroix Date: Fri, 18 Nov 2022 12:03:54 -0500 Subject: [PATCH 03/10] Estop on heartbeat timeout. --- markhor_bringup/launch/markhor_common.launch | 1 + markhor_heartbeat/src/heartbeat.cpp | 23 ++++++++++++++++---- 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/markhor_bringup/launch/markhor_common.launch b/markhor_bringup/launch/markhor_common.launch index 432c7e9..73c0a3e 100644 --- a/markhor_bringup/launch/markhor_common.launch +++ b/markhor_bringup/launch/markhor_common.launch @@ -4,4 +4,5 @@ + \ No newline at end of file diff --git a/markhor_heartbeat/src/heartbeat.cpp b/markhor_heartbeat/src/heartbeat.cpp index 4740a15..57d6d47 100644 --- a/markhor_heartbeat/src/heartbeat.cpp +++ b/markhor_heartbeat/src/heartbeat.cpp @@ -1,7 +1,9 @@ #include +#include "std_srvs/Trigger.h" #include #include +bool stopped = false; int heartbeat_timeout; int heartbeat_interval; std::chrono::_V2::system_clock::time_point last_heartbeat = std::chrono::system_clock::now(); @@ -11,13 +13,26 @@ void heartbeat(const std_msgs::String message) last_heartbeat = std::chrono::system_clock::now(); } -void check_heartbeat(const ros::TimerEvent& e) { +void check_heartbeat(const ros::TimerEvent& e, ros::NodeHandle nh) { std::chrono::_V2::system_clock::time_point now = std::chrono::system_clock::now(); std::chrono::duration since_last_heartbeat = now - last_heartbeat; - if (since_last_heartbeat.count() > heartbeat_timeout) { + if (stopped && since_last_heartbeat.count() < heartbeat_timeout) { + ROS_WARN("Connection restored."); + stopped = false; + + std_srvs::Trigger service; + if (ros::service::exists("/markhor/estop_enable", true)) { + ros::service::call("/markhor/estop_enable", service); + } + } else if (!stopped && since_last_heartbeat.count() > heartbeat_timeout) { ROS_WARN("Heartbeat timeout! Shutting down."); - // TODO : Estop? + stopped = true; + + std_srvs::Trigger service; + if (ros::service::exists("/markhor/estop_disable", true)) { + ros::service::call("/markhor/estop_disable", service); + } } } @@ -37,7 +52,7 @@ int main(int argc, char* argv[]) ros::Subscriber heartbeat_subscriber = nh.subscribe("/heartbeat", 5, heartbeat); // Create timer to verify that we received a heartbeat. - ros::Timer timer1 = nh.createTimer(ros::Duration(heartbeat_interval/1000), check_heartbeat); + ros::Timer timer1 = nh.createTimer(ros::Duration(heartbeat_interval/1000), boost::bind(check_heartbeat, _1, nh)); ros::spin(); } \ No newline at end of file From 8a9d233e658a9b6aa637d04f73e6fa1f016df39d Mon Sep 17 00:00:00 2001 From: GLDuval Date: Mon, 16 Jan 2023 13:06:38 -0500 Subject: [PATCH 04/10] Fix build, change interval and timeout rate --- markhor_heartbeat/CMakeLists.txt | 193 +--------------------- markhor_heartbeat/launch/heartbeat.launch | 4 +- 2 files changed, 6 insertions(+), 191 deletions(-) diff --git a/markhor_heartbeat/CMakeLists.txt b/markhor_heartbeat/CMakeLists.txt index acebc6d..4114c53 100644 --- a/markhor_heartbeat/CMakeLists.txt +++ b/markhor_heartbeat/CMakeLists.txt @@ -1,207 +1,22 @@ cmake_minimum_required(VERSION 3.0.2) project(markhor_heartbeat) -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## 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 COMPONENTS roscpp ) -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## 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 ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## 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 # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## 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 markhor_heartbeat - CATKIN_DEPENDS roscpp -# DEPENDS system_lib + CATKIN_DEPENDS + roscpp ) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations include_directories( -# include - src - include - ${catkin_INCLUDE_DIRS} + src + ${catkin_INCLUDE_DIRS} ) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/markhor_heartbeat.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/markhor_heartbeat_node.cpp) add_executable(${PROJECT_NAME}_node src/heartbeat.cpp) -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ) -############# -## 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 -# catkin_install_python(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}_node -# 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 ${PROJECT_NAME} -# 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/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_markhor_heartbeat.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/markhor_heartbeat/launch/heartbeat.launch b/markhor_heartbeat/launch/heartbeat.launch index abd5db6..7c78267 100644 --- a/markhor_heartbeat/launch/heartbeat.launch +++ b/markhor_heartbeat/launch/heartbeat.launch @@ -1,6 +1,6 @@ - - + + \ No newline at end of file From 218cf77cf503025e609962f5774c77a6e0b8aa9f Mon Sep 17 00:00:00 2001 From: GLDuval Date: Sat, 10 Jun 2023 16:02:39 +0200 Subject: [PATCH 05/10] Add autonomy mode to prevent estop when autonomy is running --- markhor_heartbeat/src/heartbeat.cpp | 94 ++++++++++++++++++----------- 1 file changed, 59 insertions(+), 35 deletions(-) diff --git a/markhor_heartbeat/src/heartbeat.cpp b/markhor_heartbeat/src/heartbeat.cpp index 57d6d47..7ef5105 100644 --- a/markhor_heartbeat/src/heartbeat.cpp +++ b/markhor_heartbeat/src/heartbeat.cpp @@ -1,58 +1,82 @@ #include -#include "std_srvs/Trigger.h" +#include #include #include -bool stopped = false; +bool lost_connection = false; +bool autonomy_mode = false; int heartbeat_timeout; int heartbeat_interval; std::chrono::_V2::system_clock::time_point last_heartbeat = std::chrono::system_clock::now(); void heartbeat(const std_msgs::String message) { - last_heartbeat = std::chrono::system_clock::now(); + last_heartbeat = std::chrono::system_clock::now(); } -void check_heartbeat(const ros::TimerEvent& e, ros::NodeHandle nh) { - std::chrono::_V2::system_clock::time_point now = std::chrono::system_clock::now(); - std::chrono::duration since_last_heartbeat = now - last_heartbeat; - - if (stopped && since_last_heartbeat.count() < heartbeat_timeout) { - ROS_WARN("Connection restored."); - stopped = false; - - std_srvs::Trigger service; - if (ros::service::exists("/markhor/estop_enable", true)) { - ros::service::call("/markhor/estop_enable", service); - } - } else if (!stopped && since_last_heartbeat.count() > heartbeat_timeout) { - ROS_WARN("Heartbeat timeout! Shutting down."); - stopped = true; - - std_srvs::Trigger service; - if (ros::service::exists("/markhor/estop_disable", true)) { - ros::service::call("/markhor/estop_disable", service); - } +void check_heartbeat(const ros::TimerEvent& e, ros::NodeHandle nh) +{ + std::chrono::_V2::system_clock::time_point now = std::chrono::system_clock::now(); + std::chrono::duration since_last_heartbeat = now - last_heartbeat; + + if (lost_connection && since_last_heartbeat.count() < heartbeat_timeout) + { + ROS_WARN("Connection restored."); + lost_connection = false; + + std_srvs::Trigger service; + if (!autonomy_mode && ros::service::exists("/markhor/estop_enable", true)) + { + ros::service::call("/markhor/estop_enable", service); + } + } + else if (!lost_connection && since_last_heartbeat.count() > heartbeat_timeout) + { + ROS_WARN("Heartbeat timeout! Shutting down."); + lost_connection = true; + + std_srvs::Trigger service; + if (!autonomy_mode && ros::service::exists("/markhor/estop_disable", true)) + { + ros::service::call("/markhor/estop_disable", service); } + } +} + +// Allow to toggle autonomy mode. If autonomy mode is enabled, the robot will not be stopped if the heartbeat is lost. +bool toggleAutonomyMode(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) +{ + autonomy_mode = req.data; + if (autonomy_mode) + { + res.message = "Autonomy mode enabled."; + } + else + { + res.message = "Autonomy mode disabled."; + } + + res.success = static_cast(true); + return true; } int main(int argc, char* argv[]) { - ros::init(argc, argv, "markhor_heartbeat_node"); - ros::NodeHandle nh; + ros::init(argc, argv, "markhor_heartbeat_node"); + ros::NodeHandle nh; - // Get parameters from launch file. - nh.getParam("/markhor/heartbeat/markhor_heartbeat_node/heartbeat_interval", heartbeat_interval); - nh.getParam("/markhor/heartbeat/markhor_heartbeat_node/heartbeat_timeout", heartbeat_timeout); + // Get parameters from launch file. + nh.getParam("/markhor/heartbeat/markhor_heartbeat_node/heartbeat_interval", heartbeat_interval); + nh.getParam("/markhor/heartbeat/markhor_heartbeat_node/heartbeat_timeout", heartbeat_timeout); - ROS_INFO("Heartbeat interval is : %d ms", heartbeat_interval); - ROS_INFO("Heartbeat timeout is : %d ms", heartbeat_timeout); + ROS_INFO("Heartbeat interval is : %d ms", heartbeat_interval); + ROS_INFO("Heartbeat timeout is : %d ms", heartbeat_timeout); - // Subscribe to heartbeat topic to receive heartbeats from web ui. - ros::Subscriber heartbeat_subscriber = nh.subscribe("/heartbeat", 5, heartbeat); + // Subscribe to heartbeat topic to receive heartbeats from web ui. + ros::Subscriber heartbeat_subscriber = nh.subscribe("/heartbeat", 5, heartbeat); - // Create timer to verify that we received a heartbeat. - ros::Timer timer1 = nh.createTimer(ros::Duration(heartbeat_interval/1000), boost::bind(check_heartbeat, _1, nh)); + // Create timer to verify that we received a heartbeat. + ros::Timer timer1 = nh.createTimer(ros::Duration(heartbeat_interval / 1000), boost::bind(check_heartbeat, _1, nh)); - ros::spin(); + ros::spin(); } \ No newline at end of file From 64c096fe96958877a9945b9b28d63cba4e27d684 Mon Sep 17 00:00:00 2001 From: GLDuval Date: Sat, 10 Jun 2023 18:09:46 +0200 Subject: [PATCH 06/10] Advertise service --- markhor_heartbeat/src/heartbeat.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/markhor_heartbeat/src/heartbeat.cpp b/markhor_heartbeat/src/heartbeat.cpp index 7ef5105..4778fe1 100644 --- a/markhor_heartbeat/src/heartbeat.cpp +++ b/markhor_heartbeat/src/heartbeat.cpp @@ -74,6 +74,7 @@ int main(int argc, char* argv[]) // Subscribe to heartbeat topic to receive heartbeats from web ui. ros::Subscriber heartbeat_subscriber = nh.subscribe("/heartbeat", 5, heartbeat); + ros::ServiceServer toggle_autonomy_mode = nh.advertiseService("/heartbeat/toggle_autonomy_mode", toggleAutonomyMode); // Create timer to verify that we received a heartbeat. ros::Timer timer1 = nh.createTimer(ros::Duration(heartbeat_interval / 1000), boost::bind(check_heartbeat, _1, nh)); From a83fbef4cee758645a3525de7f48a9ef10f863c3 Mon Sep 17 00:00:00 2001 From: GLDuval Date: Sun, 11 Jun 2023 12:33:27 +0200 Subject: [PATCH 07/10] Add navigation fallback logic --- markhor_heartbeat/src/heartbeat.cpp | 90 ++++++++++++++++++++++------- 1 file changed, 70 insertions(+), 20 deletions(-) diff --git a/markhor_heartbeat/src/heartbeat.cpp b/markhor_heartbeat/src/heartbeat.cpp index 4778fe1..78dc48a 100644 --- a/markhor_heartbeat/src/heartbeat.cpp +++ b/markhor_heartbeat/src/heartbeat.cpp @@ -1,12 +1,16 @@ #include #include #include +#include +#include +#include #include bool lost_connection = false; bool autonomy_mode = false; int heartbeat_timeout; int heartbeat_interval; +int reconnection_attempts = 0; std::chrono::_V2::system_clock::time_point last_heartbeat = std::chrono::system_clock::now(); void heartbeat(const std_msgs::String message) @@ -14,33 +18,79 @@ void heartbeat(const std_msgs::String message) last_heartbeat = std::chrono::system_clock::now(); } -void check_heartbeat(const ros::TimerEvent& e, ros::NodeHandle nh) +// Function that returns to known connection points until the heartbeat is back. +bool return_to_connection_pose(int attempts = 0) { - std::chrono::_V2::system_clock::time_point now = std::chrono::system_clock::now(); - std::chrono::duration since_last_heartbeat = now - last_heartbeat; - - if (lost_connection && since_last_heartbeat.count() < heartbeat_timeout) + MoveBaseClient ac("move_base", true); + while (!ac.waitForServer(ros::Duration(5.0))) { - ROS_WARN("Connection restored."); - lost_connection = false; + ROS_INFO("Waiting for the move_base action server to come up"); + } - std_srvs::Trigger service; - if (!autonomy_mode && ros::service::exists("/markhor/estop_enable", true)) - { - ros::service::call("/markhor/estop_enable", service); + // Get last connection point + ros::NodeHandle nh; + geometry_msgs::PoseStamped connection_pose; + + move_base_msgs::MoveBaseGoal goal; + goal.target_pose.header.frame_id = "map"; + goal.target_pose.header.stamp = ros::Time::now(); + + ac.sendGoal(goal); + + // Wait for the action to finish + ac.waitForResult(); + + if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) + { + ROS_INFO("Robot has reached last known connection point."); + // Check again if we received a heartbeat after the robot has reached destination. + if(validate_heartbeat()){ + lost_connection = false; + }else{ + // If we still don't have a heartbeat, try again to go back to a later known connection point. + return_to_connection_pose(++attempts); } } - else if (!lost_connection && since_last_heartbeat.count() > heartbeat_timeout) + else { - ROS_WARN("Heartbeat timeout! Shutting down."); - lost_connection = true; - + ROS_INFO("The robot failed to move to the last known connection point."); std_srvs::Trigger service; - if (!autonomy_mode && ros::service::exists("/markhor/estop_disable", true)) - { - ros::service::call("/markhor/estop_disable", service); - } + if (ros::service::exists("/markhor/estop_disable", true)) + { + ros::service::call("/markhor/estop_disable", service); + } + return false; + } +} + +bool validate_heartbeat(){ + std::chrono::_V2::system_clock::time_point now = std::chrono::system_clock::now(); + std::chrono::duration since_last_heartbeat = now - last_heartbeat; + if (since_last_heartbeat.count() > heartbeat_timeout) + { + return false; + } + else + { + return true; + } +} + +void check_heartbeat() +{ + bool heartbeat_valid = validate_heartbeat(); + // If we lost the connection, bring back robot to last known connection point + if (!autonomy_mode && !lost_connection && !heartbeat_valid) + { + ROS_WARN("Heartbeat timeout! Returning to last known connection point."); + lost_connection = true; + // Go back to known connection points until the heartbeat is back. + return_to_connection_pose(); + } + else if(lost_connection && since_last_heartbeat.count() < heartbeat_timeout){ + lost_connection = false; } + } // Allow to toggle autonomy mode. If autonomy mode is enabled, the robot will not be stopped if the heartbeat is lost. @@ -77,7 +127,7 @@ int main(int argc, char* argv[]) ros::ServiceServer toggle_autonomy_mode = nh.advertiseService("/heartbeat/toggle_autonomy_mode", toggleAutonomyMode); // Create timer to verify that we received a heartbeat. - ros::Timer timer1 = nh.createTimer(ros::Duration(heartbeat_interval / 1000), boost::bind(check_heartbeat, _1, nh)); + ros::Timer timer1 = nh.createTimer(ros::Duration(heartbeat_interval / 1000), check_heartbeat); ros::spin(); } \ No newline at end of file From 6a0eedc4a42b41ac4338d79cc4c8d3a59eeef5f0 Mon Sep 17 00:00:00 2001 From: GLDuval Date: Wed, 14 Jun 2023 10:55:06 +0200 Subject: [PATCH 08/10] Add navigation heartbeat logic --- markhor_heartbeat/launch/heartbeat.launch | 7 +- markhor_heartbeat/src/heartbeat.cpp | 113 ++++++---------------- 2 files changed, 33 insertions(+), 87 deletions(-) diff --git a/markhor_heartbeat/launch/heartbeat.launch b/markhor_heartbeat/launch/heartbeat.launch index 7c78267..d8f948a 100644 --- a/markhor_heartbeat/launch/heartbeat.launch +++ b/markhor_heartbeat/launch/heartbeat.launch @@ -1,6 +1,7 @@ - - - + + + + \ No newline at end of file diff --git a/markhor_heartbeat/src/heartbeat.cpp b/markhor_heartbeat/src/heartbeat.cpp index 78dc48a..e721d25 100644 --- a/markhor_heartbeat/src/heartbeat.cpp +++ b/markhor_heartbeat/src/heartbeat.cpp @@ -1,72 +1,32 @@ #include #include #include -#include -#include -#include #include +#include -bool lost_connection = false; -bool autonomy_mode = false; -int heartbeat_timeout; +int ui_timeout; +int navigation_timeout; int heartbeat_interval; -int reconnection_attempts = 0; +std::map last_heartbeats; std::chrono::_V2::system_clock::time_point last_heartbeat = std::chrono::system_clock::now(); void heartbeat(const std_msgs::String message) { - last_heartbeat = std::chrono::system_clock::now(); + map[message] = std::chrono::system_clock::now(); } // Function that returns to known connection points until the heartbeat is back. -bool return_to_connection_pose(int attempts = 0) +bool return_to_last_connection_point() { - MoveBaseClient ac("move_base", true); - while (!ac.waitForServer(ros::Duration(5.0))) - { - ROS_INFO("Waiting for the move_base action server to come up"); - } - - // Get last connection point - ros::NodeHandle nh; - geometry_msgs::PoseStamped connection_pose; - - move_base_msgs::MoveBaseGoal goal; - goal.target_pose.header.frame_id = "map"; - goal.target_pose.header.stamp = ros::Time::now(); - - ac.sendGoal(goal); - - // Wait for the action to finish - ac.waitForResult(); - - if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) - { - ROS_INFO("Robot has reached last known connection point."); - // Check again if we received a heartbeat after the robot has reached destination. - if(validate_heartbeat()){ - lost_connection = false; - }else{ - // If we still don't have a heartbeat, try again to go back to a later known connection point. - return_to_connection_pose(++attempts); - } - } - else - { - ROS_INFO("The robot failed to move to the last known connection point."); - std_srvs::Trigger service; - if (ros::service::exists("/markhor/estop_disable", true)) - { - ros::service::call("/markhor/estop_disable", service); - } - return false; - } + // Call service + return true; } -bool validate_heartbeat(){ +bool validate_heartbeat(std::chrono::_V2::system_clock::time_point last_heartbeat, int timeout) +{ std::chrono::_V2::system_clock::time_point now = std::chrono::system_clock::now(); std::chrono::duration since_last_heartbeat = now - last_heartbeat; - if (since_last_heartbeat.count() > heartbeat_timeout) + if (since_last_heartbeat.count() > timeout) { return false; } @@ -78,36 +38,16 @@ bool validate_heartbeat(){ void check_heartbeat() { - bool heartbeat_valid = validate_heartbeat(); - // If we lost the connection, bring back robot to last known connection point - if (!autonomy_mode && !lost_connection && !heartbeat_valid) - { - ROS_WARN("Heartbeat timeout! Returning to last known connection point."); - lost_connection = true; - // Go back to known connection points until the heartbeat is back. - return_to_connection_pose(); - } - else if(lost_connection && since_last_heartbeat.count() < heartbeat_timeout){ - lost_connection = false; - } - -} - -// Allow to toggle autonomy mode. If autonomy mode is enabled, the robot will not be stopped if the heartbeat is lost. -bool toggleAutonomyMode(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) -{ - autonomy_mode = req.data; - if (autonomy_mode) + if (!validate_heartbeat(last_heartbeats.at("navigation"), navigation_timeout) && + !validate_heartbeat(last_heartbeats.at("ui"), ui_timeout)) { - res.message = "Autonomy mode enabled."; - } - else - { - res.message = "Autonomy mode disabled."; + bool success = return_to_connection_pose(); + std_srvs::Trigger service; + if (ros::service::exists("/markhor/estop_disable", true)) + { + ros::service::call("/markhor/estop_disable", service); + } } - - res.success = static_cast(true); - return true; } int main(int argc, char* argv[]) @@ -117,16 +57,21 @@ int main(int argc, char* argv[]) // Get parameters from launch file. nh.getParam("/markhor/heartbeat/markhor_heartbeat_node/heartbeat_interval", heartbeat_interval); - nh.getParam("/markhor/heartbeat/markhor_heartbeat_node/heartbeat_timeout", heartbeat_timeout); + nh.getParam("/markhor/heartbeat/markhor_heartbeat_node/navigation_timeout", navigation_timeout); + nh.getParam("/markhor/heartbeat/markhor_heartbeat_node/ui_timeout", ui_timeout); ROS_INFO("Heartbeat interval is : %d ms", heartbeat_interval); - ROS_INFO("Heartbeat timeout is : %d ms", heartbeat_timeout); + ROS_INFO("Navigation timeout is : %d ms", navigation_timeout); + ROS_INFO("UI timeout is : %d ms", ui_timeout); + + // Init heartbeats + map["navigation"] = std::chrono::system_clock::now(); + map["ui"] = std::chrono::system_clock::now(); // Subscribe to heartbeat topic to receive heartbeats from web ui. - ros::Subscriber heartbeat_subscriber = nh.subscribe("/heartbeat", 5, heartbeat); - ros::ServiceServer toggle_autonomy_mode = nh.advertiseService("/heartbeat/toggle_autonomy_mode", toggleAutonomyMode); + ros::Subscriber heartbeat_subscriber = nh.subscribe("heartbeat", 5, heartbeat); - // Create timer to verify that we received a heartbeat. + // Timer for UI heartbeat ros::Timer timer1 = nh.createTimer(ros::Duration(heartbeat_interval / 1000), check_heartbeat); ros::spin(); From 937e7a882d7f6fee093dec7ad459131e3ddf0f67 Mon Sep 17 00:00:00 2001 From: GLDuval Date: Wed, 14 Jun 2023 11:11:31 +0200 Subject: [PATCH 09/10] Small fix --- markhor_heartbeat/src/heartbeat.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/markhor_heartbeat/src/heartbeat.cpp b/markhor_heartbeat/src/heartbeat.cpp index e721d25..3040a5d 100644 --- a/markhor_heartbeat/src/heartbeat.cpp +++ b/markhor_heartbeat/src/heartbeat.cpp @@ -8,11 +8,10 @@ int ui_timeout; int navigation_timeout; int heartbeat_interval; std::map last_heartbeats; -std::chrono::_V2::system_clock::time_point last_heartbeat = std::chrono::system_clock::now(); void heartbeat(const std_msgs::String message) { - map[message] = std::chrono::system_clock::now(); + last_heartbeats[message] = std::chrono::system_clock::now(); } // Function that returns to known connection points until the heartbeat is back. From f298fe536e5a2e7131b4a2e719e85da47c2a153e Mon Sep 17 00:00:00 2001 From: saxtot Date: Wed, 14 Jun 2023 05:28:01 -0400 Subject: [PATCH 10/10] :sparkles: Fix build --- markhor_heartbeat/src/heartbeat.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/markhor_heartbeat/src/heartbeat.cpp b/markhor_heartbeat/src/heartbeat.cpp index 3040a5d..967c3a1 100644 --- a/markhor_heartbeat/src/heartbeat.cpp +++ b/markhor_heartbeat/src/heartbeat.cpp @@ -3,6 +3,7 @@ #include #include #include +#include int ui_timeout; int navigation_timeout; @@ -11,7 +12,7 @@ std::map last_heartbeat void heartbeat(const std_msgs::String message) { - last_heartbeats[message] = std::chrono::system_clock::now(); + last_heartbeats.at(message.data) = std::chrono::system_clock::now(); } // Function that returns to known connection points until the heartbeat is back. @@ -35,12 +36,12 @@ bool validate_heartbeat(std::chrono::_V2::system_clock::time_point last_heartbea } } -void check_heartbeat() +void check_heartbeat(const ros::TimerEvent& event) { if (!validate_heartbeat(last_heartbeats.at("navigation"), navigation_timeout) && !validate_heartbeat(last_heartbeats.at("ui"), ui_timeout)) { - bool success = return_to_connection_pose(); + bool success = return_to_last_connection_point(); std_srvs::Trigger service; if (ros::service::exists("/markhor/estop_disable", true)) { @@ -61,11 +62,11 @@ int main(int argc, char* argv[]) ROS_INFO("Heartbeat interval is : %d ms", heartbeat_interval); ROS_INFO("Navigation timeout is : %d ms", navigation_timeout); - ROS_INFO("UI timeout is : %d ms", ui_timeout); + ROS_INFO("UI timeout is : %d ms", ui_timeout); // Init heartbeats - map["navigation"] = std::chrono::system_clock::now(); - map["ui"] = std::chrono::system_clock::now(); + last_heartbeats.at("navigation") = std::chrono::system_clock::now(); + last_heartbeats.at("ui") = std::chrono::system_clock::now(); // Subscribe to heartbeat topic to receive heartbeats from web ui. ros::Subscriber heartbeat_subscriber = nh.subscribe("heartbeat", 5, heartbeat);