L1.5/ros2aria
L1.5/ros2aria
7
0
Fork 0
Code Issues 5 Pull Requests Projects 1 Releases Wiki Activity

implement watchdog

Browse Source
This commit is contained in:
Wojciech Kwolek 2021-04-16 12:37:09 +00:00
parent c034da9010
commit d293fda07c
5 changed files with 111 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.devcontainer/devcontainer.json
View File

@ -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"
},

60
.vscode/settings.json vendored
View File

@ -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"
}
}

2
Dockerfile.devcontainer
View File

@ -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

42
src/ros2aria/src/raiibot.cpp
View File

@ -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);
}
}
};

34
src/ros2aria/src/ros2aria.cpp
View File

@ -20,50 +20,30 @@ public:
MinimalSubscriber()
: Node("minimal_subscriber")
{
this->robot = std::make_shared<RAIIBot>("/dev/ttyS0");
RCLCPP_INFO(this->get_logger(), "starting subs");
this->robot = std::make_shared<RAIIBot>(this, "/dev/ttyS0");
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)