move calibration check to ros-independent part

ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h

@@ -95,8 +95,6 @@ protected:
    */
   void publishPose();
 
-  void checkCalibration(const std::string& checksum);
-
   bool stopControl(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
 
   std::unique_ptr<UrDriver> ur_driver_;

ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h

@@ -52,7 +52,8 @@ public:
    * \param tool_comm_setup Configuration for using the tool communication
    */
   UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& recipe_file,
-           std::function<void(bool)> handle_program_state, std::unique_ptr<ToolCommSetup> tool_comm_setup);
+           std::function<void(bool)> handle_program_state, std::unique_ptr<ToolCommSetup> tool_comm_setup,
+           const std::string& calibration_checksum = "");
   /*!
    * \brief Constructs a new UrDriver object.
    *
@@ -60,8 +61,9 @@ public:
    * \param script_file URScript file that should be sent to the robot
    */
   UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& recipe_file,
-           std::function<void(bool)> handle_program_state)
-    : UrDriver(robot_ip, script_file, recipe_file, handle_program_state, std::unique_ptr<ToolCommSetup>{})
+           std::function<void(bool)> handle_program_state, const std::string& calibration_checksum = "")
+    : UrDriver(robot_ip, script_file, recipe_file, handle_program_state, std::unique_ptr<ToolCommSetup>{},
+               calibration_checksum)
   {
   }
 
@@ -111,6 +113,8 @@ public:
 
   void startWatchdog();
 
+  void checkCalibration(const std::string& checksum);
+
 private:
   std::string readScriptFile(const std::string& filename);
   std::string readKeepalive();
@@ -129,6 +133,8 @@ private:
   bool reverse_interface_active_;
   uint32_t reverse_port_;
   std::function<void(bool)> handle_program_state_;
+
+  std::string robot_ip_;
 };
 }  // namespace ur_driver
 #endif  // ifndef UR_RTDE_DRIVER_UR_UR_DRIVER_H_INCLUDED

ur_rtde_driver/src/ros/hardware_interface.cpp

@@ -31,10 +31,6 @@
 
 #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()
@@ -72,10 +68,6 @@ 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";
@@ -137,12 +129,13 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
     tool_comm_setup->setTxIdleChars(tx_idle_chars);
   }
 
+  std::string calibration_checksum = robot_hw_nh.param<std::string>("kinematics/hash", "");
   ROS_INFO_STREAM("Initializing urdriver");
   try
   {
     ur_driver_.reset(new UrDriver(robot_ip_, script_filename, recipe_filename,
                                   std::bind(&HardwareInterface::handleRobotProgramState, this, std::placeholders::_1),
-                                  std::move(tool_comm_setup)));
+                                  std::move(tool_comm_setup), calibration_checksum));
   }
   catch (ur_driver::ToolCommNotAvailable& e)
   {
@@ -435,27 +428,6 @@ 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.");
-}
-
 bool HardwareInterface::stopControl(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res)
 {
   if (isRobotProgramRunning())

ur_rtde_driver/src/ur/ur_driver.cpp

@@ -32,8 +32,11 @@
 
 #include "ur_rtde_driver/ur/ur_driver.h"
 #include "ur_rtde_driver/exceptions.h"
+#include "ur_rtde_driver/primary/primary_parser.h"
 #include <memory>
 
+#include <ur_rtde_driver/ur/calibration_checker.h>
+
 namespace ur_driver
 {
 static const int32_t MULT_JOINTSTATE = 1000000;
@@ -45,16 +48,17 @@ static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}");
 
 ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file,
                               const std::string& recipe_file, std::function<void(bool)> handle_program_state,
-                              std::unique_ptr<ToolCommSetup> tool_comm_setup)
+                              std::unique_ptr<ToolCommSetup> tool_comm_setup, const std::string& calibration_checksum)
   : servoj_time_(0.008)
   , servoj_gain_(2000)
   , servoj_lookahead_time_(0.03)
   , reverse_interface_active_(false)
   , handle_program_state_(handle_program_state)
+  , robot_ip_(robot_ip)
 {
   LOG_DEBUG("Initializing urdriver");
   LOG_DEBUG("Initializing RTDE client");
-  rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip, notifier_, recipe_file));
+  rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, recipe_file));
 
   if (!rtde_client_->init())
   {
@@ -66,6 +70,9 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
 
   std::string local_ip = rtde_client_->getIP();
 
+  LOG_INFO("Checking if calibration data matches connected robot.");
+  checkCalibration(calibration_checksum);
+
   uint32_t reverse_port = 50001;        // TODO: Make this a parameter
   uint32_t script_sender_port = 50002;  // TODO: Make this a parameter
 
@@ -205,4 +212,26 @@ std::string UrDriver::readKeepalive()
     return std::string("");
   }
 }
+
+void UrDriver::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