Add a checker to inform the user about an uncalibrated robot.

ur_rtde_driver/CMakeLists.txt

@@ -19,6 +19,7 @@ find_package(catkin REQUIRED
     roscpp
     sensor_msgs
     std_srvs
+    tf
     tf2_geometry_msgs
     tf2_msgs
     trajectory_msgs
@@ -41,6 +42,7 @@ catkin_package(
     industrial_msgs
     roscpp
     sensor_msgs
+    tf
     tf2_geometry_msgs
     tf2_msgs
     trajectory_msgs
@@ -99,6 +101,7 @@ add_library(ur_rtde_driver
     src/rtde/rtde_client.cpp
     src/ur/ur_driver.cpp
     src/ur/tool_communication.cpp
+    src/ros/tcp_accuracy_checker.cpp
 )
 target_link_libraries(ur_rtde_driver ${catkin_LIBRARIES})
 add_dependencies(ur_rtde_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h

@@ -41,6 +41,7 @@
 #include <ur_controllers/scaled_joint_command_interface.h>
 
 #include "ur_rtde_driver/ur/ur_driver.h"
+#include "ur_rtde_driver/ros/tcp_accuracy_checker.h"
 
 namespace ur_driver
 {
@@ -131,6 +132,8 @@ protected:
   ros::Publisher program_state_pub_;
 
   bool controller_reset_necessary_;
+
+  std::unique_ptr<TcpAccuracyChecker> tcp_accuracy_checker_;
 };
 
 }  // namespace ur_driver

ur_rtde_driver/include/ur_rtde_driver/ros/tcp_accuracy_checker.h

@@ -0,0 +1,50 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-06-13
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_RTDE_DRIVER_ROS_TCP_ACCURACY_CHECKER_H_INCLUDED
+#define UR_RTDE_DRIVER_ROS_TCP_ACCURACY_CHECKER_H_INCLUDED
+
+#include <tf/transform_listener.h>
+#include <thread>
+
+namespace ur_driver
+{
+class TcpAccuracyChecker
+{
+public:
+  TcpAccuracyChecker() = delete;
+  TcpAccuracyChecker(const std::string& frame_a, const std::string& frame_b, const double desired_accuracy);
+  virtual ~TcpAccuracyChecker() = default;
+
+  /*!
+   * \brief Performs a lookup between the configured frames and checks if the distance is smaller
+   * than the given tolerance.
+   *
+   * \returns True of accuracy is below the tolerance
+   */
+  bool checkAccuracy();
+
+  void asyncCheck(const uint32_t interval, const uint32_t num_checks);
+
+private:
+  tf::TransformListener tf_listener_;
+  tf::StampedTransform transform_;
+  std::string frame_a_;
+  std::string frame_b_;
+  std::unique_ptr<std::thread> worker_thread_;
+  double desired_accuracy_;
+  double actual_accuracy_;
+};
+}  // namespace ur_driver
+#endif  // ifndef UR_RTDE_DRIVER_ROS_TCP_ACCURACY_CHECKER_H_INCLUDED

ur_rtde_driver/package.xml

@@ -28,6 +28,7 @@
   <depend>industrial_msgs</depend>
   <depend>roscpp</depend>
   <depend>sensor_msgs</depend>
+  <depend>tf</depend>
   <depend>tf2_msgs</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>trajectory_msgs</depend>

ur_rtde_driver/src/ros/hardware_interface.cpp

@@ -65,10 +65,10 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
     return false;
   }
 
-  tcp_link_ = robot_hw_nh.param<std::string>("tcp_link", "tool0");
+  std::string tf_prefix = robot_hw_nh.param<std::string>("tf_prefix", "");
   program_state_pub_ = robot_hw_nh.advertise<std_msgs::Bool>("robot_program_running", 10, true);
   tcp_transform_.header.frame_id = "base";
-  tcp_transform_.child_frame_id = "tool_0_controller";
+  tcp_transform_.child_frame_id = "tool0_controller";
 
   bool use_tool_communication = robot_hw_nh.param<bool>("use_tool_communication", "false");
   std::unique_ptr<ToolCommSetup> tool_comm_setup;
@@ -172,7 +172,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
       ur_controllers::SpeedScalingHandle("speed_scaling_factor", &speed_scaling_combined_));
 
   fts_interface_.registerHandle(hardware_interface::ForceTorqueSensorHandle(
-      "wrench", tcp_link_, fts_measurements_.begin(), fts_measurements_.begin() + 3));
+      "wrench", "tool0_controller", fts_measurements_.begin(), fts_measurements_.begin() + 3));
 
   // Register interfaces
   registerInterface(&js_interface_);
@@ -184,6 +184,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
   tcp_pose_pub_.reset(new realtime_tools::RealtimePublisher<tf2_msgs::TFMessage>(root_nh, "/tf", 100));
 
   ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_rtde_driver hardware_interface");
+  tcp_accuracy_checker_.reset(new TcpAccuracyChecker("tool0", "tool0_controller", 1e-9));
 
   return true;
 }

ur_rtde_driver/src/ros/tcp_accuracy_checker.cpp

@@ -0,0 +1,60 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-06-13
+ *
+ */
+//----------------------------------------------------------------------
+
+#include <ur_rtde_driver/ros/tcp_accuracy_checker.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+namespace ur_driver
+{
+TcpAccuracyChecker::TcpAccuracyChecker(const std::string& frame_a, const std::string& frame_b,
+                                       const double desired_accuracy)
+  : frame_a_(frame_a), frame_b_(frame_b), desired_accuracy_(desired_accuracy), actual_accuracy_(0)
+{
+  worker_thread_.reset(new std::thread(&TcpAccuracyChecker::asyncCheck, this, 2, 10));
+}
+
+bool TcpAccuracyChecker::checkAccuracy()
+{
+  try
+  {
+    tf_listener_.waitForTransform(frame_a_, frame_b_, ros::Time(0), ros::Duration(120));
+    tf_listener_.lookupTransform(frame_a_, frame_b_, ros::Time(0), transform_);
+  }
+  catch (tf::TransformException ex)
+  {
+    ROS_ERROR("TF lookup error error: %s", ex.what());
+  }
+
+  actual_accuracy_ = transform_.getOrigin().length();
+  return actual_accuracy_ <= desired_accuracy_;
+}
+
+void TcpAccuracyChecker::asyncCheck(const uint32_t interval, const uint32_t num_checks)
+{
+  ros::Rate r(1.0 / interval);
+  for (size_t i = 0; i < num_checks; ++i)
+  {
+    if (!checkAccuracy())
+    {
+      ROS_ERROR_STREAM("Desired accuracy of "
+                       << desired_accuracy_ << " between " << frame_a_ << " and " << frame_b_
+                       << " was violated. Actual difference: " << actual_accuracy_ << std::endl
+                       << "This is critical! Your robot might not be at the expected position." << std::endl
+                       << "Use the ur_calibration tool to extract the correct calibration from the robot and pass that "
+                          "into the description. See [TODO Link to documentation] for details.");
+    }
+    r.sleep();
+  }
+}
+}  // namespace ur_driver