implement watchdog

.devcontainer/devcontainer.json

@@ -13,14 +13,14 @@
         "--cap-add=SYS_PTRACE",
         "--security-opt=seccomp:unconfined",
         "--security-opt=apparmor:unconfined",
-        "--volume=/tmp/.X11-unix:/tmp/.X11-unix",
+        "--volume=/tmp/.X11-unix:/tmp/.X11-unix"
-        "--device=/dev/ttyS0"
+        // "--device=/dev/ttyS0"
     ],
     "containerEnv": {
         "DISPLAY": "${localEnv:DISPLAY}"
     },
-    "workspaceFolder": "/ws",
+    // "workspaceFolder": "/ws",
-    "workspaceMount": "source=/home/lab1_5/ros2aria_ws,target=/ws,type=bind,consistency=delegated",
+    // "workspaceMount": "source=/home/lab1_5/ros2aria_ws,target=/ws,type=bind,consistency=delegated",
     "settings": {
         "terminal.integrated.shell.linux": "/bin/bash"
     },

.vscode/settings.json

@@ -3,6 +3,64 @@
         "/opt/ros/foxy/lib/python3.8/site-packages"
     ],
     "files.associations": {
-        "functional": "cpp"
+        "functional": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "bit": "cpp",
+        "*.tcc": "cpp",
+        "cctype": "cpp",
+        "chrono": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "condition_variable": "cpp",
+        "csignal": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdint": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "deque": "cpp",
+        "list": "cpp",
+        "map": "cpp",
+        "set": "cpp",
+        "unordered_map": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "iterator": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "ratio": "cpp",
+        "string": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "fstream": "cpp",
+        "future": "cpp",
+        "initializer_list": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "ostream": "cpp",
+        "shared_mutex": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "thread": "cpp",
+        "cinttypes": "cpp",
+        "typeindex": "cpp",
+        "typeinfo": "cpp"
     }
 }

Dockerfile.devcontainer

@@ -18,7 +18,7 @@ COPY ./src/AriaCoda /usr/local/Aria
 RUN cd /usr/local/Aria && make -j$(nproc)
 
 ARG WORKSPACE
-RUN echo "if [ -f ${WORKSPACE}/install/setup.bash ]; then source ${WORKSPACE}/install/setup.bash; fi" >> /home/ros/.bashrc
+# RUN echo "if [ -f ${WORKSPACE}/install/setup.bash ]; then source ${WORKSPACE}/install/setup.bash; fi" >> /home/ros/.bashrc
 
 ARG USERNAME=ros
 

src/ros2aria/src/raiibot.cpp

@@ -3,15 +3,20 @@
 #include <iostream>
 
 #include "Aria/Aria.h"
+#include "rclcpp/rclcpp.hpp"
+
+using namespace std::chrono_literals;
 
 class RAIIBot
 {
 public:
     typedef std::shared_ptr<RAIIBot> SharedPtr;
 
-    RAIIBot(std::string port)
+    RAIIBot(rclcpp::Node *node, std::string port)
     {
         this->robot = new ArRobot();
+        this->node = node;
+        this->clock = rclcpp::Clock::make_shared();
 
         args = new ArArgumentBuilder();
         this->argparser = new ArArgumentParser(args);
@@ -65,9 +70,44 @@ public:
             this->robot->unlock();
     }
 
+    void pokeWatchdog()
+    {
+        // this should probably be made thread safe, or something?
+        this->lastWatchdogPoke = this->clock->now();
+    }
+
 private:
     ArRobot *robot = nullptr;
     ArRobotConnector *robotConn = nullptr;
     ArArgumentBuilder *args = nullptr;
     ArArgumentParser *argparser = nullptr;
+    rclcpp::Node *node = nullptr;
+    rclcpp::TimerBase::SharedPtr watchdogTimer;
+    rclcpp::Time lastWatchdogPoke;
+    rclcpp::Clock::SharedPtr clock;
+
+    rclcpp::Logger get_logger()
+    {
+        return this->node->get_logger();
+    }
+
+    void startWatchdog()
+    {
+        this->lastWatchdogPoke = clock->now();
+        watchdogTimer = this->node->create_wall_timer(0.1s, std::bind(&RAIIBot::watchdog_cb, this));
+    }
+
+    void watchdog_cb()
+    {
+        auto now = this->clock->now();
+        if (now - this->lastWatchdogPoke > 1s)
+        {
+            std::lock_guard<RAIIBot> lock(*this);
+            auto r = this->getRobot();
+            r->setVel(0);
+            r->setRotVel(0);
+            if (r->hasLatVel())
+                r->setLatVel(0);
+        }
+    }
 };

src/ros2aria/src/ros2aria.cpp

@@ -20,50 +20,30 @@ public:
   MinimalSubscriber()
       : Node("minimal_subscriber")
   {
-    this->robot = std::make_shared<RAIIBot>("/dev/ttyS0");
+    this->robot = std::make_shared<RAIIBot>(this, "/dev/ttyS0");
-    RCLCPP_INFO(this->get_logger(), "starting subs");
 
+    RCLCPP_INFO(this->get_logger(), "starting subscribers and services");
     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:
   RAIIBot::SharedPtr robot;
 
-  rclcpp::Clock::SharedPtr clock;
-  rclcpp::TimerBase::SharedPtr timer;
-
-  void watchdog_cb()
-  {
-    auto now = this->clock->now();
-    if (now - this->watchdog > 1s)
-    {
-      std::lock_guard<RAIIBot> lock(*robot.get());
-      auto r = robot->getRobot();
-      r->setVel(0);
-      r->setRotVel(0);
-      if (r->hasLatVel())
-        r->setLatVel(0);
-    }
-  }
-
   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;
     x = msg->linear.x;
     y = msg->linear.y;
     z = msg->angular.z;
+    RCLCPP_INFO(this->get_logger(), "cmd_vel: x:%f y:%f z:%f", x, y, z);
 
     std::lock_guard<RAIIBot> lock(*robot.get());
-    auto r = robot->getRobot();
 
-    this->watchdog = this->clock->now();
+    this->robot->pokeWatchdog();
+
+    auto r = robot->getRobot();
     r->setVel(x * 1e3);
     if (r->hasLatVel())
       r->setLatVel(y * 1e3);
@@ -78,8 +58,6 @@ private:
     rclcpp::shutdown();
   }
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr stop_service_;
-
-  rclcpp::Time watchdog;
 };
 
 int main(int argc, char **argv)