From 144ff165b6eaed38923d2765046e3443dcf4c1d9 Mon Sep 17 00:00:00 2001 From: Mads Holm Peters <79145214+urmahp@users.noreply.github.com> Date: Tue, 17 Jan 2023 10:46:21 +0100 Subject: [PATCH] Added services to set tool voltage and zero force torque sensor (#466) Added launch arguments for reverse ip and script command interface port. --- .../ur_controllers/gpio_controller.hpp | 26 +- ur_controllers/src/gpio_controller.cpp | 48 ++ .../ur_robot_driver/hardware_interface.hpp | 3 + ur_robot_driver/launch/ur_control.launch.py | 560 ++++++++++++++++++ ur_robot_driver/src/hardware_interface.cpp | 47 +- 5 files changed, 660 insertions(+), 24 deletions(-) create mode 100644 ur_robot_driver/launch/ur_control.launch.py diff --git a/ur_controllers/include/ur_controllers/gpio_controller.hpp b/ur_controllers/include/ur_controllers/gpio_controller.hpp index fb2b3edab..f422475fc 100644 --- a/ur_controllers/include/ur_controllers/gpio_controller.hpp +++ b/ur_controllers/include/ur_controllers/gpio_controller.hpp @@ -63,16 +63,19 @@ enum CommandInterfaces { DIGITAL_OUTPUTS_CMD = 0u, ANALOG_OUTPUTS_CMD = 18, - IO_ASYNC_SUCCESS = 20, - TARGET_SPEED_FRACTION_CMD = 21, - TARGET_SPEED_FRACTION_ASYNC_SUCCESS = 22, - RESEND_ROBOT_PROGRAM_CMD = 23, - RESEND_ROBOT_PROGRAM_ASYNC_SUCCESS = 24, - PAYLOAD_MASS = 25, - PAYLOAD_COG_X = 26, - PAYLOAD_COG_Y = 27, - PAYLOAD_COG_Z = 28, - PAYLOAD_ASYNC_SUCCESS = 29, + TOOL_VOLTAGE_CMD = 20, + IO_ASYNC_SUCCESS = 21, + TARGET_SPEED_FRACTION_CMD = 22, + TARGET_SPEED_FRACTION_ASYNC_SUCCESS = 23, + RESEND_ROBOT_PROGRAM_CMD = 24, + RESEND_ROBOT_PROGRAM_ASYNC_SUCCESS = 25, + PAYLOAD_MASS = 26, + PAYLOAD_COG_X = 27, + PAYLOAD_COG_Y = 28, + PAYLOAD_COG_Z = 29, + PAYLOAD_ASYNC_SUCCESS = 30, + ZERO_FTSENSOR_CMD = 31, + ZERO_FTSENSOR_ASYNC_SUCCESS = 32, }; enum StateInterfaces @@ -125,6 +128,8 @@ class GPIOController : public controller_interface::ControllerInterface bool setPayload(const ur_msgs::srv::SetPayload::Request::SharedPtr req, ur_msgs::srv::SetPayload::Response::SharedPtr resp); + bool zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr resp); + void publishIO(); void publishToolData(); @@ -150,6 +155,7 @@ class GPIOController : public controller_interface::ControllerInterface rclcpp::Service::SharedPtr set_speed_slider_srv_; rclcpp::Service::SharedPtr set_io_srv_; rclcpp::Service::SharedPtr set_payload_srv_; + rclcpp::Service::SharedPtr tare_sensor_srv_; std::shared_ptr> io_pub_; std::shared_ptr> tool_data_pub_; diff --git a/ur_controllers/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp index 80d656a1f..384584e29 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -61,6 +61,8 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c config.names.emplace_back("gpio/standard_analog_output_cmd_" + std::to_string(i)); } + config.names.emplace_back("gpio/tool_voltage_cmd"); + config.names.emplace_back("gpio/io_async_success"); config.names.emplace_back("speed_scaling/target_speed_fraction_cmd"); @@ -78,6 +80,10 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c config.names.emplace_back("payload/cog.z"); config.names.emplace_back("payload/payload_async_success"); + // zero ft sensor + config.names.emplace_back("zero_ftsensor/zero_ftsensor_cmd"); + config.names.emplace_back("zero_ftsensor/zero_ftsensor_async_success"); + return config; } @@ -274,6 +280,10 @@ ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*pre set_payload_srv_ = get_node()->create_service( "~/set_payload", std::bind(&GPIOController::setPayload, this, std::placeholders::_1, std::placeholders::_2)); + + tare_sensor_srv_ = get_node()->create_service( + "~/zero_ftsensor", + std::bind(&GPIOController::zeroFTSensor, this, std::placeholders::_1, std::placeholders::_2)); } catch (...) { return LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -326,6 +336,19 @@ bool GPIOController::setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs: std::this_thread::sleep_for(std::chrono::milliseconds(50)); } + resp->success = static_cast(command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value()); + return resp->success; + } else if (req->fun == req->FUN_SET_TOOL_VOLTAGE) { + command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].set_value(ASYNC_WAITING); + command_interfaces_[CommandInterfaces::TOOL_VOLTAGE_CMD].set_value(static_cast(req->state)); + + RCLCPP_INFO(get_node()->get_logger(), "Setting tool voltage to: '%1.0f'.", req->state); + + while (command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) { + // Asynchronous wait until the hardware interface has set the io value + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + resp->success = static_cast(command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value()); return resp->success; } else { @@ -413,6 +436,31 @@ bool GPIOController::setPayload(const ur_msgs::srv::SetPayload::Request::SharedP return true; } +bool GPIOController::zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr /*req*/, + std_srvs::srv::Trigger::Response::SharedPtr resp) +{ + // reset success flag + command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].set_value(ASYNC_WAITING); + // call the service in the hardware + command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_CMD].set_value(1.0); + + while (command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) { + // Asynchronous wait until the hardware interface has set the slider value + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + resp->success = static_cast(command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].get_value()); + + if (resp->success) { + RCLCPP_INFO(get_node()->get_logger(), "Successfully zeroed the force torque sensor"); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Could not zero the force torque sensor"); + return false; + } + + return true; +} + void GPIOController::initMsgs() { io_msg_.digital_in_states.resize(standard_digital_output_cmd_.size()); diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index d702217be..f5bae8bf5 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -173,11 +173,14 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface // asynchronous commands std::array standard_dig_out_bits_cmd_; std::array standard_analog_output_cmd_; + double tool_voltage_cmd_; double io_async_success_; double target_speed_fraction_cmd_; double scaling_async_success_; double resend_robot_program_cmd_; double resend_robot_program_async_success_; + double zero_ftsensor_cmd_; + double zero_ftsensor_async_success_; bool first_pass_; bool initialized_; double system_interface_initialized_; diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py new file mode 100644 index 000000000..70f32e09c --- /dev/null +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -0,0 +1,560 @@ +# Copyright (c) 2021 PickNik, Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# +# Author: Denis Stogl + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def launch_setup(context, *args, **kwargs): + + # Initialize Arguments + ur_type = LaunchConfiguration("ur_type") + robot_ip = LaunchConfiguration("robot_ip") + safety_limits = LaunchConfiguration("safety_limits") + safety_pos_margin = LaunchConfiguration("safety_pos_margin") + safety_k_position = LaunchConfiguration("safety_k_position") + # General arguments + runtime_config_package = LaunchConfiguration("runtime_config_package") + controllers_file = LaunchConfiguration("controllers_file") + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + prefix = LaunchConfiguration("prefix") + use_fake_hardware = LaunchConfiguration("use_fake_hardware") + fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") + initial_joint_controller = LaunchConfiguration("initial_joint_controller") + activate_joint_controller = LaunchConfiguration("activate_joint_controller") + launch_rviz = LaunchConfiguration("launch_rviz") + headless_mode = LaunchConfiguration("headless_mode") + launch_dashboard_client = LaunchConfiguration("launch_dashboard_client") + use_tool_communication = LaunchConfiguration("use_tool_communication") + tool_parity = LaunchConfiguration("tool_parity") + tool_baud_rate = LaunchConfiguration("tool_baud_rate") + tool_stop_bits = LaunchConfiguration("tool_stop_bits") + tool_rx_idle_chars = LaunchConfiguration("tool_rx_idle_chars") + tool_tx_idle_chars = LaunchConfiguration("tool_tx_idle_chars") + tool_device_name = LaunchConfiguration("tool_device_name") + tool_tcp_port = LaunchConfiguration("tool_tcp_port") + tool_voltage = LaunchConfiguration("tool_voltage") + reverse_ip = LaunchConfiguration("reverse_ip") + script_command_port = LaunchConfiguration("script_command_port") + + joint_limit_params = PathJoinSubstitution( + [FindPackageShare(description_package), "config", ur_type, "joint_limits.yaml"] + ) + kinematics_params = PathJoinSubstitution( + [FindPackageShare(description_package), "config", ur_type, "default_kinematics.yaml"] + ) + physical_params = PathJoinSubstitution( + [FindPackageShare(description_package), "config", ur_type, "physical_parameters.yaml"] + ) + visual_params = PathJoinSubstitution( + [FindPackageShare(description_package), "config", ur_type, "visual_parameters.yaml"] + ) + script_filename = PathJoinSubstitution( + [FindPackageShare("ur_client_library"), "resources", "external_control.urscript"] + ) + input_recipe_filename = PathJoinSubstitution( + [FindPackageShare("ur_robot_driver"), "resources", "rtde_input_recipe.txt"] + ) + output_recipe_filename = PathJoinSubstitution( + [FindPackageShare("ur_robot_driver"), "resources", "rtde_output_recipe.txt"] + ) + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]), + " ", + "robot_ip:=", + robot_ip, + " ", + "joint_limit_params:=", + joint_limit_params, + " ", + "kinematics_params:=", + kinematics_params, + " ", + "physical_params:=", + physical_params, + " ", + "visual_params:=", + visual_params, + " ", + "safety_limits:=", + safety_limits, + " ", + "safety_pos_margin:=", + safety_pos_margin, + " ", + "safety_k_position:=", + safety_k_position, + " ", + "name:=", + ur_type, + " ", + "script_filename:=", + script_filename, + " ", + "input_recipe_filename:=", + input_recipe_filename, + " ", + "output_recipe_filename:=", + output_recipe_filename, + " ", + "prefix:=", + prefix, + " ", + "use_fake_hardware:=", + use_fake_hardware, + " ", + "fake_sensor_commands:=", + fake_sensor_commands, + " ", + "headless_mode:=", + headless_mode, + " ", + "use_tool_communication:=", + use_tool_communication, + " ", + "tool_parity:=", + tool_parity, + " ", + "tool_baud_rate:=", + tool_baud_rate, + " ", + "tool_stop_bits:=", + tool_stop_bits, + " ", + "tool_rx_idle_chars:=", + tool_rx_idle_chars, + " ", + "tool_tx_idle_chars:=", + tool_tx_idle_chars, + " ", + "tool_device_name:=", + tool_device_name, + " ", + "tool_tcp_port:=", + tool_tcp_port, + " ", + "tool_voltage:=", + tool_voltage, + " ", + "reverse_ip:=", + reverse_ip, + " ", + "script_command_port:=", + script_command_port, + " ", + ] + ) + robot_description = {"robot_description": robot_description_content} + + initial_joint_controllers = PathJoinSubstitution( + [FindPackageShare(runtime_config_package), "config", controllers_file] + ) + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package), "rviz", "view_robot.rviz"] + ) + + # define update rate + update_rate_config_file = PathJoinSubstitution( + [ + FindPackageShare(runtime_config_package), + "config", + ur_type.perform(context) + "_update_rate.yaml", + ] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, update_rate_config_file, initial_joint_controllers], + output="screen", + condition=IfCondition(use_fake_hardware), + ) + + ur_control_node = Node( + package="ur_robot_driver", + executable="ur_ros2_control_node", + parameters=[robot_description, update_rate_config_file, initial_joint_controllers], + output="screen", + condition=UnlessCondition(use_fake_hardware), + ) + + dashboard_client_node = Node( + package="ur_robot_driver", + condition=IfCondition(launch_dashboard_client) and UnlessCondition(use_fake_hardware), + executable="dashboard_client", + name="dashboard_client", + output="screen", + emulate_tty=True, + parameters=[{"robot_ip": robot_ip}], + ) + + tool_communication_node = Node( + package="ur_robot_driver", + condition=IfCondition(use_tool_communication), + executable="tool_communication.py", + name="ur_tool_comm", + output="screen", + parameters=[ + { + "robot_ip": robot_ip, + "tcp_port": tool_tcp_port, + "device_name": tool_device_name, + } + ], + ) + + controller_stopper_node = Node( + package="ur_robot_driver", + executable="controller_stopper_node", + name="controller_stopper", + output="screen", + emulate_tty=True, + condition=UnlessCondition(use_fake_hardware), + parameters=[ + {"headless_mode": headless_mode}, + {"joint_controller_active": activate_joint_controller}, + { + "consistent_controllers": [ + "io_and_status_controller", + "force_torque_sensor_broadcaster", + "joint_state_broadcaster", + "speed_scaling_state_broadcaster", + ] + }, + ], + ) + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + rviz_node = Node( + package="rviz2", + condition=IfCondition(launch_rviz), + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + io_and_status_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["io_and_status_controller", "-c", "/controller_manager"], + ) + + speed_scaling_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "speed_scaling_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], + ) + + force_torque_sensor_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "force_torque_sensor_broadcaster", + "--controller-manager", + "/controller_manager", + ], + ) + + forward_position_controller_spawner_stopped = Node( + package="controller_manager", + executable="spawner", + arguments=["forward_position_controller", "-c", "/controller_manager", "--inactive"], + ) + + # There may be other controllers of the joints, but this is the initially-started one + initial_joint_controller_spawner_started = Node( + package="controller_manager", + executable="spawner", + arguments=[initial_joint_controller, "-c", "/controller_manager"], + condition=IfCondition(activate_joint_controller), + ) + initial_joint_controller_spawner_stopped = Node( + package="controller_manager", + executable="spawner", + arguments=[initial_joint_controller, "-c", "/controller_manager", "--inactive"], + condition=UnlessCondition(activate_joint_controller), + ) + + nodes_to_start = [ + control_node, + ur_control_node, + dashboard_client_node, + tool_communication_node, + controller_stopper_node, + robot_state_publisher_node, + rviz_node, + joint_state_broadcaster_spawner, + io_and_status_controller_spawner, + speed_scaling_state_broadcaster_spawner, + force_torque_sensor_broadcaster_spawner, + forward_position_controller_spawner_stopped, + initial_joint_controller_spawner_stopped, + initial_joint_controller_spawner_started, + ] + + return nodes_to_start + + +def generate_launch_description(): + declared_arguments = [] + # UR specific arguments + declared_arguments.append( + DeclareLaunchArgument( + "ur_type", + description="Type/series of used UR robot.", + choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", description="IP address by which the robot can be reached." + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "safety_limits", + default_value="true", + description="Enables the safety limits controller if true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "safety_pos_margin", + default_value="0.15", + description="The margin to lower and upper limits in the safety controller.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "safety_k_position", + default_value="20", + description="k-position factor in the safety controller.", + ) + ) + # General arguments + declared_arguments.append( + DeclareLaunchArgument( + "runtime_config_package", + default_value="ur_robot_driver", + description='Package with the controller\'s configuration in "config" folder. \ + Usually the argument is not set, it enables use of a custom setup.', + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "controllers_file", + default_value="ur_controllers.yaml", + description="YAML file with the controllers configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="ur_description", + description="Description package with robot URDF/XACRO files. Usually the argument \ + is not set, it enables use of a custom description.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + default_value="ur.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "prefix", + default_value='""', + description="Prefix of the joint names, useful for \ + multi-robot setup. If changed than also joint names in the controllers' configuration \ + have to be updated.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_fake_hardware", + default_value="false", + description="Start robot with fake hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "fake_sensor_commands", + default_value="false", + description="Enable fake command interfaces for sensors used for simple simulations. \ + Used only if 'use_fake_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "headless_mode", + default_value="false", + description="Enable headless mode for robot control", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "initial_joint_controller", + default_value="scaled_joint_trajectory_controller", + description="Initially loaded robot controller.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "activate_joint_controller", + default_value="true", + description="Activate loaded joint controller.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?") + ) + declared_arguments.append( + DeclareLaunchArgument( + "launch_dashboard_client", default_value="true", description="Launch Dashboard Client?" + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_tool_communication", + default_value="false", + description="Only available for e series!", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_parity", + default_value="0", + description="Parity configuration for serial communication. Only effective, if \ + use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_baud_rate", + default_value="115200", + description="Baud rate configuration for serial communication. Only effective, if \ + use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_stop_bits", + default_value="1", + description="Stop bits configuration for serial communication. Only effective, if \ + use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_rx_idle_chars", + default_value="1.5", + description="RX idle chars configuration for serial communication. Only effective, \ + if use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_tx_idle_chars", + default_value="3.5", + description="TX idle chars configuration for serial communication. Only effective, \ + if use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_device_name", + default_value="/tmp/ttyUR", + description="File descriptor that will be generated for the tool communication device. \ + The user has be be allowed to write to this location. \ + Only effective, if use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_tcp_port", + default_value="54321", + description="Remote port that will be used for bridging the tool's serial device. \ + Only effective, if use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_voltage", + default_value="0", # 0 being a conservative value that won't destroy anything + description="Tool voltage that will be setup.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "reverse_ip", + default_value="0.0.0.0", + description="IP that will be used for the robot controller to communicate back to the driver.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "script_command_port", + default_value="50004", + description="Port that will be opened to forward script commands from the driver to the robot", + ) + ) + + return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 5f854f0fd..2861e9efb 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -264,6 +264,14 @@ std::vector URPositionHardwareInterface::e "gpio", "standard_analog_output_cmd_" + std::to_string(i), &standard_analog_output_cmd_[i])); } + command_interfaces.emplace_back(hardware_interface::CommandInterface("gpio", "tool_voltage_cmd", &tool_voltage_cmd_)); + + command_interfaces.emplace_back( + hardware_interface::CommandInterface("zero_ftsensor", "zero_ftsensor_cmd", &zero_ftsensor_cmd_)); + + command_interfaces.emplace_back(hardware_interface::CommandInterface("zero_ftsensor", "zero_ftsensor_async_success", + &zero_ftsensor_async_success_)); + return command_interfaces; } @@ -288,6 +296,16 @@ CallbackReturn URPositionHardwareInterface::on_activate(const rclcpp_lifecycle:: int reverse_port = stoi(info_.hardware_parameters["reverse_port"]); // The driver will offer an interface to receive the program's URScript on this port. int script_sender_port = stoi(info_.hardware_parameters["script_sender_port"]); + // IP that will be used for the robot controller to communicate back to the driver. + std::string reverse_ip = info_.hardware_parameters["reverse_ip"]; + if (reverse_ip == "0.0.0.0") { + reverse_ip = ""; + } + // Port that will be opened to send trajectory points from the driver to the robot. Note: this feature hasn't been + // implemented in ROS2 + int trajectory_port = 50003; + // Port that will be opened to forward script commands from the driver to the robot + int script_command_port = stoi(info_.hardware_parameters["script_command_port"]); // std::string tf_prefix = info_.hardware_parameters["tf_prefix"]; // std::string tf_prefix; @@ -380,7 +398,7 @@ CallbackReturn URPositionHardwareInterface::on_activate(const rclcpp_lifecycle:: 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, - servoj_lookahead_time, non_blocking_read_); + servoj_lookahead_time, non_blocking_read_, reverse_ip, trajectory_port, script_command_port); } catch (urcl::ToolCommNotAvailable& e) { RCLCPP_FATAL_STREAM(rclcpp::get_logger("URPositionHardwareInterface"), "See parameter use_tool_communication"); @@ -538,6 +556,7 @@ hardware_interface::return_type URPositionHardwareInterface::read() urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; target_speed_fraction_cmd_ = NO_NEW_CMD_; resend_robot_program_cmd_ = NO_NEW_CMD_; + zero_ftsensor_cmd_ = NO_NEW_CMD_; initialized_ = true; } @@ -590,6 +609,8 @@ void URPositionHardwareInterface::initAsyncIO() standard_analog_output_cmd_[i] = NO_NEW_CMD_; } + tool_voltage_cmd_ = NO_NEW_CMD_; + payload_mass_ = NO_NEW_CMD_; payload_center_of_gravity_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ }; } @@ -619,6 +640,11 @@ void URPositionHardwareInterface::checkAsyncIO() } } + if (!std::isnan(tool_voltage_cmd_) && ur_driver_ != nullptr) { + io_async_success_ = ur_driver_->setToolVoltage(static_cast(tool_voltage_cmd_)); + tool_voltage_cmd_ = NO_NEW_CMD_; + } + if (!std::isnan(target_speed_fraction_cmd_) && ur_driver_ != nullptr) { scaling_async_success_ = ur_driver_->getRTDEWriter().sendSpeedSlider(target_speed_fraction_cmd_); target_speed_fraction_cmd_ = NO_NEW_CMD_; @@ -636,22 +662,15 @@ void URPositionHardwareInterface::checkAsyncIO() if (!std::isnan(payload_mass_) && !std::isnan(payload_center_of_gravity_[0]) && !std::isnan(payload_center_of_gravity_[1]) && !std::isnan(payload_center_of_gravity_[2]) && ur_driver_ != nullptr) { - try { - // create command as string from interfaces - // ROS1 driver hardware_interface.cpp#L450-L456 - std::stringstream str_command; - str_command.imbue(std::locale::classic()); - str_command << "sec setup():" << std::endl - << " set_payload(" << payload_mass_ << ", [" << payload_center_of_gravity_[0] << ", " - << payload_center_of_gravity_[1] << ", " << payload_center_of_gravity_[2] << "])" << std::endl - << "end"; - payload_async_success_ = ur_driver_->sendScript(str_command.str()); - } catch (const urcl::UrException& e) { - RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Service Call failed: '%s'", e.what()); - } + payload_async_success_ = ur_driver_->setPayload(payload_mass_, payload_center_of_gravity_); payload_mass_ = NO_NEW_CMD_; payload_center_of_gravity_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ }; } + + if (!std::isnan(zero_ftsensor_cmd_) && ur_driver_ != nullptr) { + zero_ftsensor_async_success_ = ur_driver_->zeroFTSensor(); + zero_ftsensor_cmd_ = NO_NEW_CMD_; + } } void URPositionHardwareInterface::updateNonDoubleValues()