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

add basic watchdog

Browse Source
This commit is contained in:
Wojciech Kwolek 2021-03-25 13:28:51 +00:00
parent 50767ebc27
commit cc053a899f
1 changed files with 26 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
src/ros2aria/src/ros2aria.cpp
View File

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