From d293fda07c530f651946e53d0e0dfa5b617bec54 Mon Sep 17 00:00:00 2001 From: Wojciech Kwolek Date: Fri, 16 Apr 2021 12:37:09 +0000 Subject: [PATCH] implement watchdog --- .devcontainer/devcontainer.json | 8 ++--- .vscode/settings.json | 60 ++++++++++++++++++++++++++++++++- Dockerfile.devcontainer | 2 +- src/ros2aria/src/raiibot.cpp | 42 ++++++++++++++++++++++- src/ros2aria/src/ros2aria.cpp | 34 ++++--------------- 5 files changed, 111 insertions(+), 35 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 85959f7..d5d2a41 100644 --- a/.devcontainer/devcontainer.json +++ b/.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", - "--device=/dev/ttyS0" + "--volume=/tmp/.X11-unix:/tmp/.X11-unix" + // "--device=/dev/ttyS0" ], "containerEnv": { "DISPLAY": "${localEnv:DISPLAY}" }, - "workspaceFolder": "/ws", - "workspaceMount": "source=/home/lab1_5/ros2aria_ws,target=/ws,type=bind,consistency=delegated", + // "workspaceFolder": "/ws", + // "workspaceMount": "source=/home/lab1_5/ros2aria_ws,target=/ws,type=bind,consistency=delegated", "settings": { "terminal.integrated.shell.linux": "/bin/bash" }, diff --git a/.vscode/settings.json b/.vscode/settings.json index b01d19d..120d6fc 100644 --- a/.vscode/settings.json +++ b/.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" } } \ No newline at end of file diff --git a/Dockerfile.devcontainer b/Dockerfile.devcontainer index 5b9b2cc..71f45fc 100644 --- a/Dockerfile.devcontainer +++ b/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 diff --git a/src/ros2aria/src/raiibot.cpp b/src/ros2aria/src/raiibot.cpp index 9f83273..275a785 100644 --- a/src/ros2aria/src/raiibot.cpp +++ b/src/ros2aria/src/raiibot.cpp @@ -3,15 +3,20 @@ #include #include "Aria/Aria.h" +#include "rclcpp/rclcpp.hpp" + +using namespace std::chrono_literals; class RAIIBot { public: typedef std::shared_ptr 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 lock(*this); + auto r = this->getRobot(); + r->setVel(0); + r->setRotVel(0); + if (r->hasLatVel()) + r->setLatVel(0); + } + } }; \ No newline at end of file diff --git a/src/ros2aria/src/ros2aria.cpp b/src/ros2aria/src/ros2aria.cpp index 7e6cafc..0baf3a5 100644 --- a/src/ros2aria/src/ros2aria.cpp +++ b/src/ros2aria/src/ros2aria.cpp @@ -20,50 +20,30 @@ public: MinimalSubscriber() : Node("minimal_subscriber") { - this->robot = std::make_shared("/dev/ttyS0"); - RCLCPP_INFO(this->get_logger(), "starting subs"); + this->robot = std::make_shared(this, "/dev/ttyS0"); + RCLCPP_INFO(this->get_logger(), "starting subscribers and services"); cmd_vel_sub_ = this->create_subscription( "cmd_vel", 10, std::bind(&MinimalSubscriber::cmd_vel_callback, this, _1)); stop_service_ = this->create_service("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 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 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::SharedPtr stop_service_; - - rclcpp::Time watchdog; }; int main(int argc, char **argv)