From b73112e2f0afe0f49b2d7a534392e2930da73a96 Mon Sep 17 00:00:00 2001
From: urmarp <marp@universal-robots.com>
Date: Wed, 28 Dec 2022 11:40:12 +0100
Subject: [PATCH] Updated for improvement of primary interface

Added feature to tell if robot is simulated or real
---
 ur_robot_driver/launch/ur10_bringup.launch  |  1 +
 ur_robot_driver/launch/ur10e_bringup.launch |  1 +
 ur_robot_driver/launch/ur16e_bringup.launch |  1 +
 ur_robot_driver/launch/ur3_bringup.launch   |  1 +
 ur_robot_driver/launch/ur3e_bringup.launch  |  1 +
 ur_robot_driver/launch/ur5_bringup.launch   |  1 +
 ur_robot_driver/launch/ur5e_bringup.launch  |  1 +
 ur_robot_driver/launch/ur_common.launch     |  4 +++-
 ur_robot_driver/launch/ur_control.launch    |  2 ++
 ur_robot_driver/src/hardware_interface.cpp  | 22 +++++++--------------
 ur_robot_driver/test/driver.test            |  1 +
 11 files changed, 20 insertions(+), 16 deletions(-)

diff --git a/ur_robot_driver/launch/ur10_bringup.launch b/ur_robot_driver/launch/ur10_bringup.launch
index 8686e418c..5b5312af7 100644
--- a/ur_robot_driver/launch/ur10_bringup.launch
+++ b/ur_robot_driver/launch/ur10_bringup.launch
@@ -14,6 +14,7 @@
   <arg name="robot_description_file" default="$(find ur_description)/launch/load_ur10.launch" doc="Robot description launch file."/>
   <arg name="kinematics_config" default="$(find ur_description)/config/ur10/default_kinematics.yaml" doc="Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description."/>
   <arg name="headless_mode" default="false" doc="Automatically send URScript to robot to execute. On e-Series this does require the robot to be in 'remote-control' mode. With this, the URCap is not needed on the robot."/>
+  <arg name="simulated_robot" default="false" doc="Determine whether the robot to control is simulated or not."/>
   <arg name="ur_hardware_interface_node_required" default="true" doc="Shut down ros environment if ur_hardware_interface-node dies."/>
 
   <include file="$(find ur_robot_driver)/launch/ur_common.launch" pass_all_args="true">
diff --git a/ur_robot_driver/launch/ur10e_bringup.launch b/ur_robot_driver/launch/ur10e_bringup.launch
index 2b98f6407..4962672ec 100644
--- a/ur_robot_driver/launch/ur10e_bringup.launch
+++ b/ur_robot_driver/launch/ur10e_bringup.launch
@@ -24,6 +24,7 @@
   <arg name="tool_device_name" default="/tmp/ttyUR" doc="Local device name used for tool communication. Only used, when `use_tool_communication` is set to true."/>
   <arg name="tool_tcp_port" default="54321" doc="Port on which the robot controller publishes the tool comm interface. Only used, when `use_tool_communication` is set to true."/>
   <arg name="headless_mode" default="false" doc="Automatically send URScript to robot to execute. On e-Series this does require the robot to be in 'remote-control' mode. With this, the URCap is not needed on the robot."/>
+  <arg name="simulated_robot" default="false" doc="Determine whether the robot to control is simulated or not."/>
   <arg name="ur_hardware_interface_node_required" default="true" doc="Shut down ros environment if ur_hardware_interface-node dies."/>
 
   <include file="$(find ur_robot_driver)/launch/ur_common.launch" pass_all_args="true"/>
diff --git a/ur_robot_driver/launch/ur16e_bringup.launch b/ur_robot_driver/launch/ur16e_bringup.launch
index 40b6a7bf8..3930b82da 100644
--- a/ur_robot_driver/launch/ur16e_bringup.launch
+++ b/ur_robot_driver/launch/ur16e_bringup.launch
@@ -24,6 +24,7 @@
   <arg name="tool_device_name" default="/tmp/ttyUR" doc="Local device name used for tool communication. Only used, when `use_tool_communication` is set to true."/>
   <arg name="tool_tcp_port" default="54321" doc="Port on which the robot controller publishes the tool comm interface. Only used, when `use_tool_communication` is set to true."/>
   <arg name="headless_mode" default="false" doc="Automatically send URScript to robot to execute. On e-Series this does require the robot to be in 'remote-control' mode. With this, the URCap is not needed on the robot."/>
