Merge branch 'pr/86' 'Adding non blocking read'

ur_robot_driver/doc/ROS_INTERFACE.md

@@ -762,6 +762,10 @@ This is the actual driver node containing the ROS-Control stack. Interfaces docu
 
     Hash of the calibration reported by the robot. This is used for validating the robot description is using the correct calibration. If the robot's calibration doesn't match this hash, an error will be printed. You can use the robot as usual, however Cartesian poses of the endeffector might be inaccurate. See the "ur_calibration" package on help how to generate your own hash matching your actual robot.
 
+ * "**non_blocking_read**" (default: "false")
+
+    Enables non_blocking_read mode. Should only be used with combined_robot_hw. Disables error generated when read returns without any data, sets the read timeout to zero, and synchronises read/write operations. Enabling this when not used with combined_robot_hw can suppress important errors and affect real-time performance.
+
  * "**output_recipe_file**" (Required)
 
     Path to the file containing the recipe used for requesting RTDE outputs.

ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h

@@ -258,6 +258,9 @@ protected:
   bool controller_reset_necessary_;
   bool controllers_initialized_;
 
+  bool packet_read_;
+  bool non_blocking_read_;
+
   std::string robot_ip_;
   std::string tf_prefix_;
 };

ur_robot_driver/include/ur_robot_driver/ur/ur_driver.h

@@ -62,6 +62,7 @@ public:
    * and the robot controller.
    * \param script_sending_port The driver will offer an interface to receive the program's URScript on this port. If
    * the robot cannot connect to this port, `External Control` will stop immediately.
+   * \param non_blocking_read Enable non-blocking mode for read (useful when used with combined_robot_hw)
    * \param servoj_gain Proportional gain for arm joints following target position, range [100,2000]
    * \param servoj_lookahead_time Time [S], range [0.03,0.2] smoothens the trajectory with this lookahead time
    */
@@ -69,7 +70,7 @@ public:
            const std::string& input_recipe_file, std::function<void(bool)> handle_program_state, bool headless_mode,
            std::unique_ptr<ToolCommSetup> tool_comm_setup, const std::string& calibration_checksum = "",
            const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000,
-           double servoj_lookahead_time = 0.03);
+           double servoj_lookahead_time = 0.03, bool non_blocking_read = false);
   /*!
    * \brief Constructs a new UrDriver object.
    *
@@ -85,16 +86,18 @@ public:
    * and the robot controller
    * \param script_sending_port The driver will offer an interface to receive the program's URScript on this port.
    * If the robot cannot connect to this port, `External Control` will stop immediately.
+   * \param non_blocking_read Enable non-blocking mode for read (useful when used with combined_robot_hw)
    * \param servoj_gain Proportional gain for arm joints following target position, range [100,2000]
    * \param servoj_lookahead_time Time [S], range [0.03,0.2] smoothens the trajectory with this lookahead time
    */
   UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file,
            const std::string& input_recipe_file, std::function<void(bool)> handle_program_state, bool headless_mode,
            const std::string& calibration_checksum = "", const uint32_t reverse_port = 50001,
-           const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03)
+           const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03,
+           bool non_blocking_read = false)
     : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode,
                std::unique_ptr<ToolCommSetup>{}, calibration_checksum, reverse_port, script_sender_port, servoj_gain,
-               servoj_lookahead_time)
+               servoj_lookahead_time, non_blocking_read)
   {
   }
 
@@ -223,6 +226,9 @@ private:
   bool in_headless_mode_;
   std::string full_robot_program_;
 
+  int get_packet_timeout_;
+  bool non_blocking_read_;
+
   VersionInformation robot_version_;
 };
 }  // namespace ur_driver

ur_robot_driver/src/ros/hardware_interface.cpp

@@ -104,6 +104,11 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
     return false;
   }
 
