diff --git a/ur_bringup/launch/ur_control.launch.py b/ur_bringup/launch/ur_control.launch.py index e2cccc14f..7f672aaf2 100644 --- a/ur_bringup/launch/ur_control.launch.py +++ b/ur_bringup/launch/ur_control.launch.py @@ -58,6 +58,7 @@ def launch_setup(context, *args, **kwargs): activate_joint_controller = LaunchConfiguration("activate_joint_controller") launch_rviz = LaunchConfiguration("launch_rviz") headless_mode = LaunchConfiguration("headless_mode") + simulated_robot = LaunchConfiguration("simulated_robot") launch_dashboard_client = LaunchConfiguration("launch_dashboard_client") use_tool_communication = LaunchConfiguration("use_tool_communication") tool_parity = LaunchConfiguration("tool_parity") @@ -145,6 +146,9 @@ def launch_setup(context, *args, **kwargs): "headless_mode:=", headless_mode, " ", + "simulated_robot:=", + simulated_robot, + " ", "use_tool_communication:=", use_tool_communication, " ", @@ -449,6 +453,13 @@ def generate_launch_description(): description="Enable headless mode for robot control", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "simulated_robot", + default_value="false", + description="Determine whether robot is simulated or real", + ) + ) declared_arguments.append( DeclareLaunchArgument( "initial_joint_controller", diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index a0db12cb5..47105f0b5 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -58,6 +58,7 @@ def launch_setup(context, *args, **kwargs): activate_joint_controller = LaunchConfiguration("activate_joint_controller") launch_rviz = LaunchConfiguration("launch_rviz") headless_mode = LaunchConfiguration("headless_mode") + simulated_robot = LaunchConfiguration("simulated_robot") launch_dashboard_client = LaunchConfiguration("launch_dashboard_client") use_tool_communication = LaunchConfiguration("use_tool_communication") tool_parity = LaunchConfiguration("tool_parity") @@ -145,6 +146,9 @@ def launch_setup(context, *args, **kwargs): "headless_mode:=", headless_mode, " ", + "simulated_robot:=", + simulated_robot, + " ", "use_tool_communication:=", use_tool_communication, " ", @@ -441,6 +445,13 @@ def generate_launch_description(): description="Enable headless mode for robot control", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "simulated_robot", + default_value="false", + description="Determine whether the robot is simulated or real", + ) + ) declared_arguments.append( DeclareLaunchArgument( "initial_joint_controller", diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index e2df89158..02518f013 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -377,10 +377,13 @@ URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Initializing driver..."); registerUrclLogHandler(); try { + bool simulated_robot = false; + simulated_robot = (info_.hardware_parameters["simulated_robot"] == "true") || + (info_.hardware_parameters["simulated_robot"] == "True"); ur_driver_ = std::make_unique( 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), (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_); } catch (urcl::ToolCommNotAvailable& e) { RCLCPP_FATAL_STREAM(rclcpp::get_logger("URPositionHardwareInterface"), "See parameter use_tool_communication"); @@ -394,18 +397,6 @@ URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous 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_ROS2_Driver/blob/main/ur_calibration/" - "README.md] for details."); - } ur_driver_->startRTDECommunication(); diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index ea5c2b62f..c8c983c2a 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -96,6 +96,7 @@ def generate_test_description(): "launch_rviz": "false", "initial_joint_controller": "scaled_joint_trajectory_controller", "headless_mode": "true", + "simulated_robot": "true", "launch_dashboard_client": "false", "start_joint_controller": "false", }.items(),