From cc053a899f2a13c9813b48334d308f2b25c9df9b Mon Sep 17 00:00:00 2001
From: Wojciech Kwolek <me@irth.pl>
Date: Thu, 25 Mar 2021 13:28:51 +0000
Subject: [PATCH] add basic watchdog

---
 src/ros2aria/src/ros2aria.cpp | 27 ++++++++++++++++++++++++++-
 1 file changed, 26 insertions(+), 1 deletion(-)

diff --git a/src/ros2aria/src/ros2aria.cpp b/src/ros2aria/src/ros2aria.cpp
index 23fe0c4..9c07684 100644
--- a/src/ros2aria/src/ros2aria.cpp
+++ b/src/ros2aria/src/ros2aria.cpp
@@ -10,6 +10,7 @@
 
 using std::placeholders::_1;
 using std::placeholders::_2;
+using namespace std::chrono_literals;
 
 class MinimalSubscriber : public rclcpp::Node
 {
@@ -40,13 +41,34 @@ public:
     cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
         "cmd_vel", 10, std::bind(&MinimalSubscriber::cmd_vel_callback, this, _1));
     stop_service_ = this->create_service<std_srvs::srv::Empty>("stop", std::bind(&MinimalSubscriber::stop, this, _1, _2));
+
+    this->clock = rclcpp::Clock::make_shared();
+    this->watchdog = this->clock->now();
+    timer = this->create_wall_timer(0.1s, std::bind(&MinimalSubscriber::watchdog_cb, this));
   }
 
 private:
   ArRobot *robot;
   ArRobotConnector *robotConn;
 
-  void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) const
+  rclcpp::Clock::SharedPtr clock;
+  rclcpp::TimerBase::SharedPtr timer;
+
+  void watchdog_cb()
+  {
+    auto now = this->clock->now();
+    if (now - this->watchdog > 1s)
+    {
+      this->robot->lock();
+      this->robot->setVel(0);
+      this->robot->setRotVel(0);
+      if (this->robot->hasLatVel())
+        this->robot->setLatVel(0);
+      this->robot->unlock();
+    }
+  }
+
+  void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
   {
     RCLCPP_INFO(this->get_logger(), "cmd_vel: %f", msg->linear.x);
     float x, y, z;
@@ -54,6 +76,7 @@ private:
     y = msg->linear.y;
     z = msg->angular.z;
     this->robot->lock();
+    this->watchdog = this->clock->now();
     this->robot->setVel(x * 1e3);
     if (robot->hasLatVel())
       robot->setLatVel(y * 1e3);
@@ -69,6 +92,8 @@ private:
     rclcpp::shutdown();
   }
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr stop_service_;
+
+  rclcpp::Time watchdog;
 };
 
 int main(int argc, char **argv)