From 6abc1eac62f526195eedc266122bcf9c8e632da2 Mon Sep 17 00:00:00 2001 From: ynyBonfennil Date: Wed, 28 Aug 2024 18:34:59 +0900 Subject: [PATCH] Separate mg400_node state from mg400 connection state, and always try to reconnect when mg400_node is active --- mg400_node/include/mg400_node/mg400_node.hpp | 3 + mg400_node/src/mg400_node.cpp | 65 +++++++++++++------- 2 files changed, 47 insertions(+), 21 deletions(-) diff --git a/mg400_node/include/mg400_node/mg400_node.hpp b/mg400_node/include/mg400_node/mg400_node.hpp index 83ad001d..1e9a4e3d 100644 --- a/mg400_node/include/mg400_node/mg400_node.hpp +++ b/mg400_node/include/mg400_node/mg400_node.hpp @@ -59,12 +59,14 @@ class MG400Node : public rclcpp_lifecycle::LifecycleNode mg400_interface::MG400Interface::SharedPtr interface_; mg400_plugin_base::DashboardApiLoader::SharedPtr dashboard_api_loader_; mg400_plugin_base::MotionApiLoader::SharedPtr motion_api_loader_; + bool mg400_connected_; rclcpp::TimerBase::SharedPtr init_timer_; rclcpp::TimerBase::SharedPtr joint_state_timer_; rclcpp::TimerBase::SharedPtr robot_mode_timer_; rclcpp::TimerBase::SharedPtr error_timer_; rclcpp::TimerBase::SharedPtr interface_check_timer_; + rclcpp::TimerBase::SharedPtr activate_timer_; rclcpp::TimerBase::SharedPtr connect_timer_; rclcpp::Publisher::SharedPtr joint_state_pub_; @@ -91,6 +93,7 @@ class MG400Node : public rclcpp_lifecycle::LifecycleNode CallbackReturn on_shutdown(const rclcpp_lifecycle::State &) override; CallbackReturn on_error(const rclcpp_lifecycle::State &) override; + void connect(); void runTimer(); void cancelTimer(); }; diff --git a/mg400_node/src/mg400_node.cpp b/mg400_node/src/mg400_node.cpp index 67587918..783f338a 100644 --- a/mg400_node/src/mg400_node.cpp +++ b/mg400_node/src/mg400_node.cpp @@ -46,10 +46,11 @@ MG400Node::~MG400Node() CallbackReturn MG400Node::on_configure(const State &) { + this->mg400_connected_ = false; this->mg400_connected_pub_ = this->create_publisher( "mg400_connected", rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local()); - this->mg400_connected_pub_->publish(std_msgs::msg::Bool().set__data(false)); + this->mg400_connected_pub_->publish(std_msgs::msg::Bool().set__data(this->mg400_connected_)); this->ip_address_ = this->get_parameter("ip_address").as_string(); this->interface_ = @@ -98,7 +99,7 @@ CallbackReturn MG400Node::on_configure(const State &) if (this->get_parameter("auto_connect").as_bool()) { RCLCPP_INFO(this->get_logger(), "Try connecting to MG400 at %s ...", this->ip_address_.c_str()); - this->connect_timer_ = this->create_wall_timer(0s, [this]() {this->activate();}); + this->activate_timer_ = this->create_wall_timer(0s, [this]() {this->activate();}); } return CallbackReturn::SUCCESS; @@ -106,32 +107,24 @@ CallbackReturn MG400Node::on_configure(const State &) CallbackReturn MG400Node::on_activate(const State &) { - this->connect_timer_.reset(); - - if (!this->interface_->activate()) { - RCLCPP_WARN(this->get_logger(), "Failed to connect to MG400 at %s", this->ip_address_.c_str()); - if (this->get_parameter("auto_connect").as_bool()) { - RCLCPP_INFO(this->get_logger(), "Try reconnecting in 5 seconds ..."); - this->connect_timer_ = this->create_wall_timer(5s, [this]() {this->activate();}); - } - return CallbackReturn::FAILURE; - } - - this->runTimer(); - - RCLCPP_INFO(this->get_logger(), "Connected to MG400 at %s", this->ip_address_.c_str()); - this->mg400_connected_pub_->publish(std_msgs::msg::Bool().set__data(true)); + this->activate_timer_.reset(); + this->connect_timer_ = this->create_wall_timer(0s, [this]() {this->connect();}); return CallbackReturn::SUCCESS; } CallbackReturn MG400Node::on_deactivate(const State &) { - RCLCPP_WARN(this->get_logger(), "Disconnected from MG400 at %s", this->ip_address_.c_str()); - this->mg400_connected_pub_->publish(std_msgs::msg::Bool().set__data(false)); + this->connect_timer_.reset(); this->cancelTimer(); this->interface_->deactivate(); + if (this->mg400_connected_) { + this->mg400_connected_ = false; + RCLCPP_WARN(this->get_logger(), "Disconnected from MG400 at %s", this->ip_address_.c_str()); + this->mg400_connected_pub_->publish(std_msgs::msg::Bool().set__data(this->mg400_connected_)); + } + return CallbackReturn::SUCCESS; } @@ -176,6 +169,20 @@ CallbackReturn MG400Node::on_error(const State &) return CallbackReturn::SUCCESS; } +void MG400Node::connect() +{ + this->connect_timer_.reset(); + + if (!this->interface_->activate()) { + RCLCPP_WARN(this->get_logger(), "Failed to connect to MG400 at %s", this->ip_address_.c_str()); + RCLCPP_INFO(this->get_logger(), "Try reconnecting in 5 seconds ..."); + this->connect_timer_ = this->create_wall_timer(5s, [this]() {this->connect();}); + return; + } + + this->runTimer(); +} + void MG400Node::onJointStateTimer() { if (!this->interface_->ok()) { @@ -262,8 +269,24 @@ void MG400Node::onErrorTimer() void MG400Node::onInterfaceCheckTimer() { - if (!this->interface_->ok()) { - this->deactivate(); + // when connection is established + if (!this->mg400_connected_ && this->interface_->ok()) { + this->mg400_connected_ = true; + RCLCPP_INFO(this->get_logger(), "Connected to MG400 at %s", this->ip_address_.c_str()); + this->mg400_connected_pub_->publish(std_msgs::msg::Bool().set__data(this->mg400_connected_)); + } + + // when connection is lost + if (this->mg400_connected_ && !this->interface_->ok()) { + this->mg400_connected_ = false; + RCLCPP_WARN(this->get_logger(), "Disconnected from MG400 at %s", this->ip_address_.c_str()); + this->mg400_connected_pub_->publish(std_msgs::msg::Bool().set__data(this->mg400_connected_)); + + this->interface_->deactivate(); + this->cancelTimer(); + + RCLCPP_INFO(this->get_logger(), "Try reconnecting in 5 seconds ..."); + this->connect_timer_ = this->create_wall_timer(5s, [this]() {this->connect();}); } }