+  <arg name="simulated_robot" default="false" doc="Determine whether the robot to control is simulated or not."/>
   <arg name="ur_hardware_interface_node_required" default="true" doc="Shut down ros environment if ur_hardware_interface-node dies."/>
 
   <include file="$(find ur_robot_driver)/launch/ur_common.launch" pass_all_args="true"/>
diff --git a/ur_robot_driver/launch/ur3_bringup.launch b/ur_robot_driver/launch/ur3_bringup.launch
index 764b36634..4d136ec91 100644
--- a/ur_robot_driver/launch/ur3_bringup.launch
+++ b/ur_robot_driver/launch/ur3_bringup.launch
@@ -14,6 +14,7 @@
   <arg name="robot_description_file" default="$(find ur_description)/launch/load_ur3.launch" doc="Robot description launch file."/>
   <arg name="kinematics_config" default="$(find ur_description)/config/ur3/default_kinematics.yaml" doc="Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description."/>
   <arg name="headless_mode" default="false" doc="Automatically send URScript to robot to execute. On e-Series this does require the robot to be in 'remote-control' mode. With this, the URCap is not needed on the robot."/>
+  <arg name="simulated_robot" default="false" doc="Determine whether the robot to control is simulated or not."/>
   <arg name="ur_hardware_interface_node_required" default="true" doc="Shut down ros environment if ur_hardware_interface-node dies."/>
 
   <include file="$(find ur_robot_driver)/launch/ur_common.launch" pass_all_args="true">
diff --git a/ur_robot_driver/launch/ur3e_bringup.launch b/ur_robot_driver/launch/ur3e_bringup.launch
index bbf4ff696..e2ebc69d5 100644
--- a/ur_robot_driver/launch/ur3e_bringup.launch
+++ b/ur_robot_driver/launch/ur3e_bringup.launch
@@ -23,6 +23,7 @@
   <arg name="tool_device_name" default="/tmp/ttyUR" doc="Local device name used for tool communication. Only used, when `use_tool_communication` is set to true."/>
   <arg name="tool_tcp_port" default="54321" doc="Port on which the robot controller publishes the tool comm interface. Only used, when `use_tool_communication` is set to true."/>
   <arg name="headless_mode" default="false" doc="Automatically send URScript to robot to execute. On e-Series this does require the robot to be in 'remote-control' mode. With this, the URCap is not needed on the robot."/>
+  <arg name="simulated_robot" default="false" doc="Determine whether the robot to control is simulated or not."/>
   <arg name="ur_hardware_interface_node_required" default="true" doc="Shut down ros environment if ur_hardware_interface-node dies."/>
 
   <include file="$(find ur_robot_driver)/launch/ur_common.launch" pass_all_args="true"/>
diff --git a/ur_robot_driver/launch/ur5_bringup.launch b/ur_robot_driver/launch/ur5_bringup.launch
index 619118c2a..926731c68 100644
--- a/ur_robot_driver/launch/ur5_bringup.launch
+++ b/ur_robot_driver/launch/ur5_bringup.launch
@@ -14,6 +14,7 @@
   <arg name="robot_description_file" default="$(find ur_description)/launch/load_ur5.launch" doc="Robot description launch file."/>
   <arg name="kinematics_config" default="$(find ur_description)/config/ur5/default_kinematics.yaml" doc="Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description."/>
   <arg name="headless_mode" default="false" doc="Automatically send URScript to robot to execute. On e-Series this does require the robot to be in 'remote-control' mode. With this, the URCap is not needed on the robot."/>
