Unverified Commit 8cd1c9b2 authored by Tom Queen's avatar Tom Queen Committed by GitHub
Browse files

Merge pull request #1 from UniversalRobots/master

update our master
parents fad8c0b3 c9b25b5a
......@@ -2,7 +2,7 @@
<package format="2">
<name>ur_dashboard_msgs</name>
<version>0.0.0</version>
<description>Messages around the UR Dashbaord server.</description>
<description>Messages around the UR Dashboard server.</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
......
......@@ -32,6 +32,10 @@ Standalone launchfile to startup a ur3e. This requires a robot reachable via a n
Use the description in limited mode (Every axis rotates from -PI to PI)
* "**reverse_port**" (default: "50001")
Port that will be opened by the driver to allow direct communication between the driver and the robot controller.
* "**robot_description_file**" (default: "$(find ur_e_description)/launch/ur3e_upload.launch")
Robot description launch file.
......@@ -40,6 +44,10 @@ Standalone launchfile to startup a ur3e. This requires a robot reachable via a n
IP address by which the robot can be reached.
* "**script_sender_port**" (default: "50002")
The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately.
* "**stopped_controllers**" (default: "pos_traj_controller")
Controllers that are initally loaded, but not started.
......@@ -113,6 +121,10 @@ Standalone launchfile to startup a ur10 robot. This requires a robot reachable v
Use the description in limited mode (Every axis rotates from -PI to PI)
* "**reverse_port**" (default: "50001")
Port that will be opened by the driver to allow direct communication between the driver and the robot controller.
* "**robot_description_file**" (default: "$(find ur_description)/launch/ur10_upload.launch")
Robot description launch file.
......@@ -121,6 +133,10 @@ Standalone launchfile to startup a ur10 robot. This requires a robot reachable v
IP address by which the robot can be reached.
* "**script_sender_port**" (default: "50002")
The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately.
* "**stopped_controllers**" (default: "pos_traj_controller")
Controllers that are initally loaded, but not started.
......@@ -158,6 +174,10 @@ Robot bringup launchfile without the robot description. Include this, if you wan
Please add description. See file "launch/ur_control.launch".
* "**reverse_port**" (default: "50001")
Port that will be opened by the driver to allow direct communication between the driver and the robot controller.
* "**robot_ip**" (Required)
IP address by which the robot can be reached.
......@@ -170,6 +190,10 @@ Robot bringup launchfile without the robot description. Include this, if you wan
Recipe file used for the RTDE-outputs. Only change this if you know what you're doing.
* "**script_sender_port**" (default: "50002")
The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately.
* "**stopped_controllers**" (default: "pos_traj_controller")
Controllers that are initally loaded, but not started.
......@@ -247,6 +271,10 @@ Launchfile that starts a robot description with robot_state publisher and the dr
Use the description in limited mode (Every axis rotates from -PI to PI)
* "**reverse_port**" (default: "50001")
Port that will be opened by the driver to allow direct communication between the driver and the robot controller.
* "**robot_description_file**" (Required)
Robot description launch file.
......@@ -255,6 +283,10 @@ Launchfile that starts a robot description with robot_state publisher and the dr
IP address by which the robot can be reached.
* "**script_sender_port**" (default: "50002")
The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately.
* "**stopped_controllers**" (default: "pos_traj_controller")
Controllers that are initally loaded, but not started.
......@@ -328,6 +360,10 @@ Standalone launchfile to startup a ur5 robot. This requires a robot reachable vi
Use the description in limited mode (Every axis rotates from -PI to PI)
* "**reverse_port**" (default: "50001")
Port that will be opened by the driver to allow direct communication between the driver and the robot controller.
* "**robot_description_file**" (default: "$(find ur_description)/launch/ur5_upload.launch")
Robot description launch file.
......@@ -336,6 +372,10 @@ Standalone launchfile to startup a ur5 robot. This requires a robot reachable vi
IP address by which the robot can be reached.
* "**script_sender_port**" (default: "50002")
The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately.
* "**stopped_controllers**" (default: "pos_traj_controller")
Controllers that are initally loaded, but not started.
......@@ -373,6 +413,10 @@ Standalone launchfile to startup a ur5e robot. This requires a robot reachable v
Use the description in limited mode (Every axis rotates from -PI to PI)
* "**reverse_port**" (default: "50001")
Port that will be opened by the driver to allow direct communication between the driver and the robot controller.
* "**robot_description_file**" (default: "$(find ur_e_description)/launch/ur5e_upload.launch")
Robot description launch file.
......@@ -381,6 +425,10 @@ Standalone launchfile to startup a ur5e robot. This requires a robot reachable v
IP address by which the robot can be reached.
* "**script_sender_port**" (default: "50002")
The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately.
* "**stopped_controllers**" (default: "pos_traj_controller")
Controllers that are initally loaded, but not started.
......@@ -454,6 +502,10 @@ Standalone launchfile to startup a ur5 robot. This requires a robot reachable vi
Use the description in limited mode (Every axis rotates from -PI to PI)
* "**reverse_port**" (default: "50001")
Port that will be opened by the driver to allow direct communication between the driver and the robot controller.
* "**robot_description_file**" (default: "$(find ur_description)/launch/ur3_upload.launch")
Robot description launch file.
......@@ -462,6 +514,10 @@ Standalone launchfile to startup a ur5 robot. This requires a robot reachable vi
IP address by which the robot can be reached.
* "**script_sender_port**" (default: "50002")
The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately.
* "**stopped_controllers**" (default: "pos_traj_controller")
Controllers that are initally loaded, but not started.
......@@ -499,6 +555,10 @@ Standalone launchfile to startup a ur10e robot. This requires a robot reachable
Use the description in limited mode (Every axis rotates from -PI to PI)
* "**reverse_port**" (default: "50001")
Port that will be opened by the driver to allow direct communication between the driver and the robot controller.
* "**robot_description_file**" (default: "$(find ur_e_description)/launch/ur10e_upload.launch")
Robot description launch file.
......@@ -507,6 +567,10 @@ Standalone launchfile to startup a ur10e robot. This requires a robot reachable
IP address by which the robot can be reached.
* "**script_sender_port**" (default: "50002")
The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately.
* "**stopped_controllers**" (default: "pos_traj_controller")
Controllers that are initally loaded, but not started.
......@@ -673,6 +737,10 @@ This is the actual driver node containing the ROS-Control stack. Interfaces docu
Set the speed slider fraction used by the robot's execution. Values should be between 0 and 1. Only set this smaller than 1 if you are using the scaled controllers (as by default) or you know what you're doing. Using this with other controllers might lead to unexpected behaviors.
* "**zero_ftsensor**" ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html))
Calling this service will zero the robot's ftsensor. Note: On e-Series robots this will only work when the robot is in remote-control mode.
#### Parameters
* "**dashboard/receive_timeout**" (Required)
......@@ -698,6 +766,10 @@ This is the actual driver node containing the ROS-Control stack. Interfaces docu
Path to the file containing the recipe used for requesting RTDE outputs.
* "**reverse_port**" (Required)
Port that will be opened to communicate between the driver and the robot controller.
* "**robot_ip**" (Required)
The robot's IP address.
......@@ -706,9 +778,13 @@ This is the actual driver node containing the ROS-Control stack. Interfaces docu
Path to the urscript code that will be sent to the robot.
* "**script_sender_port**" (Required)
The driver will offer an interface to receive the program's URScript on this port.
* "**tf_prefix**" (default: "")
Please add description. See hardware_interface.cpp line number: 68
Please add description. See hardware_interface.cpp line number: 74
......
......@@ -190,12 +190,14 @@ protected:
bool setSpeedSlider(ur_msgs::SetSpeedSliderFractionRequest& req, ur_msgs::SetSpeedSliderFractionResponse& res);
bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& res);
bool resendRobotProgram(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
bool zeroFTSensor(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
void commandCallback(const std_msgs::StringConstPtr& msg);
std::unique_ptr<UrDriver> ur_driver_;
std::unique_ptr<DashboardClientROS> dashboard_client_;
ros::ServiceServer deactivate_srv_;
ros::ServiceServer tare_sensor_srv_;
hardware_interface::JointStateInterface js_interface_;
ur_controllers::ScaledPositionJointInterface spj_interface_;
......
......@@ -31,6 +31,7 @@
#include "ur_robot_driver/comm/reverse_interface.h"
#include "ur_robot_driver/comm/script_sender.h"
#include "ur_robot_driver/ur/tool_communication.h"
#include "ur_robot_driver/ur/version_information.h"
#include "ur_robot_driver/primary/robot_message/version_message.h"
#include "ur_robot_driver/rtde/rtde_writer.h"
......@@ -57,10 +58,15 @@ public:
* \param tool_comm_setup Configuration for using the tool communication.
* \param calibration_checksum Expected checksum of calibration. Will be matched against the
* calibration reported by the robot.
* \param reverse_port Port that will be opened by the driver to allow direct communication between the driver
* and the robot controller.
* \param script_sending_port The driver will offer an interface to receive the program's URScript on this port. If
* the robot cannot connect to this port, `External Control` will stop immediately.
*/
UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file,
const std::string& input_recipe_file, std::function<void(bool)> handle_program_state, bool headless_mode,
std::unique_ptr<ToolCommSetup> tool_comm_setup, const std::string& calibration_checksum = "");
std::unique_ptr<ToolCommSetup> tool_comm_setup, const std::string& calibration_checksum = "",
const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002);
/*!
* \brief Constructs a new UrDriver object.
*
......@@ -72,12 +78,17 @@ public:
* \param headless_mode Parameter to control if the driver should be started in headless mode.
* \param calibration_checksum Expected checksum of calibration. Will be matched against the
* calibration reported by the robot.
* \param reverse_port Port that will be opened by the driver to allow direct communication between the driver
* and the robot controller
* \param script_sending_port The driver will offer an interface to receive the program's URScript on this port.
* If the robot cannot connect to this port, `External Control` will stop immediately.
*/
UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file,
const std::string& input_recipe_file, std::function<void(bool)> handle_program_state, bool headless_mode,
const std::string& calibration_checksum = "")
const std::string& calibration_checksum = "", const uint32_t reverse_port = 50001,
const uint32_t script_sender_port = 50002)
: UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode,
std::unique_ptr<ToolCommSetup>{}, calibration_checksum)
std::unique_ptr<ToolCommSetup>{}, calibration_checksum, reverse_port, script_sender_port)
{
}
......@@ -173,6 +184,14 @@ public:
*/
bool sendRobotProgram();
/*!
* \brief Returns version information about the currently connected robot
*/
const VersionInformation& getVersion()
{
return robot_version_;
}
private:
std::string readScriptFile(const std::string& filename);
std::string readKeepalive();
......@@ -197,6 +216,8 @@ private:
std::string robot_ip_;
bool in_headless_mode_;
std::string full_robot_program_;
VersionInformation robot_version_;
};
} // namespace ur_driver
#endif // ifndef UR_RTDE_DRIVER_UR_UR_DRIVER_H_INCLUDED
......@@ -44,6 +44,12 @@ struct VersionInformation
bugfix = 0;
build = 0;
}
friend std::ostream& operator<<(std::ostream& os, const VersionInformation& version_info)
{
os << version_info.major << "." << version_info.minor << "." << version_info.bugfix << "-" << version_info.build;
return os;
}
uint32_t major; ///< Major version number
uint32_t minor; ///< Minor version number
uint32_t bugfix; ///< Bugfix version number
......
......@@ -2,6 +2,8 @@
<launch>
<arg name="debug" default="false" doc="Debug flag that will get passed on to ur_common.launch"/>
<arg name="robot_ip" doc="IP address by which the robot can be reached."/>
<arg name="reverse_port" default="50001" doc="Port that will be opened by the driver to allow direct communication between the driver and the robot controller."/>
<arg name="script_sender_port" default="50002" doc="The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately."/>
<arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
<arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/>
<arg name="stopped_controllers" default="pos_traj_controller" doc="Controllers that are initally loaded, but not started."/>
......@@ -17,6 +19,8 @@
<arg name="controller_config_file" value="$(arg controller_config_file)"/>
<arg name="robot_description_file" value="$(arg robot_description_file)"/>
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="reverse_port" value="$(arg reverse_port)"/>
<arg name="script_sender_port" value="$(arg script_sender_port)"/>
<arg name="kinematics_config" value="$(arg kinematics_config)"/>
<arg name="limited" value="$(arg limited)"/>
<arg name="tf_prefix" value="$(arg tf_prefix)"/>
......
......@@ -3,6 +3,8 @@
<arg name="debug" default="false" doc="Debug flag that will get passed on to ur_common.launch"/>
<arg name="robot_ip" doc="IP address by which the robot can be reached."/>
<arg name="reverse_port" default="50001" doc="Port that will be opened by the driver to allow direct communication between the driver and the robot controller."/>
<arg name="script_sender_port" default="50002" doc="The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately."/>
<arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
<arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/>
<arg name="stopped_controllers" default="pos_traj_controller" doc="Controllers that are initally loaded, but not started."/>
......@@ -28,6 +30,8 @@
<arg name="robot_description_file" value="$(arg robot_description_file)"/>
<arg name="kinematics_config" value="$(arg kinematics_config)"/>
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="reverse_port" value="$(arg reverse_port)"/>
<arg name="script_sender_port" value="$(arg script_sender_port)"/>
<arg name="limited" value="$(arg limited)"/>
<arg name="tf_prefix" value="$(arg tf_prefix)"/>
<arg name="controllers" value="$(arg controllers)"/>
......
......@@ -2,6 +2,8 @@
<launch>
<arg name="debug" default="false" doc="Debug flag that will get passed on to ur_common.launch"/>
<arg name="robot_ip" doc="IP address by which the robot can be reached."/>
<arg name="reverse_port" default="50001" doc="Port that will be opened by the driver to allow direct communication between the driver and the robot controller."/>
<arg name="script_sender_port" default="50002" doc="The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately."/>
<arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
<arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/>
<arg name="stopped_controllers" default="pos_traj_controller" doc="Controllers that are initally loaded, but not started."/>
......@@ -17,6 +19,8 @@
<arg name="controller_config_file" value="$(arg controller_config_file)"/>
<arg name="robot_description_file" value="$(arg robot_description_file)"/>
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="reverse_port" value="$(arg reverse_port)"/>
<arg name="script_sender_port" value="$(arg script_sender_port)"/>
<arg name="kinematics_config" value="$(arg kinematics_config)"/>
<arg name="limited" value="$(arg limited)"/>
<arg name="tf_prefix" value="$(arg tf_prefix)"/>
......
......@@ -2,6 +2,8 @@
<launch>
<arg name="debug" default="false" doc="Debug flag that will get passed on to ur_common.launch"/>
<arg name="robot_ip" doc="IP address by which the robot can be reached."/>
<arg name="reverse_port" default="50001" doc="Port that will be opened by the driver to allow direct communication between the driver and the robot controller."/>
<arg name="script_sender_port" default="50002" doc="The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately."/>
<arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
<arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/>
<arg name="stopped_controllers" default="pos_traj_controller" doc="Controllers that are initally loaded, but not started."/>
......@@ -27,6 +29,8 @@
<arg name="robot_description_file" value="$(arg robot_description_file)"/>
<arg name="kinematics_config" value="$(arg kinematics_config)"/>
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="reverse_port" value="$(arg reverse_port)"/>
<arg name="script_sender_port" value="$(arg script_sender_port)"/>
<arg name="limited" value="$(arg limited)"/>
<arg name="tf_prefix" value="$(arg tf_prefix)"/>
<arg name="controllers" value="$(arg controllers)"/>
......
......@@ -2,6 +2,8 @@
<launch>
<arg name="debug" default="false" doc="Debug flag that will get passed on to ur_common.launch"/>
<arg name="robot_ip" doc="IP address by which the robot can be reached."/>
<arg name="reverse_port" default="50001" doc="Port that will be opened by the driver to allow direct communication between the driver and the robot controller."/>
<arg name="script_sender_port" default="50002" doc="The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately."/>
<arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
<arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/>
<arg name="stopped_controllers" default="pos_traj_controller" doc="Controllers that are initally loaded, but not started."/>
......@@ -17,6 +19,8 @@
<arg name="controller_config_file" value="$(arg controller_config_file)"/>
<arg name="robot_description_file" value="$(arg robot_description_file)"/>
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="reverse_port" value="$(arg reverse_port)"/>
<arg name="script_sender_port" value="$(arg script_sender_port)"/>
<arg name="kinematics_config" value="$(arg kinematics_config)"/>
<arg name="limited" value="$(arg limited)"/>
<arg name="tf_prefix" value="$(arg tf_prefix)"/>
......
......@@ -2,6 +2,8 @@
<launch>
<arg name="debug" default="false" doc="Debug flag that will get passed on to ur_common.launch"/>
<arg name="robot_ip" doc="IP address by which the robot can be reached."/>
<arg name="reverse_port" default="50001" doc="Port that will be opened by the driver to allow direct communication between the driver and the robot controller."/>
<arg name="script_sender_port" default="50002" doc="The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately."/>
<arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
<arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/>
<arg name="stopped_controllers" default="pos_traj_controller" doc="Controllers that are initally loaded, but not started."/>
......@@ -27,6 +29,8 @@
<arg name="robot_description_file" value="$(arg robot_description_file)"/>
<arg name="kinematics_config" value="$(arg kinematics_config)"/>
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="reverse_port" value="$(arg reverse_port)"/>
<arg name="script_sender_port" value="$(arg script_sender_port)"/>
<arg name="limited" value="$(arg limited)"/>
<arg name="tf_prefix" value="$(arg tf_prefix)"/>
<arg name="controllers" value="$(arg controllers)"/>
......
......@@ -4,6 +4,8 @@
<arg name="use_tool_communication" doc="On e-Series robots tool communication can be enabled with this argument"/>
<arg name="controller_config_file" doc="Config file used for defining the ROS-Control controllers."/>
<arg name="robot_ip" doc="IP address by which the robot can be reached."/>
<arg name="reverse_port" default="50001" doc="Port that will be opened by the driver to allow direct communication between the driver and the robot controller."/>
<arg name="script_sender_port" default="50002" doc="The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately."/>
<arg name="kinematics_config" 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="tf_prefix" default="" doc="tf_prefix used for the robot."/>
<arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/>
......@@ -34,6 +36,8 @@
<arg name="use_tool_communication" value="$(arg use_tool_communication)"/>
<arg name="controller_config_file" value="$(arg controller_config_file)"/>
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="reverse_port" value="$(arg reverse_port)"/>
<arg name="script_sender_port" value="$(arg script_sender_port)"/>
<arg name="kinematics_config" value="$(arg kinematics_config)"/>
<arg name="tf_prefix" value="$(arg tf_prefix)"/>
<arg name="controllers" value="$(arg controllers)"/>
......
......@@ -9,6 +9,8 @@
<arg name="use_tool_communication" doc="On e-Series robots tool communication can be enabled with this argument"/>
<arg name="controller_config_file" doc="Config file used for defining the ROS-Control controllers."/>
<arg name="robot_ip" doc="IP address by which the robot can be reached."/>
<arg name="reverse_port" default="50001" doc="Port that will be opened by the driver to allow direct communication between the driver and the robot controller."/>
<arg name="script_sender_port" default="50002" doc="The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately."/>
<arg name="kinematics_config" doc="Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description. Pass the same config file that is passed to the robot_description."/>
<arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
<arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/>
......@@ -29,6 +31,8 @@
<!-- Load hardware interface -->
<node name="ur_hardware_interface" pkg="ur_robot_driver" type="ur_robot_driver_node" output="screen" launch-prefix="$(arg launch_prefix)" required="true">
<param name="robot_ip" type="str" value="$(arg robot_ip)"/>
<param name="reverse_port" type="int" value="$(arg reverse_port)"/>
<param name="script_sender_port" type="int" value="$(arg script_sender_port)"/>
<rosparam command="load" file="$(arg kinematics_config)" />
<param name="script_file" value="$(arg urscript_file)"/>
<param name="output_recipe_file" value="$(arg rtde_output_recipe_file)"/>
......@@ -48,6 +52,8 @@
<!-- Starts socat to bridge the robot's tool communication interface to a local tty device -->
<node if="$(arg use_tool_communication)" name="ur_tool_communication_bridge" pkg="ur_robot_driver" type="tool_communication" respawn="false" output="screen">
<param name="robot_ip" value="$(arg robot_ip)"/>
<param name="reverse_port" type="int" value="$(arg reverse_port)"/>
<param name="script_sender_port" type="int" value="$(arg script_sender_port)"/>
<param name="device_name" value="$(arg tool_device_name)"/>
<param name="tcp_port" value="$(arg tool_tcp_port)"/>
</node>
......
......@@ -15,8 +15,8 @@
<license>Zlib</license>
<url type="website">http://wiki.ros.org/ur_robot_driver</url>
<url type="bugtracker">https://github.com/ros-industrial/ur_robot_driver/issues</url>
<url type="repository">https://github.com/ros-industrial/ur_robot_driver</url>
<url type="bugtracker">https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/issues</url>
<url type="repository">https://github.com/UniversalRobots/Universal_Robots_ROS_Driver</url>
<buildtool_depend>catkin</buildtool_depend>
......
......@@ -35,7 +35,6 @@ DashboardClientROS::DashboardClientROS(const ros::NodeHandle& nh, const std::str
connect();
// Service to release the brakes. If the robot is currently powered off, it will get powered on on the fly.
// brake_release_service_ = create_dashboard_trigger_srv("brake_release", "brake release\n", "Brake releasing");
brake_release_service_ = create_dashboard_trigger_srv("brake_release", "brake release\n", "Brake releasing");
// If this service is called the operational mode can again be changed from PolyScope, and the user password is
......@@ -130,7 +129,7 @@ DashboardClientROS::DashboardClientROS(const ros::NodeHandle& nh, const std::str
});
// Service to query the current program state
popup_service_ =
program_state_service_ =
nh_.advertiseService<ur_dashboard_msgs::GetProgramState::Request, ur_dashboard_msgs::GetProgramState::Response>(
"program_state",
[&](ur_dashboard_msgs::GetProgramState::Request& req, ur_dashboard_msgs::GetProgramState::Response& resp) {
......@@ -163,7 +162,7 @@ DashboardClientROS::DashboardClientROS(const ros::NodeHandle& nh, const std::str
});
// General purpose service to send arbitrary messages to the dashboard server
running_service_ =
raw_request_service_ =
nh_.advertiseService<ur_dashboard_msgs::RawRequest::Request, ur_dashboard_msgs::RawRequest::Response>(
"raw_request",
[&](ur_dashboard_msgs::RawRequest::Request& req, ur_dashboard_msgs::RawRequest::Response& resp) {
......
......@@ -65,6 +65,12 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
return false;
}
// Port that will be opened to communicate between the driver and the robot controller.
int reverse_port = robot_hw_nh.param("reverse_port", 50001);
// The driver will offer an interface to receive the program's URScript on this port.
int script_sender_port = robot_hw_nh.param("script_sender_port", 50002);
robot_hw_nh.param<std::string>("tf_prefix", tf_prefix_, "");
// Path to the urscript code that will be sent to the robot.
......@@ -207,7 +213,8 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
{
ur_driver_.reset(new 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), calibration_checksum));
headless_mode, std::move(tool_comm_setup), calibration_checksum,
(uint32_t)reverse_port, (uint32_t)script_sender_port));
}
catch (ur_driver::ToolCommNotAvailable& e)
{
......@@ -315,6 +322,10 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
// Calling this service will make the "External Control" program node on the UR-Program return.
deactivate_srv_ = robot_hw_nh.advertiseService("hand_back_control", &HardwareInterface::stopControl, this);
// Calling this service will zero the robot's ftsensor. Note: On e-Series robots this will only
// work when the robot is in remote-control mode.
tare_sensor_srv_ = robot_hw_nh.advertiseService("zero_ftsensor", &HardwareInterface::zeroFTSensor, this);
ur_driver_->startRTDECommunication();
ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_robot_driver hardware_interface");
......@@ -694,6 +705,28 @@ bool HardwareInterface::resendRobotProgram(std_srvs::TriggerRequest& req, std_sr
return true;
}
bool HardwareInterface::zeroFTSensor(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res)
{
if (ur_driver_->getVersion().major < 5)
{
std::stringstream ss;
ss << "Zeroing the Force-Torque sensor is only available for e-Series robots (Major version >= 5). This robot's "
"version is "
<< ur_driver_->getVersion();
ROS_ERROR_STREAM(ss.str());
res.message = ss.str();
res.success = false;
}
else
{
res.success = this->ur_driver_->sendScript(R"(sec tareSensor():
zero_ftsensor()
end
)");
}
return true;
}
void HardwareInterface::commandCallback(const std_msgs::StringConstPtr& msg)
{
std::string str = msg->data;
......@@ -745,4 +778,4 @@ void HardwareInterface::publishRobotAndSafetyMode()
}
} // namespace ur_driver
PLUGINLIB_EXPORT_CLASS(ur_driver::HardwareInterface, hardware_interface::RobotHW)
\ No newline at end of file
PLUGINLIB_EXPORT_CLASS(ur_driver::HardwareInterface, hardware_interface::RobotHW)
......@@ -58,7 +58,7 @@ std::unordered_map<std::string, DataPackage::_rtde_type_variant> DataPackage::g_
{ "speed_scaling", double() },
{ "target_speed_fraction", double() },
{ "actual_momentum", double() },
{ "actial_main_voltage", double() },
{ "actual_main_voltage", double() },
{ "actual_robot_voltage", double() },
{ "actual_robot_current", double() },
{ "actual_joint_voltage", vector6d_t() },
......
......@@ -50,11 +50,13 @@ 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& output_recipe_file, const std::string& input_recipe_file,
std::function<void(bool)> handle_program_state, bool headless_mode,
std::unique_ptr<ToolCommSetup> tool_comm_setup, const std::string& calibration_checksum)
std::unique_ptr<ToolCommSetup> tool_comm_setup, const std::string& calibration_checksum,
const uint32_t reverse_port, const uint32_t script_sender_port)
: servoj_time_(0.008)
, servoj_gain_(2000)
, servoj_lookahead_time_(0.03)
, reverse_interface_active_(false)
, reverse_port_(reverse_port)
, handle_program_state_(handle_program_state)
, robot_ip_(robot_ip)
{
......@@ -80,9 +82,6 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
std::string local_ip = rtde_client_->getIP();
uint32_t reverse_port = 50001; // TODO: Make this a parameter
uint32_t script_sender_port = 50002; // TODO: Make this a parameter