Heartbeat

markhor_bringup/launch/markhor_common.launch

@@ -4,6 +4,6 @@
     <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" />
 </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,6 @@
+<launch>
+    <node pkg="markhor_heartbeat" type="markhor_heartbeat_node" name="markhor_heartbeat_node" ns="/markhor/heartbeat" output="screen">
+        <param name="heartbeat_interval" type="int" value="500" /> 
+        <param name="heartbeat_timeout" type="int" value="1500" />
+    </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,83 @@
+#include <ros/ros.h>
+#include <std_srvs/SetBool.h>
+#include <std_msgs/String.h>
+#include <chrono>
+
+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();
+}
+
+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<double, std::milli> 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<unsigned char>(true);
+  return true;
+}
+
+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);
+  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::spin();
+}