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

@@ -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<sensor_msgs::msg::JointState>::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();
 };

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<std_msgs::msg::Bool>(
     "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,40 +99,32 @@ 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;
 }
 
 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();});
   }
 }