add basic watchdog

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)