+  <arg name="simulated_robot" default="false" doc="Determine whether the robot to control is simulated or not."/>
   <arg name="ur_hardware_interface_node_required" default="true" doc="Shut down ros environment if ur_hardware_interface-node dies."/>
 
   <include file="$(find ur_robot_driver)/launch/ur_common.launch" pass_all_args="true">
diff --git a/ur_robot_driver/launch/ur5e_bringup.launch b/ur_robot_driver/launch/ur5e_bringup.launch
index 10a03b253..5c440cb4f 100644
--- a/ur_robot_driver/launch/ur5e_bringup.launch
+++ b/ur_robot_driver/launch/ur5e_bringup.launch
@@ -23,6 +23,7 @@
   <arg name="tool_device_name" default="/tmp/ttyUR" doc="Local device name used for tool communication. Only used, when `use_tool_communication` is set to true."/>
   <arg name="tool_tcp_port" default="54321" doc="Port on which the robot controller publishes the tool comm interface. Only used, when `use_tool_communication` is set to true."/>
   <arg name="headless_mode" default="false" doc="Automatically send URScript to robot to execute. On e-Series this does require the robot to be in 'remote-control' mode. With this, the URCap is not needed on the robot."/>
+  <arg name="simulated_robot" default="false" doc="Determine whether the robot to control is simulated or not."/>
   <arg name="ur_hardware_interface_node_required" default="true" doc="Shut down ros environment if ur_hardware_interface-node dies."/>
 
   <include file="$(find ur_robot_driver)/launch/ur_common.launch" pass_all_args="true" />
diff --git a/ur_robot_driver/launch/ur_common.launch b/ur_robot_driver/launch/ur_common.launch
index 1699ec2b2..b41944ef5 100644
--- a/ur_robot_driver/launch/ur_common.launch
+++ b/ur_robot_driver/launch/ur_common.launch
@@ -23,6 +23,7 @@
   <arg name="tool_tcp_port" default="54321" doc="Port on which the robot controller publishes the tool comm interface. Only used, when `use_tool_communication` is set to true."/>
   <arg name="robot_description_file" doc="Robot description launch file."/>
   <arg name="headless_mode" default="false" doc="Automatically send URScript to robot to execute. On e-Series this does require the robot to be in 'remote-control' mode. With this, the URCap is not needed on the robot."/>
+  <arg name="simulated_robot" default="false" doc="Determine whether the robot to control is simulated or not."/>
   <arg name="ur_hardware_interface_node_required" default="true" doc="Shut down ros environment if ur_hardware_interface-node dies."/>
 
   <!-- robot model -->
@@ -56,6 +57,7 @@
     <arg name="tool_tx_idle_chars" value="$(arg tool_tx_idle_chars)"/>
     <arg name="tool_device_name" value="$(arg tool_device_name)"/>
     <arg name="tool_tcp_port" value="$(arg tool_tcp_port)"/>
-    <arg name="ur_hardware_interface_node_required" value="$(arg ur_hardware_interface_node_required)"/>
+    <arg name="simulated_robot" value="$(arg simulated_robot)"/>
+  <arg name="ur_hardware_interface_node_required" value="$(arg ur_hardware_interface_node_required)"/>
   </include>
 </launch>
diff --git a/ur_robot_driver/launch/ur_control.launch b/ur_robot_driver/launch/ur_control.launch
index 4d842e2e5..a301dc440 100644
--- a/ur_robot_driver/launch/ur_control.launch
+++ b/ur_robot_driver/launch/ur_control.launch
@@ -30,6 +30,7 @@
   <arg name="tool_device_name" default="/tmp/ttyUR" doc="Local device name used for tool communication. Only used, when `use_tool_communication` is set to true."/>
   <arg name="tool_tcp_port" default="54321" doc="Port on which the robot controller publishes the tool comm interface. Only used, when `use_tool_communication` is set to true."/>
   <arg name="headless_mode" default="false" doc="Automatically send URScript to robot to execute. On e-Series this does require the robot to be in 'remote-control' mode. With this, the URCap is not needed on the robot."/>
