Fix communication error handling

src/ypspur_ros.cpp

@@ -742,17 +742,17 @@ class YpspurRosNode
   }
   ~YpspurRosNode()
   {
-    if (pid_ > 0)
+    // Kill ypspur-coordinator if the communication is still active.
+    if (pid_ > 0 && YP::YP_get_error_state() == 0)
     {
       ROS_INFO("killing ypspur-coordinator (%d)", (int)pid_);
       kill(pid_, SIGINT);
       int status;
       waitpid(pid_, &status, 0);
       ROS_INFO("ypspur-coordinator is killed (status: %d)", status);
     }
-    ros::shutdown();
   }
-  void spin()
+  bool spin()
   {
     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = frames_["odom"];
@@ -819,8 +819,8 @@ class YpspurRosNode
         if (!simulate_control_)
         {
           t = YP::YPSpur_get_pos(YP::CS_BS, &x, &y, &yaw);
-          if (t == 0.0)
-            t = now.toSec();
+          if (t <= 0.0)
+            break;
           YP::YPSpur_get_vel(&v, &w);
         }
         else
@@ -853,9 +853,12 @@ class YpspurRosNode
         odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(yaw);
         tf_broadcaster_.sendTransform(odom_trans);
 
-        t = YP::YPSpur_get_force(&wrench.wrench.force.x, &wrench.wrench.torque.z);
-        if (t == 0.0)
-          t = now.toSec();
+        if (!simulate_control_)
+        {
+          t = YP::YPSpur_get_force(&wrench.wrench.force.x, &wrench.wrench.torque.z);
+          if (t <= 0.0)
+            break;
+        }
         wrench.header.stamp = ros::Time(t);
         wrench.wrench.force.y = 0;
         wrench.wrench.force.z = 0;
@@ -917,9 +920,9 @@ class YpspurRosNode
               }
               i++;
             }
-            if (t == 0.0)
-              t = ros::Time::now().toSec();
           }
+          if (t <= 0.0)
+            break;
           joint.header.stamp = ros::Time(t);
         }
         else
@@ -1192,10 +1195,8 @@ class YpspurRosNode
       updateDiagnostics(now);
 
       if (YP::YP_get_error_state())
-      {
-        ROS_ERROR("ypspur-coordinator is not active");
         break;
-      }
+
       ros::spinOnce();
       loop.sleep();
 
@@ -1223,25 +1224,35 @@ class YpspurRosNode
       }
     }
     ROS_INFO("ypspur_ros main loop terminated");
+
+    if (YP::YP_get_error_state())
+    {
+      ROS_ERROR("ypspur-coordinator is not active");
+      return false;
+    }
+    return true;
   }
 };
 
 int main(int argc, char* argv[])
 {
   ros::init(argc, argv, "ypspur_ros");
+  ros::NodeHandle nh;
+
+  int ret = 0;
 
   try
   {
     YpspurRosNode yr;
-    yr.spin();
+    if (!yr.spin())
+      ret = 1;
   }
   catch (std::runtime_error& e)
   {
     ROS_ERROR("%s", e.what());
-    ros::WallDuration(0.1).sleep();
-    return -1;
+    ret = 1;
   }
 
   ros::WallDuration(0.1).sleep();
-  return 0;
+  return ret;
 }