replace tcp_accuracy_checker with hash comparison

This closes #34

ur_calibration/src/calibration_consumer.cpp

@@ -50,6 +50,7 @@ bool CalibrationConsumer::consume(
     calibration.correctChain();
 
     calibration_parameters_ = calibration.toYaml();
+    calibration_parameters_["kinematics"]["hash"] = kin_info->toHash();
     calibrated_ = true;
   }
   return true;

ur_calibration/src/calibration_correction.cpp

@@ -46,8 +46,6 @@ using namespace ur_driver;
 using namespace primary_interface;
 using namespace ur_calibration;
 
-static const int UR_PRIMARY_PORT = 30001;
-
 class ParamaterMissingException : public ros::Exception
 {
 public:

ur_rtde_driver/CMakeLists.txt

@@ -98,8 +98,8 @@ add_library(ur_rtde_driver
     src/rtde/text_message.cpp
     src/rtde/rtde_client.cpp
     src/ur/ur_driver.cpp
+    src/ur/calibration_checker.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/primary/package_header.h

@@ -38,6 +38,7 @@ namespace ur_driver
 {
 namespace primary_interface
 {
+static const int UR_PRIMARY_PORT = 30001;
 enum class RobotPackageType : int8_t
 {
   DISCONNECT = -1,

ur_rtde_driver/include/ur_rtde_driver/primary/robot_state/kinematics_info.h

@@ -51,6 +51,8 @@ public:
   virtual bool parseWith(comm::BinParser& bp);
   virtual std::string toString() const;
 
+  std::string toHash() const;
+
   vector6uint32_t checksum_;
   vector6d_t dh_theta_;
   vector6d_t dh_a_;

ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h

@@ -41,7 +41,6 @@
 #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
 {
@@ -95,6 +94,8 @@ protected:
    */
   void publishPose();
 
+  void checkCalibration(const std::string& checksum);
+
   std::unique_ptr<UrDriver> ur_driver_;
 
   hardware_interface::JointStateInterface js_interface_;
@@ -133,7 +134,7 @@ protected:
 
   bool controller_reset_necessary_;
 
-  std::unique_ptr<TcpAccuracyChecker> tcp_accuracy_checker_;
+  std::string robot_ip_;
 };
 
 }  // namespace ur_driver

ur_rtde_driver/include/ur_rtde_driver/ur/calibration_checker.h

@@ -20,44 +20,49 @@
 /*!\file
  *
  * \author  Felix Mauch mauch@fzi.de
- * \date    2019-06-13
+ * \date    2019-06-14
  *
  */
 //----------------------------------------------------------------------
+#ifndef UR_RTDE_DRIVER_UR_CALIBRATION_CHECKER_H_INCLUDED
+#define UR_RTDE_DRIVER_UR_CALIBRATION_CHECKER_H_INCLUDED
 
-#ifndef UR_RTDE_DRIVER_ROS_TCP_ACCURACY_CHECKER_H_INCLUDED
+#include <ur_rtde_driver/comm/pipeline.h>
-#define UR_RTDE_DRIVER_ROS_TCP_ACCURACY_CHECKER_H_INCLUDED
 
-#include <tf/transform_listener.h>
+#include <ur_rtde_driver/primary/robot_state/kinematics_info.h>
-#include <thread>
 
 namespace ur_driver
 {
-class TcpAccuracyChecker
+class CalibrationChecker : public comm::IConsumer<comm::URPackage<primary_interface::PackageHeader>>
 {
 public:
-  TcpAccuracyChecker() = delete;
+  CalibrationChecker(const std::string& expected_hash);
-  TcpAccuracyChecker(const std::string& frame_a, const std::string& frame_b, const double desired_accuracy);
+  virtual ~CalibrationChecker() = default;
-  virtual ~TcpAccuracyChecker() = default;
 
-  /*!
+  virtual void setupConsumer()
-   * \brief Performs a lookup between the configured frames and checks if the distance is smaller
+  {
-   * than the given tolerance.
+  }
-   *
+  virtual void teardownConsumer()
-   * \returns True of accuracy is below the tolerance
+  {
-   */
+  }
-  bool checkAccuracy();
+  virtual void stopConsumer()
+  {
+  }
+  virtual void onTimeout()
+  {
+  }
 
-  void asyncCheck(const uint32_t interval, const uint32_t num_checks);
+  virtual bool consume(std::shared_ptr<comm::URPackage<primary_interface::PackageHeader>> product);
+
+  bool isChecked()
+  {
+    return checked_;
+  }
 
 private:
-  tf::TransformListener tf_listener_;
+  std::string expected_hash_;
-  tf::StampedTransform transform_;
+  bool checked_;
-  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
+
+#endif  // ifndef UR_RTDE_DRIVER_UR_CALIBRATION_CHECKER_H_INCLUDED

ur_rtde_driver/launch/ur_common.launch

@@ -33,6 +33,7 @@
     <arg name="use_tool_communication" value="$(arg use_tool_communication)"/>
     <arg name="controller_config_file" value="$(arg controller_config_file)"/>
     <arg name="robot_ip" value="$(arg robot_ip)"/>
+    <arg name="kinematics_config" value="$(arg kinematics_config)"/>
     <arg name="tf_prefix" value="$(arg tf_prefix)"/>
     <arg name="controllers" value="$(arg controllers)"/>
     <arg name="tool_voltage" value="$(arg tool_voltage)"/>

ur_rtde_driver/launch/ur_control.launch

@@ -9,6 +9,7 @@
   <arg name="use_tool_communication"/>
   <arg name="controller_config_file"/>
   <arg name="robot_ip"/>
+  <arg name="kinematics_config"/>
   <arg name="tf_prefix" default="" />
   <arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
   <arg name="stopped_controllers" default=""/>
@@ -26,6 +27,7 @@
   <!-- Load hardware interface -->
   <node name="ur_hardware_interface" pkg="ur_rtde_driver" type="ur_rtde_driver_node" output="screen" launch-prefix="$(arg launch_prefix)" required="true">
     <param name="robot_ip" type="str" value="$(arg robot_ip)"/>
+    <rosparam command="load" file="$(arg kinematics_config)" />
     <param name="script_file" value="$(arg urscript_file)"/>
     <param name="recipe_file" value="$(arg rtde_recipe_file)"/>
     <param name="tf_prefix" value="$(arg tf_prefix)"/>

ur_rtde_driver/src/primary/robot_state/kinematics_info.cpp

@@ -87,5 +87,19 @@ std::string KinematicsInfo::toString() const
 
   return os.str();
 }
+
+std::string KinematicsInfo ::toHash() const
+{
+  std::stringstream ss;
+  for (size_t i = 0; i < 6; ++i)
+  {
+    ss << dh_theta_[i];
+    ss << dh_d_[i];
+    ss << dh_a_[i];
+    ss << dh_alpha_[i];
+  }
+  std::hash<std::string> hash_fn;
+  return "calib_" + std::to_string(hash_fn(ss.str()));
+}
 }  // namespace primary_interface
 }  // namespace ur_driver

ur_rtde_driver/src/ros/hardware_interface.cpp

@@ -31,6 +31,10 @@
 
 #include <Eigen/Geometry>
 
+#include <ur_rtde_driver/primary/package_header.h>
+#include <ur_rtde_driver/primary/primary_parser.h>
+#include <ur_rtde_driver/ur/calibration_checker.h>
+
 namespace ur_driver
 {
 HardwareInterface::HardwareInterface()
@@ -50,7 +54,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
 {
   joint_velocities_ = { { 0, 0, 0, 0, 0, 0 } };
   joint_efforts_ = { { 0, 0, 0, 0, 0, 0 } };
-  std::string robot_ip = robot_hw_nh.param<std::string>("robot_ip", "192.168.56.101");
+  robot_ip_ = robot_hw_nh.param<std::string>("robot_ip", "192.168.56.101");
   std::string script_filename;
   std::string recipe_filename;
   if (!robot_hw_nh.getParam("script_file", script_filename))
@@ -66,6 +70,11 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
   }
 
   std::string tf_prefix = robot_hw_nh.param<std::string>("tf_prefix", "");
+
+  ROS_INFO_STREAM("Checking if calibration data matches connected robot.");
+  std::string calibration_checksum = robot_hw_nh.param<std::string>("kinematics/hash", "");
+  checkCalibration(calibration_checksum);
+
   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 = "tool0_controller";
@@ -130,7 +139,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
   ROS_INFO_STREAM("Initializing urdriver");
   try
   {
-    ur_driver_.reset(new UrDriver(robot_ip, script_filename, recipe_filename,
+    ur_driver_.reset(new UrDriver(robot_ip_, script_filename, recipe_filename,
                                   std::bind(&HardwareInterface::handleRobotProgramState, this, std::placeholders::_1),
                                   std::move(tool_comm_setup)));
   }
@@ -184,7 +193,6 @@ 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;
 }
@@ -394,4 +402,24 @@ void HardwareInterface ::publishPose()
   }
 }
 
+void HardwareInterface ::checkCalibration(const std::string& checksum)
+{
+  comm::URStream<ur_driver::primary_interface::PackageHeader> stream(robot_ip_,
+                                                                     ur_driver::primary_interface::UR_PRIMARY_PORT);
+  primary_interface::PrimaryParser parser;
+  comm::URProducer<ur_driver::primary_interface::PackageHeader> prod(stream, parser);
+
+  CalibrationChecker consumer(checksum);
+
+  comm::INotifier notifier;
+
+  comm::Pipeline<ur_driver::primary_interface::PackageHeader> pipeline(prod, consumer, "Pipeline", notifier);
+  pipeline.run();
+
+  while (!consumer.isChecked())
+  {
+    ros::Duration(1).sleep();
+  }
+  ROS_DEBUG_STREAM("Got calibration information from robot.");
+}
 }  // namespace ur_driver

ur_rtde_driver/src/ros/tcp_accuracy_checker.cpp

@@ -1,73 +0,0 @@
-// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
-
-// -- BEGIN LICENSE BLOCK ----------------------------------------------
-// Copyright 2019 FZI Forschungszentrum Informatik
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-// -- 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

ur_rtde_driver/src/ur/calibration_checker.cpp

@@ -0,0 +1,47 @@
+// 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-14
+ *
+ */
+//----------------------------------------------------------------------
+
+#include <ur_rtde_driver/ur/calibration_checker.h>
+
+namespace ur_driver
+{
+CalibrationChecker::CalibrationChecker(const std::string& expected_hash)
+  : expected_hash_(expected_hash), checked_(false)
+{
+}
+bool CalibrationChecker::consume(std::shared_ptr<comm::URPackage<primary_interface::PackageHeader>> product)
+{
+  auto kin_info = std::dynamic_pointer_cast<primary_interface::KinematicsInfo>(product);
+  if (kin_info != nullptr)
+  {
+    // LOG_INFO("%s", product->toString().c_str());
+    //
+    if (kin_info->toHash() != expected_hash_)
+    {
+      LOG_ERROR("The calibration parameters of the connected robot don't match the ones from the given kinematics "
+                "config file. Please be aware that this can lead to critical inaccuracies of tcp positions. 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.");
+    }
+    else
+    {
+      LOG_INFO("Calibration checked successfully.");
+    }
+
+    checked_ = true;
+  }
+
+  return true;
+}
+}  // namespace ur_driver