Catch missed read/write packets (#19)

Author: MarqRazz

robotiq_driver/src/hardware_interface.cpp

@@ -211,7 +211,7 @@ RobotiqGripperHardwareInterface::on_activate(const rclcpp_lifecycle::State& /*pr
         }
         catch (serial::IOException& e)
         {
-          RCLCPP_ERROR(kLogger, "Check Robotiq Gripper connection and restart drivers.");
+          RCLCPP_ERROR(kLogger, "Check Robotiq Gripper connection and restart drivers. ERROR: %s", e.what());
           command_interface_is_running_.store(false);
         }
       }

robotiq_driver/src/robotiq_gripper_interface.cpp

@@ -152,6 +152,11 @@ void RobotiqGripperInterface::setGripperPosition(uint8_t pos)
   {
     // catch connection error and rethrow
     std::cerr << "Failed to set gripper position\n";
+    if (port_.isOpen())
+    {
+      std::cerr << "Error caught while reading or writing to device. Connection is open, continuing to attempt communication with gripper.\n  ERROR: " << e.what();
+      return;
+    }
     throw;
   }
 }
@@ -320,6 +325,11 @@ void RobotiqGripperInterface::updateStatus()
   catch (const serial::IOException& e)
   {
     std::cerr << "Failed to update gripper status.\n";
+    if (port_.isOpen())
+    {
+      std::cerr << "Error caught while reading or writing to device. Connection is open, continuing to attempt communication with gripper.\n  ERROR: " << e.what();
+      return;
+    }
     throw;
   }
 }