Backport: Change driver constructor and change calibration check (#282) (#302)

* Change driver constructor and change calibration check (#282)

* fix loading the kinematics hash without quotes

Co-authored-by: livanov93 <[email protected]>
Co-authored-by: Felix Exner <[email protected]>

Author: 3 people

ur_description/urdf/ur.ros2_control.xacro

@@ -29,7 +29,7 @@
           <param name="servoj_gain">2000</param>
           <param name="servoj_lookahead_time">0.03</param>
           <param name="use_tool_communication">0</param>
-          <param name="kinematics/hash">"${hash_kinematics}"</param>
+          <param name="kinematics/hash">${hash_kinematics}</param>
           <param name="tool_voltage">0</param>
           <param name="tool_parity">0</param>
           <param name="tool_baud_rate">115200</param>

ur_robot_driver/src/hardware_interface.cpp

@@ -341,8 +341,8 @@ return_type URPositionHardwareInterface::start()
     ur_driver_ = std::make_unique<urcl::UrDriver>(
         robot_ip, script_filename, output_recipe_filename, input_recipe_filename,
         std::bind(&URPositionHardwareInterface::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, non_blocking_read_);
+        std::move(tool_comm_setup), (uint32_t)reverse_port, (uint32_t)script_sender_port, servoj_gain,
+        servoj_lookahead_time, non_blocking_read_);
   } catch (urcl::ToolCommNotAvailable& e) {
     RCLCPP_FATAL_STREAM(rclcpp::get_logger("URPositionHardwareInterface"), "See parameter use_tool_communication");
 
@@ -351,6 +351,28 @@ return_type URPositionHardwareInterface::start()
     RCLCPP_FATAL_STREAM(rclcpp::get_logger("URPositionHardwareInterface"), e.what());
     return return_type::ERROR;
   }
+  RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Calibration checksum: '%s'.",
+              calibration_checksum.c_str());
+  // check calibration
+  // https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/c3378599d5fa73a261328b326392e847f312ab6b/ur_robot_driver/src/hardware_interface.cpp#L296-L309
+  if (ur_driver_->checkCalibration(calibration_checksum)) {
+    RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Calibration checked successfully.");
+  } else {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("URPositionHardwareInterface"), "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 "
+                                                                           "[https://github.com/UniversalRobots/"
+                                                                           "Universal_Robots_ROS_Driver#extract-"
+                                                                           "calibration-information] "
+                                                                           "for details.");
+  }
 
   ur_driver_->startRTDECommunication();