Heartbeat

markhor_bringup/launch/markhor_common.launch

@@ -4,7 +4,7 @@
     <include file="$(find markhor_bringup)/launch/teleop_twist_joy.launch" />
     <include file="$(find markhor_bringup)/launch/tpv_controller.launch" />
     <include file="$(find markhor_bringup)/launch/markhor_web_ui.launch" />
-    <node name="tf2_web_republisher" pkg="tf2_web_republisher" type="tf2_web_republisher" respawn="false"/>
+    <include file="$(find markhor_heartbeat)/launch/heartbeat.launch" />
     <include file="$(find markhor_bringup)/launch/cameras/markhor_all_cameras.launch" />
     <include file="$(find zed_wrapper)/launch/zed2i.launch" />
     <include file="$(find ros_rtsp)/launch/rtsp_streams.launch" />

markhor_heartbeat/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(markhor_heartbeat)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+)
+
+catkin_package(
+  CATKIN_DEPENDS
+    roscpp 
+)
+include_directories(
+    src
+    ${catkin_INCLUDE_DIRS}
+)
+
+add_executable(${PROJECT_NAME}_node src/heartbeat.cpp)
+
+target_link_libraries(${PROJECT_NAME}_node
+  ${catkin_LIBRARIES}
+)
+

markhor_heartbeat/launch/heartbeat.launch

@@ -0,0 +1,7 @@
+<launch>
+    <node pkg="markhor_heartbeat" type="markhor_heartbeat_node" name="markhor_heartbeat_node" ns="/markhor" output="screen">
+        <param name="heartbeat_interval" type="int" value="1000" /> 
+        <param name="ui_timeout" type="int" value="45000" />
+        <param name="navigation_timeout" type="int" value="5000" />
+    </node>
+</launch>

markhor_heartbeat/package.xml

@@ -0,0 +1,62 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>markhor_heartbeat</name>
+  <version>0.0.0</version>
+  <description>The markhor_heartbeat package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="myriam.lacroix.1@ens.etsmtl.ca">Myriam</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/markhor_heartbeat</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

markhor_heartbeat/src/heartbeat.cpp

@@ -0,0 +1,78 @@
+#include <ros/ros.h>
+#include <std_srvs/SetBool.h>
+#include <std_msgs/String.h>
+#include <chrono>
+#include <std_srvs/Trigger.h>
+#include <map>
+
+int ui_timeout;
+int navigation_timeout;
+int heartbeat_interval;
+std::map<std::string, std::chrono::_V2::system_clock::time_point> last_heartbeats;
+
+void heartbeat(const std_msgs::String message)
+{
+  last_heartbeats.at(message.data) = std::chrono::system_clock::now();
+}
+
+// Function that returns to known connection points until the heartbeat is back.
+bool return_to_last_connection_point()
+{
+  // Call service
+  return true;
+}
+
+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<double, std::milli> since_last_heartbeat = now - last_heartbeat;
+  if (since_last_heartbeat.count() > timeout)
+  {
+    return false;
+  }
+  else
+  {
+    return true;
+  }
+}
+
+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_last_connection_point();
+    std_srvs::Trigger service;
+    if (ros::service::exists("/markhor/estop_disable", true))
+    {
+      ros::service::call("/markhor/estop_disable", service);
+    }
+  }
+}
+
+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/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("Navigation timeout is : %d ms", navigation_timeout);
+  ROS_INFO("UI timeout is : %d ms", ui_timeout);
+
+  // Init heartbeats
+  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);
+
+  // Timer for UI heartbeat
+  ros::Timer timer1 = nh.createTimer(ros::Duration(heartbeat_interval / 1000), check_heartbeat);
+
+  ros::spin();
+}