+  <arg name="simulated_robot" default="false" doc="Determine whether the robot to control is simulated or not."/>
   <arg name="servoj_gain" default="2000" doc="Specify gain for servoing to position in joint space. A higher gain can sharpen the trajectory."/>
   <arg name="servoj_lookahead_time" default="0.03" doc="Specify lookahead time for servoing to position in joint space. A longer lookahead time can smooth the trajectory."/>
   <arg name="ur_hardware_interface_node_required" default="true" doc="Shut down ros environment if ur_hardware_interface-node dies."/>
@@ -56,6 +57,7 @@
     <param name="tool_stop_bits" value="$(arg tool_stop_bits)"/>
     <param name="tool_rx_idle_chars" value="$(arg tool_rx_idle_chars)"/>
     <param name="tool_tx_idle_chars" value="$(arg tool_tx_idle_chars)"/>
+    <param name="simulated_robot" value="$(arg simulated_robot)"/>
     <param name="servoj_gain" value="$(arg servoj_gain)"/>
     <param name="servoj_lookahead_time" value="$(arg servoj_lookahead_time)"/>
   </node>
diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp
index 9d6810768..c2acd4780 100644
--- a/ur_robot_driver/src/hardware_interface.cpp
+++ b/ur_robot_driver/src/hardware_interface.cpp
@@ -277,6 +277,12 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   // endeffector might be inaccurate. See the "ur_calibration" package on help how to generate your
   // own hash matching your actual robot.
   std::string calibration_checksum = robot_hw_nh.param<std::string>("kinematics/hash", "");
+  bool simulated_robot = false;
+  if (!robot_hw_nh.getParam("simulated_robot", simulated_robot))
+  {
+    ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("simulated_robot") << " not given.");
+    return false;
+  }
   ROS_INFO_STREAM("Initializing dashboard client");
   ros::NodeHandle dashboard_nh(robot_hw_nh, "dashboard");
   dashboard_client_.reset(new DashboardClientROS(dashboard_nh, robot_ip_));
@@ -286,7 +292,7 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
     ur_driver_.reset(new urcl::UrDriver(
         robot_ip_, script_filename, output_recipe_filename, input_recipe_filename,
         std::bind(&HardwareInterface::handleRobotProgramState, this, std::placeholders::_1), headless_mode,
-        std::move(tool_comm_setup), (uint32_t)reverse_port, (uint32_t)script_sender_port, servoj_gain,
+        std::move(tool_comm_setup), calibration_checksum, simulated_robot, (uint32_t)reverse_port, (uint32_t)script_sender_port, servoj_gain,
         servoj_lookahead_time, non_blocking_read_, reverse_ip, trajectory_port, script_command_port));
   }
   catch (urcl::ToolCommNotAvailable& e)
@@ -299,20 +305,6 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
     ROS_FATAL_STREAM(e.what());
     return false;
   }
-  URCL_LOG_INFO("Checking if calibration data matches connected robot.");
-  if (ur_driver_->checkCalibration(calibration_checksum))
-  {
-    ROS_INFO_STREAM("Calibration checked successfully.");
-  }
-  else
-  {
-    ROS_ERROR_STREAM("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_->registerTrajectoryDoneCallback(
       std::bind(&HardwareInterface::passthroughTrajectoryDoneCb, this, std::placeholders::_1));
 
diff --git a/ur_robot_driver/test/driver.test b/ur_robot_driver/test/driver.test
index e8ec1f72c..2c6f34ce6 100644
--- a/ur_robot_driver/test/driver.test
+++ b/ur_robot_driver/test/driver.test
@@ -9,6 +9,7 @@
   <include file="$(find ur_robot_driver)/launch/$(arg robot_type)_bringup.launch">
     <arg name="robot_ip" value="$(arg robot_ip)"/>
     <arg name="headless_mode" value="true"/>
+    <arg name="simulated_robot" value="true"/>
     <arg name="stopped_controllers" value="forward_joint_traj_controller forward_cartesian_traj_controller twist_controller pose_based_cartesian_traj_controller joint_based_cartesian_traj_controller"/>
   </include>