+  // Enables non_blocking_read mode. Useful when used with combined_robot_hw. Disables error
+  // generated when read returns without any data, sets the read timeout to zero, and
+  // synchronises read/write operations.
+  robot_hw_nh.param("non_blocking_read", non_blocking_read_, false);
+
   // Specify gain for servoing to position in joint space.
   // A higher gain can sharpen the trajectory.
   int servoj_gain = robot_hw_nh.param("servoj_gain", 2000);
@@ -233,7 +238,7 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
                                   std::bind(&HardwareInterface::handleRobotProgramState, this, std::placeholders::_1),
                                   headless_mode, std::move(tool_comm_setup), calibration_checksum,
                                   (uint32_t)reverse_port, (uint32_t)script_sender_port, servoj_gain,
-                                  servoj_lookahead_time));
+                                  servoj_lookahead_time, non_blocking_read_));
   }
   catch (ur_driver::ToolCommNotAvailable& e)
   {
@@ -380,6 +385,7 @@ void HardwareInterface::read(const ros::Time& time, const ros::Duration& period)
   std::unique_ptr<rtde_interface::DataPackage> data_pkg = ur_driver_->getDataPackage();
   if (data_pkg)
   {
+    packet_read_ = true;
     readData(data_pkg, "actual_q", joint_positions_);
     readData(data_pkg, "actual_qd", joint_velocities_);
     readData(data_pkg, "target_speed_fraction", target_speed_fraction_);
@@ -450,7 +456,10 @@ void HardwareInterface::read(const ros::Time& time, const ros::Duration& period)
   }
   else
   {
-    ROS_ERROR("Could not get fresh data package from robot");
+    if (!non_blocking_read_)
+    {
+      ROS_ERROR("Could not get fresh data package from robot");
+    }
   }
 }
 
@@ -458,7 +467,7 @@ void HardwareInterface::write(const ros::Time& time, const ros::Duration& period
 {
   if ((runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PLAYING) ||
        runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PAUSING)) &&
-      robot_program_running_)
+      robot_program_running_ && (!non_blocking_read_ || packet_read_))
   {
     if (position_controller_running_)
     {
@@ -472,6 +481,7 @@ void HardwareInterface::write(const ros::Time& time, const ros::Duration& period
     {
       ur_driver_->stopControl();
     }
+    packet_read_ = false;
   }
 }
 

ur_robot_driver/src/ur/ur_driver.cpp

@@ -52,7 +52,7 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
                               std::function<void(bool)> handle_program_state, bool headless_mode,
                               std::unique_ptr<ToolCommSetup> tool_comm_setup, const std::string& calibration_checksum,
                               const uint32_t reverse_port, const uint32_t script_sender_port, int servoj_gain,
-                              double servoj_lookahead_time)
+                              double servoj_lookahead_time, bool non_blocking_read)
   : servoj_time_(0.008)
   , servoj_gain_(servoj_gain)
   , servoj_lookahead_time_(servoj_lookahead_time)
@@ -73,6 +73,9 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
   LOG_INFO("Checking if calibration data matches connected robot.");
   checkCalibration(calibration_checksum);
 
+  non_blocking_read_ = non_blocking_read;
+  get_packet_timeout_ = non_blocking_read_ ? 0 : 100;
+
   if (!rtde_client_->init())
   {
     throw UrException("Initialization of RTDE client went wrong.");
@@ -142,8 +145,11 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
 
 std::unique_ptr<rtde_interface::DataPackage> ur_driver::UrDriver::getDataPackage()
 {
-  std::chrono::milliseconds timeout(100);  // We deliberately have a quite large timeout here, as the robot itself
-                                           // should command the control loop's timing.
+  // This can take one of two values, 0ms or 100ms. The large timeout is for when the robot is commanding the control
+  // loop's timing (read is blocking). The zero timeout is for when the robot is sharing a control loop with
+  // something else (combined_robot_hw)
+  std::chrono::milliseconds timeout(get_packet_timeout_);
+
   return rtde_client_->getDataPackage(timeout);
 }