Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-15 12:30:47 +02:00
Code Releases Activity

Renamed the driver to ur_robot_driver

Browse Source
This commit is contained in:
Felix Mauch
2019-10-02 13:58:56 +02:00
parent 5138ecab82
commit 745b2c5fb7
117 changed files with 208 additions and 208 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
ur_robot_driver/CHANGELOG.rst Normal file
View File

@@ -0,0 +1,22 @@
0.0.3 (2019-08-09)
------------------
* Added a service to end ROS control from ROS side
* Publish IO state on ROS topics
* Added write channel through RTDE with speed slider and IO services
* Added subscriber to send arbitrary URScript commands to the robot
0.0.2 (2019-07-03)
------------------
* Fixed dependencies and installation
* Updated README
* Fixed passing parameters through launch files
* Added support for correctly switching controllers during runtime and using the standard
joint_trajectory_controller
* Updated externalcontrol URCap to version 1.0.2
+ Fixed Script timeout when running the URCap inside of a looping tree
+ Fixed a couple of typos
* Increased minimal required UR software version to 3.7/5.1
0.0.1 (2019-06-28)
------------------
Initial release

133
ur_robot_driver/CMakeLists.txt Normal file
View File

@@ -0,0 +1,133 @@
cmake_minimum_required(VERSION 2.8.12)
project(ur_robot_driver)
add_definitions( -DROS_BUILD )
if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
message("${PROJECT_NAME}: You did not request a specific build type: selecting 'RelWithDebInfo'.")
set(CMAKE_BUILD_TYPE RelWithDebInfo)
endif()
find_package(catkin REQUIRED
COMPONENTS
actionlib
control_msgs
controller_manager
geometry_msgs
hardware_interface
industrial_msgs
roscpp
sensor_msgs
std_srvs
tf
tf2_geometry_msgs
tf2_msgs
trajectory_msgs
ur_controllers
ur_msgs
)
find_package(Boost REQUIRED)
catkin_package(
INCLUDE_DIRS
include
LIBRARIES
ur_robot_driver
CATKIN_DEPENDS
actionlib
control_msgs
controller_manager
geometry_msgs
hardware_interface
industrial_msgs
roscpp
sensor_msgs
tf
tf2_geometry_msgs
tf2_msgs
trajectory_msgs
ur_controllers
ur_msgs
std_srvs
DEPENDS
Boost
)
# check c++11 / c++0x
include(CheckCXXCompilerFlag)
check_cxx_compiler_flag("-std=c++11" COMPILER_SUPPORTS_CXX11)
check_cxx_compiler_flag("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
add_compile_options(-std=c++11)
elseif(COMPILER_SUPPORTS_CXX0X)
add_compile_options(-std=c++0x)
else()
message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler. Suggested solution: update the pkg build-essential ")
endif()
add_compile_options(-Wall)
add_compile_options(-Wextra)
add_compile_options(-Wno-unused-parameter)
include_directories(
include
${catkin_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
)
add_library(ur_robot_driver
src/comm/tcp_socket.cpp
src/comm/server.cpp
#src/ros/service_stopper.cpp
#src/ur/commander.cpp
#src/ur/master_board.cpp
#src/ur/messages.cpp
#src/ur/robot_mode.cpp
src/primary/primary_package.cpp
src/primary/robot_message.cpp
src/primary/robot_message/version_message.cpp
src/primary/robot_state/kinematics_info.cpp
src/rtde/control_package_pause.cpp
src/rtde/control_package_setup_inputs.cpp
src/rtde/control_package_setup_outputs.cpp
src/rtde/control_package_start.cpp
src/rtde/data_package.cpp
src/rtde/get_urcontrol_version.cpp
src/rtde/request_protocol_version.cpp
src/rtde/rtde_package.cpp
src/rtde/text_message.cpp
src/rtde/rtde_client.cpp
src/ur/ur_driver.cpp
src/ur/calibration_checker.cpp
src/ur/tool_communication.cpp
src/rtde/rtde_writer.cpp
)
target_link_libraries(ur_robot_driver ${catkin_LIBRARIES})
add_dependencies(ur_robot_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_executable(ur_robot_driver_node
src/ros/hardware_interface.cpp
src/ros/hardware_interface_node.cpp
)
target_link_libraries(ur_robot_driver_node ${catkin_LIBRARIES} ur_robot_driver)
add_dependencies(ur_robot_driver_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
install(TARGETS ur_robot_driver ur_robot_driver_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(PROGRAMS scripts/tool_communication
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
install(DIRECTORY config launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
)

180
ur_robot_driver/LICENSE Normal file
View File

@@ -0,0 +1,180 @@
Copyright 2019 FZI Forschungszentrum Informatik
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS

56
ur_robot_driver/README.md Normal file
View File

@@ -0,0 +1,56 @@
# ur_robot_driver
This package contains the actual driver for UR robots. It is part of the *universal_robots_driver*
repository and requires other packages from that repository. Also, see the [main repository's
README](../README.md) for information on how to install and startup this driver.
## ROS-API
The ROS API is documented in a [standalone document](doc/ROS_INTERFACE.md). It is auto-generated
using [catkin_doc](https://github.com/fzi-forschungszentrum-informatik/catkin_doc).
## Technical details
The following image shows a very coarse overview of the driver's architecture.
![Architecture overview](doc/architecture_coarse.svg "Architecture overview")
Upon connection to the primary interface the robot sends version and calibration information which
is consumed by the *calibration_check*. If the calibration reported by the robot doesn't match the
one configured (See [calibration guide](../ur_calibration/README.md)) an error will be printed to Roslog.
Real-time data from the robot is read through the RTDE interface. This is done automatically as soon
as a connection to the robot could be established. Thus joint states and IO data will be immediately
available.
To actually control the robot, a program node from the **External Control** URCap must be running on
the robot interpreting commands sent from an external source. When this program is not running, no
controllers moving the robot around will be available, which is handled by the
[controller_stopper](../controller_stopper/README.md). Please see the [initial setup
guide](../README.md) on how to install and start this on the robot.
The URScript that will be running on the robot is requested by the **External Control** program node
from the remote ROS PC. The robot *ur_control.launch* file has a parameter called `urscript_file` to
select a different program than the default one that will be sent as a response to a program
request.
**Custom script snippets** can be sent to the robot on a topic basis. By default, they will
interrupt other programs (such as the one controlling the robot). For a certain subset of functions,
it is however possible to send them as secondary programs. See [UR
documentation](https://www.universal-robots.com/how-tos-and-faqs/how-to/ur-how-tos/secondary-program-17257/)
on details.
<br/>
**Note to e-Series users:**
The robot won't accept script code from a remote source unless the robot is put into
*remote_control-mode*. However, if put into *remote_control-mode*, the program containing the
**External Control** program node can't be started from the panel. If this behavior is required,
please use the headless mode that does not require having a program running on the teach pendant.
Note: The current headless mode doesn't offer the full functionality of the teach pendant.
For using the **tool communication interface** on e-Series robots, a `socat` script is prepared to
forward the robot's tool communication interface to a local device on the ROS PC. See [the tool
communication setup guide](doc/setup_tool_communication.md) for details.
This driver is using [ROS-Control](https://wiki.ros.org/ros_control) for any control statements.
Therefor, it can be used with all position-based controllers available in ROS-Control. However, we
recommend using the controllers from the `ur_controllers` package. See it's
[documentation](../ur_controllers/README.md) for details. **Note: Speed scaling support will only be
available using the controllers from `ur_controllers`**

62
ur_robot_driver/config/ur10_controllers.yaml Normal file
View File

@@ -0,0 +1,62 @@
# Settings for ros_control control loop
hardware_control_loop:
loop_hz: 125
# Settings for ros_control hardware interface
hardware_interface:
joints: &robot_joints
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
# Publish all joint states ----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 125
# Publish wrench ----------------------------------
force_torque_sensor_controller:
type: force_torque_sensor_controller/ForceTorqueSensorController
publish_rate: 125
# Publish speed_scaling factor
speed_scaling_state_controller:
type: ur_controllers/SpeedScalingStateController
publish_rate: 125
# Joint Trajectory Controller - position based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
scaled_pos_traj_controller:
type: position_controllers/ScaledJointTrajectoryController
joints: *robot_joints
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
elbow_joint: {trajectory: 0.2, goal: 0.1}
wrist_1_joint: {trajectory: 0.2, goal: 0.1}
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 125
action_monitor_rate: 10
pos_traj_controller:
type: position_controllers/JointTrajectoryController
joints: *robot_joints
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
elbow_joint: {trajectory: 0.2, goal: 0.1}
wrist_1_joint: {trajectory: 0.2, goal: 0.1}
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 125
action_monitor_rate: 10

62
ur_robot_driver/config/ur10e_controllers.yaml Normal file
View File

@@ -0,0 +1,62 @@
# Settings for ros_control control loop
hardware_control_loop:
loop_hz: 500
# Settings for ros_control hardware interface
hardware_interface:
joints: &robot_joints
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
# Publish all joint states ----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 500
# Publish wrench ----------------------------------
force_torque_sensor_controller:
type: force_torque_sensor_controller/ForceTorqueSensorController
publish_rate: 500
# Publish speed_scaling factor
speed_scaling_state_controller:
type: ur_controllers/SpeedScalingStateController
publish_rate: 500
# Joint Trajectory Controller - position based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
scaled_pos_traj_controller:
type: position_controllers/ScaledJointTrajectoryController
joints: *robot_joints
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
elbow_joint: {trajectory: 0.2, goal: 0.1}
wrist_1_joint: {trajectory: 0.2, goal: 0.1}
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 500
action_monitor_rate: 10
pos_traj_controller:
type: position_controllers/JointTrajectoryController
joints: *robot_joints
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
elbow_joint: {trajectory: 0.2, goal: 0.1}
wrist_1_joint: {trajectory: 0.2, goal: 0.1}
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 500
action_monitor_rate: 10

62
ur_robot_driver/config/ur3_controllers.yaml Normal file
View File

@@ -0,0 +1,62 @@
# Settings for ros_control control loop
hardware_control_loop:
loop_hz: 125
# Settings for ros_control hardware interface
hardware_interface:
joints: &robot_joints
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
# Publish all joint states ----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 125
# Publish wrench ----------------------------------
force_torque_sensor_controller:
type: force_torque_sensor_controller/ForceTorqueSensorController
publish_rate: 125
# Publish speed_scaling factor
speed_scaling_state_controller:
type: ur_controllers/SpeedScalingStateController
publish_rate: 125
# Joint Trajectory Controller - position based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
scaled_pos_traj_controller:
type: position_controllers/ScaledJointTrajectoryController
joints: *robot_joints
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
elbow_joint: {trajectory: 0.2, goal: 0.1}
wrist_1_joint: {trajectory: 0.2, goal: 0.1}
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 125
action_monitor_rate: 10
pos_traj_controller:
type: position_controllers/JointTrajectoryController
joints: *robot_joints
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
elbow_joint: {trajectory: 0.2, goal: 0.1}
wrist_1_joint: {trajectory: 0.2, goal: 0.1}
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 125
action_monitor_rate: 10

62
ur_robot_driver/config/ur3e_controllers.yaml Normal file
View File

@@ -0,0 +1,62 @@
# Settings for ros_control control loop
hardware_control_loop:
loop_hz: 500
# Settings for ros_control hardware interface
hardware_interface:
joints: &robot_joints
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
# Publish all joint states ----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 500
# Publish wrench ----------------------------------
force_torque_sensor_controller:
type: force_torque_sensor_controller/ForceTorqueSensorController
publish_rate: 500
# Publish speed_scaling factor
speed_scaling_state_controller:
type: ur_controllers/SpeedScalingStateController
publish_rate: 500
# Joint Trajectory Controller - position based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
scaled_pos_traj_controller:
type: position_controllers/ScaledJointTrajectoryController
joints: *robot_joints
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
elbow_joint: {trajectory: 0.2, goal: 0.1}
wrist_1_joint: {trajectory: 0.2, goal: 0.1}
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 500
action_monitor_rate: 10
pos_traj_controller:
type: position_controllers/JointTrajectoryController
joints: *robot_joints
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
elbow_joint: {trajectory: 0.2, goal: 0.1}
wrist_1_joint: {trajectory: 0.2, goal: 0.1}
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 500
action_monitor_rate: 10

62
ur_robot_driver/config/ur5_controllers.yaml Normal file
View File

@@ -0,0 +1,62 @@
# Settings for ros_control control loop
hardware_control_loop:
loop_hz: 125
# Settings for ros_control hardware interface
hardware_interface:
joints: &robot_joints
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
# Publish all joint states ----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 125
# Publish wrench ----------------------------------
force_torque_sensor_controller:
type: force_torque_sensor_controller/ForceTorqueSensorController
publish_rate: 125
# Publish speed_scaling factor
speed_scaling_state_controller:
type: ur_controllers/SpeedScalingStateController
publish_rate: 125
# Joint Trajectory Controller - position based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
scaled_pos_traj_controller:
type: position_controllers/ScaledJointTrajectoryController
joints: *robot_joints
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
elbow_joint: {trajectory: 0.2, goal: 0.1}
wrist_1_joint: {trajectory: 0.2, goal: 0.1}
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 125
action_monitor_rate: 10
pos_traj_controller:
type: position_controllers/JointTrajectoryController
joints: *robot_joints
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
elbow_joint: {trajectory: 0.2, goal: 0.1}
wrist_1_joint: {trajectory: 0.2, goal: 0.1}
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 125
action_monitor_rate: 10

62
ur_robot_driver/config/ur5e_controllers.yaml Normal file
View File

@@ -0,0 +1,62 @@
# Settings for ros_control control loop
hardware_control_loop:
loop_hz: 500
# Settings for ros_control hardware interface
hardware_interface:
joints: &robot_joints
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
# Publish all joint states ----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 500
# Publish wrench ----------------------------------
force_torque_sensor_controller:
type: force_torque_sensor_controller/ForceTorqueSensorController
publish_rate: 500
# Publish speed_scaling factor
speed_scaling_state_controller:
type: ur_controllers/SpeedScalingStateController
publish_rate: 500
# Joint Trajectory Controller - position based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
scaled_pos_traj_controller:
type: position_controllers/ScaledJointTrajectoryController
joints: *robot_joints
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
elbow_joint: {trajectory: 0.2, goal: 0.1}
wrist_1_joint: {trajectory: 0.2, goal: 0.1}
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 500
action_monitor_rate: 10
pos_traj_controller:
type: position_controllers/JointTrajectoryController
joints: *robot_joints
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
elbow_joint: {trajectory: 0.2, goal: 0.1}
wrist_1_joint: {trajectory: 0.2, goal: 0.1}
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 500
action_monitor_rate: 10

675
ur_robot_driver/doc/ROS_INTERFACE.md Normal file
View File

@@ -0,0 +1,675 @@
# ur_robot_driver
The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series.
## Launchfiles
### ur3e_bringup.launch
Standalone launchfile to startup a ur3e. This requires a robot reachable via a network connection.
#### Arguments
* "**controller_config_file**" (default: "$(find ur_robot_driver)/config/ur3e_controllers.yaml")
Config file used for defining the ROS-Control controllers.
* "**controllers**" (default: "joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller")
Controllers that are activated by default.
* "**debug**" (default: "false")
Debug flag that will get passed on to ur_common.launch
* "**headless_mode**" (default: "false")
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.
* "**kinematics_config**" (default: "$(find ur_e_description)/config/ur3e_default.yaml")
Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description.
* "**limited**" (default: "false")
Use the description in limited mode (Every axis rotates from -PI to PI)
* "**robot_description_file**" (default: "$(find ur_e_description)/launch/ur3e_upload.launch")
Robot description launch file.
* "**robot_ip**" (Required)
IP address by which the robot can be reached.
* "**stopped_controllers**" (default: "pos_traj_controller")
Controllers that are initally loaded, but not started.
* "**tf_prefix**" (default: "")
tf_prefix used for the robot.
* "**tool_baud_rate**" (default: "115200")
Baud rate used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_device_name**" (default: "/tmp/ttyUR")
Local device name used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_parity**" (default: "0")
Parity configuration used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_rx_idle_chars**" (default: "1.5")
Number of idle chars in RX channel used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_stop_bits**" (default: "1")
Number of stop bits used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_tcp_port**" (default: "54321")
Port on which the robot controller publishes the tool comm interface. Only used, when `use_tool_communication` is set to true.
* "**tool_tx_idle_chars**" (default: "3.5")
Number of idle chars in TX channel used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_voltage**" (default: "0")
Tool voltage set at the beginning of the UR program. Only used, when `use_tool_communication` is set to true.
* "**use_tool_communication**" (default: "false")
On e-Series robots tool communication can be enabled with this argument
### ur10_bringup.launch
Standalone launchfile to startup a ur10 robot. This requires a robot reachable via a network connection.
#### Arguments
* "**controller_config_file**" (default: "$(find ur_robot_driver)/config/ur10_controllers.yaml")
Config file used for defining the ROS-Control controllers.
* "**controllers**" (default: "joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller")
Controllers that are activated by default.
* "**debug**" (default: "false")
Debug flag that will get passed on to ur_common.launch
* "**headless_mode**" (default: "false")
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.
* "**kinematics_config**" (default: "$(find ur_description)/config/ur10_default.yaml")
Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description.
* "**limited**" (default: "false")
Use the description in limited mode (Every axis rotates from -PI to PI)
* "**robot_description_file**" (default: "$(find ur_description)/launch/ur10_upload.launch")
Robot description launch file.
* "**robot_ip**" (Required)
IP address by which the robot can be reached.
* "**stopped_controllers**" (default: "pos_traj_controller")
Controllers that are initally loaded, but not started.
* "**tf_prefix**" (default: "")
tf_prefix used for the robot.
### ur_control.launch
Robot bringup launchfile without the robot description. Include this, if you want to include robot control into a larger launchfile structure.
#### Arguments
* "**controller_config_file**" (Required)
Config file used for defining the ROS-Control controllers.
* "**controllers**" (default: "joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller")
Controllers that are activated by default.
* "**debug**" (default: "false")
If set to true, will start the driver inside gdb
* "**headless_mode**" (default: "false")
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.
* "**kinematics_config**" (Required)
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.
* "**launch_prefix**" (Required)
Please add description. See file "launch/ur_control.launch".
* "**robot_ip**" (Required)
IP address by which the robot can be reached.
* "**rtde_input_recipe_file**" (default: "$(find ur_robot_driver)/resources/rtde_input_recipe.txt")
Recipe file used for the RTDE-inputs. Only change this if you know what you're doing.
* "**rtde_output_recipe_file**" (default: "$(find ur_robot_driver)/resources/rtde_output_recipe.txt")
Recipe file used for the RTDE-outputs. Only change this if you know what you're doing.
* "**stopped_controllers**" (default: "pos_traj_controller")
Controllers that are initally loaded, but not started.
* "**tf_prefix**" (default: "")
tf_prefix used for the robot.
* "**tool_baud_rate**" (default: "115200")
Baud rate used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_device_name**" (default: "/tmp/ttyUR")
Local device name used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_parity**" (default: "0")
Parity configuration used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_rx_idle_chars**" (default: "1.5")
Number of idle chars in RX channel used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_stop_bits**" (default: "1")
Number of stop bits used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_tcp_port**" (default: "54321")
Port on which the robot controller publishes the tool comm interface. Only used, when `use_tool_communication` is set to true.
* "**tool_tx_idle_chars**" (default: "3.5")
Number of idle chars in TX channel used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_voltage**" (default: "0")
Tool voltage set at the beginning of the UR program. Only used, when `use_tool_communication` is set to true.
* "**urscript_file**" (default: "$(find ur_robot_driver)/resources/servoj.urscript")
Path to URScript that will be sent to the robot and that forms the main control program.
* "**use_tool_communication**" (Required)
On e-Series robots tool communication can be enabled with this argument
### ur_common.launch
Launchfile that starts a robot description with robot_state publisher and the driver for a given robot. It is recommended to use the individual launch files instead such as `ur10_bringup.launch`. Additionally, this launchfile can be used as a template to include this driver into a larger launch file structure.
#### Arguments
* "**controller_config_file**" (Required)
Config file used for defining the ROS-Control controllers.
* "**controllers**" (default: "joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller")
Controllers that are activated by default.
* "**debug**" (default: "false")
Debug flag that will get passed on to ur_control.launch
* "**headless_mode**" (default: "false")
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.
* "**kinematics_config**" (Required)
Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description.
* "**limited**" (default: "false")
Use the description in limited mode (Every axis rotates from -PI to PI)
* "**robot_description_file**" (Required)
Robot description launch file.
* "**robot_ip**" (Required)
IP address by which the robot can be reached.
* "**stopped_controllers**" (default: "pos_traj_controller")
Controllers that are initally loaded, but not started.
* "**tf_prefix**" (default: "")
tf_prefix used for the robot.
* "**tool_baud_rate**" (default: "115200")
Baud rate used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_device_name**" (default: "/tmp/ttyUR")
Local device name used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_parity**" (default: "0")
Parity configuration used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_rx_idle_chars**" (default: "1.5")
Number of idle chars in RX channel used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_stop_bits**" (default: "1")
Number of stop bits used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_tcp_port**" (default: "54321")
Port on which the robot controller publishes the tool comm interface. Only used, when `use_tool_communication` is set to true.
* "**tool_tx_idle_chars**" (default: "3.5")
Number of idle chars in TX channel used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_voltage**" (default: "0")
Tool voltage set at the beginning of the UR program. Only used, when `use_tool_communication` is set to true.
* "**use_tool_communication**" (Required)
On e-Series robots tool communication can be enabled with this argument
### ur5_bringup.launch
Standalone launchfile to startup a ur5 robot. This requires a robot reachable via a network connection.
#### Arguments
* "**controller_config_file**" (default: "$(find ur_robot_driver)/config/ur5_controllers.yaml")
Config file used for defining the ROS-Control controllers.
* "**controllers**" (default: "joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller")
Controllers that are activated by default.
* "**debug**" (default: "false")
Debug flag that will get passed on to ur_common.launch
* "**headless_mode**" (default: "false")
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.
* "**kinematics_config**" (default: "$(find ur_description)/config/ur5_default.yaml")
Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description.
* "**limited**" (default: "false")
Use the description in limited mode (Every axis rotates from -PI to PI)
* "**robot_description_file**" (default: "$(find ur_description)/launch/ur5_upload.launch")
Robot description launch file.
* "**robot_ip**" (Required)
IP address by which the robot can be reached.
* "**stopped_controllers**" (default: "pos_traj_controller")
Controllers that are initally loaded, but not started.
* "**tf_prefix**" (default: "")
tf_prefix used for the robot.
### ur5e_bringup.launch
Standalone launchfile to startup a ur5e robot. This requires a robot reachable via a network connection.
#### Arguments
* "**controller_config_file**" (default: "$(find ur_robot_driver)/config/ur5e_controllers.yaml")
Config file used for defining the ROS-Control controllers.
* "**controllers**" (default: "joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller")
Controllers that are activated by default.
* "**debug**" (default: "false")
Debug flag that will get passed on to ur_common.launch
* "**headless_mode**" (default: "false")
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.
* "**kinematics_config**" (default: "$(find ur_e_description)/config/ur5e_default.yaml")
Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description.
* "**limited**" (default: "false")
Use the description in limited mode (Every axis rotates from -PI to PI)
* "**robot_description_file**" (default: "$(find ur_e_description)/launch/ur5e_upload.launch")
Robot description launch file.
* "**robot_ip**" (Required)
IP address by which the robot can be reached.
* "**stopped_controllers**" (default: "pos_traj_controller")
Controllers that are initally loaded, but not started.
* "**tf_prefix**" (default: "")
tf_prefix used for the robot.
* "**tool_baud_rate**" (default: "115200")
Baud rate used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_device_name**" (default: "/tmp/ttyUR")
Local device name used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_parity**" (default: "0")
Parity configuration used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_rx_idle_chars**" (default: "1.5")
Number of idle chars in RX channel used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_stop_bits**" (default: "1")
Number of stop bits used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_tcp_port**" (default: "54321")
Port on which the robot controller publishes the tool comm interface. Only used, when `use_tool_communication` is set to true.
* "**tool_tx_idle_chars**" (default: "3.5")
Number of idle chars in TX channel used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_voltage**" (default: "0")
Tool voltage set at the beginning of the UR program. Only used, when `use_tool_communication` is set to true.
* "**use_tool_communication**" (default: "false")
On e-Series robots tool communication can be enabled with this argument
### ur3_bringup.launch
Standalone launchfile to startup a ur5 robot. This requires a robot reachable via a network connection.
#### Arguments
* "**controller_config_file**" (default: "$(find ur_robot_driver)/config/ur3_controllers.yaml")
Config file used for defining the ROS-Control controllers.
* "**controllers**" (default: "joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller")
Controllers that are activated by default.
* "**debug**" (default: "false")
Debug flag that will get passed on to ur_common.launch
* "**headless_mode**" (default: "false")
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.
* "**kinematics_config**" (default: "$(find ur_description)/config/ur3_default.yaml")
Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description.
* "**limited**" (default: "false")
Use the description in limited mode (Every axis rotates from -PI to PI)
* "**robot_description_file**" (default: "$(find ur_description)/launch/ur3_upload.launch")
Robot description launch file.
* "**robot_ip**" (Required)
IP address by which the robot can be reached.
* "**stopped_controllers**" (default: "pos_traj_controller")
Controllers that are initally loaded, but not started.
* "**tf_prefix**" (default: "")
tf_prefix used for the robot.
### ur10e_bringup.launch
Standalone launchfile to startup a ur10e robot. This requires a robot reachable via a network connection.
#### Arguments
* "**controller_config_file**" (default: "$(find ur_robot_driver)/config/ur10e_controllers.yaml")
Config file used for defining the ROS-Control controllers.
* "**controllers**" (default: "joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller")
Controllers that are activated by default.
* "**debug**" (default: "false")
Debug flag that will get passed on to ur_common.launch
* "**headless_mode**" (default: "false")
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.
* "**kinematics_config**" (default: "$(find ur_e_description)/config/ur10e_default.yaml")
Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description.
* "**limited**" (default: "false")
Use the description in limited mode (Every axis rotates from -PI to PI)
* "**robot_description_file**" (default: "$(find ur_e_description)/launch/ur10e_upload.launch")
Robot description launch file.
* "**robot_ip**" (Required)
IP address by which the robot can be reached.
* "**stopped_controllers**" (default: "pos_traj_controller")
Controllers that are initally loaded, but not started.
* "**tf_prefix**" (default: "")
tf_prefix used for the robot.
* "**tool_baud_rate**" (default: "115200")
Baud rate used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_device_name**" (default: "/tmp/ttyUR")
Local device name used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_parity**" (default: "0")
Parity configuration used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_rx_idle_chars**" (default: "1.5")
Number of idle chars in RX channel used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_stop_bits**" (default: "1")
Number of stop bits used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_tcp_port**" (default: "54321")
Port on which the robot controller publishes the tool comm interface. Only used, when `use_tool_communication` is set to true.
* "**tool_tx_idle_chars**" (default: "3.5")
Number of idle chars in TX channel used for tool communication. Only used, when `use_tool_communication` is set to true.
* "**tool_voltage**" (default: "0")
Tool voltage set at the beginning of the UR program. Only used, when `use_tool_communication` is set to true.
* "**use_tool_communication**" (default: "false")
On e-Series robots tool communication can be enabled with this argument
## Nodes
### ur_robot_driver_node
This is the actual driver node containing the ROS-Control stack. Interfaces documented here refer to the robot's hardware interface. Controller-specific API elements might be present for the individual controllers outside of this package.
#### Advertised Services
* "**hand_back_control**" ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html))
Calling this service will make the "External Control" program node on the UR-Program return.
* "**resend_robot_program**" ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html))
When in headless mode, this sends the URScript program to the robot for execution. Use this after the program has been interrupted, e.g. by a protective- or EM-stop.
* "**set_io**" (ur_msgs/SetIO)
Service to set any of the robot's IOs
* "**set_speed_slider**" (ur_msgs/SetSpeedSliderFraction)
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.
#### Parameters
* "**hardware_interface/joints**" (Required)
Names of the joints. Usually, this is given in the controller config file.
* "**headless_mode**" (Required)
Start robot in headless mode. This does not require the 'External Control' URCap to be running on the robot, but this will send the URScript to the robot directly. On e-Series robots this requires the robot to run in 'remote-control' mode.
* "**input_recipe_file**" (Required)
Path to the file containing the recipe used for requesting RTDE inputs.
* "**kinematics/hash**" (Required)
Hash of the calibration reported by the robot. This is used for validating the robot description is using the correct calibration. If the robot's calibration doesn't match this hash, an error will be printed. You can use the robot as usual, however Cartesian poses of the endeffector might be inaccurate. See the "ur_calibration" package on help how to generate your own hash matching your actual robot.
* "**output_recipe_file**" (Required)
Path to the file containing the recipe used for requesting RTDE outputs.
* "**robot_ip**" (Required)
The robot's IP address.
* "**script_file**" (Required)
Path to the urscript code that will be sent to the robot.
* "**tf_prefix**" (default: "")
Please add description. See hardware_interface.cpp line number: 67
robot_hw_nh.param<std::string>("tf_prefix", tf_prefix_, "");
* "Symbol: **this_thread**" (default: "&params")
Please add description. See hardware_interface_node.cpp line number: 98
int ret = pthread_setschedparam(this_thread, SCHED_FIFO, &params);
* "**tool_baud_rate**" (Required)
Baud rate used for tool communication. Will be set as soon as the UR-Program on the robot is started. See UR documentation for valid baud rates. Note: This parameter is only evaluated, when the parameter "use_tool_communication" is set to TRUE. Then, this parameter is required.
* "**tool_parity**" (Required)
Parity used for tool communication. Will be set as soon as the UR-Program on the robot is started. Can be 0 (None), 1 (odd) and 2 (even). Note: This parameter is only evaluated, when the parameter "use_tool_communication" is set to TRUE. Then, this parameter is required.
* "**tool_rx_idle_chars**" (Required)
Number of idle chars for the RX unit used for tool communication. Will be set as soon as the UR-Program on the robot is started. Valid values: min=1.0, max=40.0 Note: This parameter is only evaluated, when the parameter "use_tool_communication" is set to TRUE. Then, this parameter is required.
* "**tool_stop_bits**" (Required)
Number of stop bits used for tool communication. Will be set as soon as the UR-Program on the robot is started. Can be 1 or 2. Note: This parameter is only evaluated, when the parameter "use_tool_communication" is set to TRUE. Then, this parameter is required.
* "**tool_tx_idle_chars**" (Required)
Number of idle chars for the TX unit used for tool communication. Will be set as soon as the UR-Program on the robot is started. Valid values: min=0.0, max=40.0 Note: This parameter is only evaluated, when the parameter "use_tool_communication" is set to TRUE. Then, this parameter is required.
* "**tool_voltage**" (Required)
Tool voltage that will be set as soon as the UR-Program on the robot is started. Note: This parameter is only evaluated, when the parameter "use_tool_communication" is set to TRUE. Then, this parameter is required.
* "**use_tool_communication**" (Required)
Should the tool's RS485 interface be forwarded to the ROS machine? This is only available on e-Series models. Setting this parameter to TRUE requires multiple other parameters to be set,as well.
#### Published topics
* "**robot_program_running**" ([std_msgs/Bool](http://docs.ros.org/api/std_msgs/html/msg/Bool.html))
Whenever the runtime state of the "External Control" program node in the UR-program changes, a message gets published here. So this is equivalent to the information whether the robot accepts commands from ROS side.
#### Subscribed topics
* "**script_command**" ([std_msgs/String](http://docs.ros.org/api/std_msgs/html/msg/String.html))
Send arbitrary script commands to this topic. Note: On e-Series the robot has to be in remote-control mode. Sending scripts to this will stop program execution unless wrapped in a secondary program: sec myProgram(): set_digital_out(0, True) end
### tool_communication
This node is used to start the RS485 tunneling interface on the ROS machine. This requires that the RS485 daemon is running on the robot controller and tool communication is enabled on the robot.
#### Parameters
* "**~device_name**" (Required)
By default, socat will create a pty in /dev/pts/N with n being an increasing number. Additionally, a symlink at the given location will be created. Use an absolute path here.
* "**~robot_ip**" (Required)
IP address of the robot
* "**~tcp_port**" (default: "54321")
Port on which the remote pc (robot) publishes the interface

302
ur_robot_driver/doc/architecture_coarse.svg Normal file
View File

File diff suppressed because one or more lines are too long
Side by Side

After

Width:  |  Height:  |  Size: 142 KiB

34
ur_robot_driver/doc/features.md Normal file
View File

@@ -0,0 +1,34 @@
# Feature comparison and roadmap
| Feature | ur_modern_driver | this_driver |
| --- | --- | --- |
| position-based control | yes | yes |
| scaled position-based control | - | yes |
| velocity-based control | yes | planned |
| reporting of tcp wrench | yes | yes |
| reporting of tcp wrench in tcp link | - | yes |
| pausing of programs | - | yes |
| continue trajectories after EM-Stop resume | - | yes |
| continue trajectories after protective stop | - | yes |
| panel interaction in between possible | no<sup>1</sup> | yes |
| get and set IO states | yes | yes |
| use tool communication on e-series | - | yes |
| use the driver without a teach pendant necessary | - | planned |
| support of CB2 robots | yes | - |
| trajectory extrapolation on robot on missing packages | no<sup>2</sup> | yes |
| use ROS as drop-in for TP-programs | - | yes |
| extract calibration from robot | - | yes |
| send custom script commands to robot | yes | yes |
| ROS 2 support | ? | (planned)<sup>3</sup> |
| Reconnect on a disconnected robot | yes | yes |
<sup>1</sup> Depending on the mode the driver is running the panel won't react or using the panel
will stop the program without notifying the ROS user.
<sup>2</sup> In velocity mode this is implicitly given.
<sup>3</sup> There is no specific plan to do this inside of the first driver development. However,
it is structured in a way so that a ROS2 driver should be developed as easy as possible by keeping
as much as possible in a ros-independent library.

BIN
ur_robot_driver/doc/initial_setup_images/cb3_01_welcome.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 29 KiB

BIN
ur_robot_driver/doc/initial_setup_images/cb3_05_urcaps_installed.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 61 KiB

BIN
ur_robot_driver/doc/initial_setup_images/cb3_07_installation_excontrol.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 35 KiB

BIN
ur_robot_driver/doc/initial_setup_images/cb3_10_prog_structure_urcaps.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 46 KiB

BIN
ur_robot_driver/doc/initial_setup_images/cb3_11_program_view_excontrol.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 40 KiB

BIN
ur_robot_driver/doc/initial_setup_images/e-Series.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 203 KiB

BIN
ur_robot_driver/doc/initial_setup_images/es_01_welcome.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 44 KiB

BIN
ur_robot_driver/doc/initial_setup_images/es_05_urcaps_installed.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 75 KiB

BIN
ur_robot_driver/doc/initial_setup_images/es_07_installation_excontrol.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 44 KiB

BIN
ur_robot_driver/doc/initial_setup_images/es_10_prog_structure_urcaps.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 76 KiB

BIN
ur_robot_driver/doc/initial_setup_images/es_11_program_view_excontrol.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 64 KiB

44
ur_robot_driver/doc/install_urcap_cb3.md Normal file
View File

@@ -0,0 +1,44 @@
# Installing a URCap on a CB3 robot
For using the *ur_robot_driver* with a real robot you need to install the
**externalcontrol-1.0.urcap** which can be found inside the **resources** folder of this driver.
**Note**: For installing this URCap a minimal PolyScope version of 3.7 is necessary.
To install it you first have to copy it to the robot's **programs** folder which can be done either
via scp or using a USB stick.
On the welcome screen select *Setup Robot* and then *URCaps* to enter the URCaps installation
screen.
![Welcome screen of a CB3 robot](initial_setup_images/cb3_01_welcome.png)
There, click the little plus sign at the bottom to open the file selector. There you should see
all urcap files stored inside the robot's programs folder or a plugged USB drive. Select and open
the **externalcontrol-1.0.urcap** file and click *open*. Your URCaps view should now show the
**External Control** in the list of active URCaps and a notification to restart the robot. Do that
now.
![URCaps screen with installed urcaps](initial_setup_images/cb3_05_urcaps_installed.png)
After the reboot you should find the **External Control** URCaps inside the *Installation* section.
For this select *Program Robot* on the welcome screen, select the *Installation* tab and select
**External Control** from the list.
![Installation screen of URCaps](initial_setup_images/cb3_07_installation_excontrol.png)
Here you'll have to setup the IP address of the external PC which will be running the ROS driver.
Note that the robot and the external PC have to be in the same network, ideally in a direct
connection with each other to minimize network disturbances. The custom port should be left
untouched for now.
![Insert the external control node](initial_setup_images/cb3_10_prog_structure_urcaps.png)
To use the new URCaps, create a new program and insert the **External Control** program node into
the program tree
![Program view of external control](initial_setup_images/cb3_11_program_view_excontrol.png)
If you click on the *command* tab again, you'll see the settings entered inside the *Installation*.
Check that they are correct, then save the program. Your robot is now ready to be used together with
this driver.

43
ur_robot_driver/doc/install_urcap_e_series.md Normal file
View File

@@ -0,0 +1,43 @@
# Installing a URCap on a s-Series robot
For using the *ur_robot_driver* with a real robot you need to install the
**externalcontrol-1.0.urcap** which can be found inside the **resources** folder of this driver.
**Note**: For installing this URCap a minimal PolyScope version of 5.1 is necessary.
To install it you first have to copy it to the robot's **programs** folder which can be done either
via scp or using a USB stick.
On the welcome screen click on the hamburger menu in the top-right corner and select *Settings* to enter the robot's setup. There select *System* and then *URCaps* to enter the URCaps installation screen.
![Welcome screen of an e-Series robot](initial_setup_images/es_01_welcome.png)
There, click the little plus sign at the bottom to open the file selector. There you should see
all urcap files stored inside the robot's programs folder or a plugged USB drive. Select and open
the **externalcontrol-1.0.urcap** file and click *open*. Your URCaps view should now show the
**External Control** in the list of active URCaps and a notification to restart the robot. Do that
now.
![URCaps screen with installed urcaps](initial_setup_images/es_05_urcaps_installed.png)
After the reboot you should find the **External Control** URCaps inside the *Installation* section.
For this select *Program Robot* on the welcome screen, select the *Installation* tab and select
**External Control** from the list.
![Installation screen of URCaps](initial_setup_images/es_07_installation_excontrol.png)
Here you'll have to setup the IP address of the external PC which will be running the ROS driver.
Note that the robot and the external PC have to be in the same network, ideally in a direct
connection with each other to minimize network disturbances. The custom port should be left
untouched for now.
![Insert the external control node](initial_setup_images/es_10_prog_structure_urcaps.png)
To use the new URCaps, create a new program and insert the **External Control** program node into
the program tree
![Program view of external control](initial_setup_images/es_11_program_view_excontrol.png)
If you click on the *command* tab again, you'll see the settings entered inside the *Installation*.
Check that they are correct, then save the program. Your robot is now ready to be used together with
this driver.

282
ur_robot_driver/doc/real_time.md Normal file
View File

@@ -0,0 +1,282 @@
# Setting up Ubuntu with a PREEMPT_RT kernel
In order to run the `universal_robot_driver`, we highly recommend to setup a ubuntu system with
real-time capabilities. Especially with a robot from the e-Series the higher control frequency
might lead to non-smooth trajectory execution if not run using a real-time-enabled system.
You might still be able to control the robot using a non-real-time system. This is, however, not recommended.
To get real-time support into a ubuntu system, the following steps have to be performed:
1. Get the sources of a real-time kernel
2. Compile the real-time kernel
3. Setup user privileges to execute real-time tasks
This guide will help you setup your system with a real-time kernel.
## Preparing
To build the kernel, you will need a couple of tools available on your system. You can install them
using
``` bash
$ sudo apt-get install build-essential bc ca-certificates gnupg2 libssl-dev wget gawk
```
Before you download the sources of a real-time-enabled kernel, check the kernel version that is currently installed:
```bash
$ uname -r
4.15.0-62-generic
```
To continue with this tutorial, please create a temporary folder and navigate into it. You should
have sufficient space (around 25GB) there, as the extracted kernel sources take much space. After
the new kernel is installed, you can delete this folder again.
In this example we will use a temporary folder inside our home folder:
```bash
$ mkdir -p ${HOME}/rt_kernel_build
$ cd ${HOME}/rt_kernel_build
```
All future commands are expected to be run inside this folder. If the folder is different, the `$`
sign will be prefixed with a path relative to the above folder.
## Getting the sources for a real-time kernel
To build a real-time kernel, we first need to get the kernel sources and the real-time patch.
First, we must decide on the kernel version that we want to use. Above, we
determined that our system has a 4.15 kernel installed. However, real-time
patches exist only for selected kernel versions. Those can be found on the
[linuxfoundation wiki](https://wiki.linuxfoundation.org/realtime/preempt_rt_versions).
In this example, we will select a 4.14 kernel. Select a kernel version close to the
one installed on your system.
Go ahead and download the kernel sources, patch sources and their signature files:
```bash
$ wget https://cdn.kernel.org/pub/linux/kernel/projects/rt/4.14/patch-4.14.139-rt66.patch.xz
$ wget https://cdn.kernel.org/pub/linux/kernel/projects/rt/4.14/patch-4.14.139-rt66.patch.sign
$ wget https://www.kernel.org/pub/linux/kernel/v4.x/linux-4.14.139.tar.xz
$ wget https://www.kernel.org/pub/linux/kernel/v4.x/linux-4.14.139.tar.sign
```
To unzip the downloaded files do
```bash
$ xz -d patch-4.14.139-rt66.patch.xz
$ xz -d linux-4.14.139.tar.xz
```
### Verification
Technically, you can skip this section, it is however highly recommended to verify the file
integrity of such a core component of your system!
To verify file integrity, you must first import public keys by the kernel developers and the patch
author. For the kernel sources use (as suggested on
[kernel.org](https://www.kernel.org/signature.html))
```bash
$ gpg2 --locate-keys torvalds@kernel.org gregkh@kernel.org
```
and for the patch search for a key of the author listed on
[linuxfoundation wiki](https://wiki.linuxfoundation.org/realtime/preempt_rt_versions).
```bash
$ gpg2 --keyserver hkp://keys.gnupg.net --search-keys zanussi
gpg: data source: http://51.38.91.189:11371
(1) German Daniel Zanussi <german.zanussi@globant.com>
4096 bit RSA key 0x537F98A9D92CEAC8, created: 2019-07-24, expires: 2023-07-24
(2) Michael Zanussi <mzanussi@gmail.com>
4096 bit RSA key 0x7C7F76A2C1E3D9EB, created: 2019-05-08
(3) Tom Zanussi <tzanussi@gmail.com>
Tom Zanussi <zanussi@kernel.org>
Tom Zanussi <tom.zanussi@linux.intel.com>
4096 bit RSA key 0xDE09826778A38521, created: 2017-12-15
(4) Riccardo Zanussi <riccardo.zanussi@gmail.com>
2048 bit RSA key 0xD299A06261D919C3, created: 2014-08-27, expires: 2018-08-27 (expired)
(5) Zanussi Gianni <g.zanussi@virgilio.it>
1024 bit DSA key 0x78B89CB020D1836C, created: 2004-04-06
(6) Michael Zanussi <zanussi@unm.edu>
Michael Zanussi <mzanussi@gmail.com>
Michael Zanussi <michael_zanussi@yahoo.com>
Michael Zanussi <michael@michaelzanussi.com>
1024 bit DSA key 0xB3E952DCAC653064, created: 2000-09-05
(7) Michael Zanussi <surfpnk@yahoo.com>
1024 bit DSA key 0xEB10BBD9BA749318, created: 1999-05-31
(8) Michael B. Zanussi <surfpnk@yahoo.com>
1024 bit DSA key 0x39EE4EAD7BBB1E43, created: 1998-07-16
Keys 1-8 of 8 for "zanussi". Enter number(s), N)ext, or Q)uit > 3
```
Now we can verify the downloaded sources:
```bash
$ gpg2 --verify linux-4.14.139.tar.sign
gpg: assuming signed data in 'linux-4.14.139.tar'
gpg: Signature made Fr 16 Aug 2019 10:15:17 CEST
gpg: using RSA key 647F28654894E3BD457199BE38DBBDC86092693E
gpg: Good signature from "Greg Kroah-Hartman <gregkh@kernel.org>" [unknown]
gpg: WARNING: This key is not certified with a trusted signature!
gpg: There is no indication that the signature belongs to the owner.
Primary key fingerprint: 647F 2865 4894 E3BD 4571 99BE 38DB BDC8 6092 693E
$ gpg2 --verify patch-4.14.139-rt66.patch.sign
gpg: assuming signed data in 'patch-4.14.139-rt66.patch'
gpg: Signature made Fr 23 Aug 2019 21:09:20 CEST
gpg: using RSA key 0x0129F38552C38DF1
gpg: Good signature from "Tom Zanussi <tom.zanussi@linux.intel.com>" [unknown]
gpg: aka "Tom Zanussi <zanussi@kernel.org>" [unknown]
gpg: aka "Tom Zanussi <tzanussi@gmail.com>" [unknown]
gpg: WARNING: This key is not certified with a trusted signature!
gpg: There is no indication that the signature belongs to the owner.
Primary key fingerprint: 5BDF C45C 2ECC 5387 D50C E5EF DE09 8267 78A3 8521
Subkey fingerprint: ACF8 5F98 16A8 D5F0 96AE 1FD2 0129 F385 52C3 8DF1
```
## Compilation
Before we can compile the sources, we have to extract the tar archive and apply the patch
```bash
$ tar xf linux-4.14.139.tar
$ cd linux-4.14.139
linux-4.14.139$ xzcat ../patch-4.14.139-rt66.patch.xz | patch -p1
```
Now to configure your kernel, just type
```bash
linux-4.14.139$ make oldconfig
```
This will ask for kernel options. For everything else then the `Preemption Model` use the default
value (just press Enter) or adapt to your preferences. For the preemption model select `Fully Preemptible Kernel`:
```bash
Preemption Model
1. No Forced Preemption (Server) (PREEMPT_NONE)
> 2. Voluntary Kernel Preemption (Desktop) (PREEMPT_VOLUNTARY)
3. Preemptible Kernel (Low-Latency Desktop) (PREEMPT__LL) (NEW)
4. Preemptible Kernel (Basic RT) (PREEMPT_RTB) (NEW)
5. Fully Preemptible Kernel (RT) (PREEMPT_RT_FULL) (NEW)
choice[1-5]: 5
```
Now you can build the kernel. This will take some time...
```bash
linux-4.14.139$ make -j `getconf _NPROCESSORS_ONLN` deb-pkg
```
After building, install the `linux-headers` and `linux-image` packages in the parent folder (only
the ones without the `-dbg` in the name)
```bash
linux-4.14.139$ sudo apt install ../linux-headers-4.14.139-rt66_*.deb ../linux-image-4.14.139-rt66_*.deb
```
## Setup user privileges to use real-time scheduling
To be able to schedule threads with user privileges (what the driver will do) you'll have to change
the user's limits by changing `/etc/security/limits.conf` (See [the manpage](https://manpages.ubuntu.com/manpages/bionic/man5/limits.conf.5.html) for details)
We recommend to setup a group for real-time users instead of writing a fixed username into the config
file:
```bash
$ sudo groupadd realtime
$ sudo usermod -aG realtime $(whoami)
```
Then, make sure `/etc/security/limits.conf` contains
```
@realtime soft rtprio 99
@realtime soft priority 99
@realtime soft memlock 102400
@realtime hard rtprio 99
@realtime hard priority 99
@realtime hard memlock 102400
```
Note: You will have to log out and log back in (Not only close your terminal window) for these
changes to take effect. No need to do this now, as we will reboot later on, anyway.
## Setup GRUB to always boot the real-time kernel
To make the new kernel the default kernel that the system will boot into every time, you'll have to
change the grub config file inside `/etc/default/grub`.
Note: This works for ubuntu, but might not be working for other linux systems. It might be necessary
to use another menuentry name there.
But first, let's find out the name of the entry that we will want to make the default. You can list
all available kernels using
```bash
$ awk -F\' '/menuentry |submenu / {print $1 $2}' /boot/grub/grub.cfg
menuentry Ubuntu
submenu Advanced options for Ubuntu
menuentry Ubuntu, with Linux 4.15.0-62-generic
menuentry Ubuntu, with Linux 4.15.0-62-generic (recovery mode)
menuentry Ubuntu, with Linux 4.15.0-60-generic
menuentry Ubuntu, with Linux 4.15.0-60-generic (recovery mode)
menuentry Ubuntu, with Linux 4.15.0-58-generic
menuentry Ubuntu, with Linux 4.15.0-58-generic (recovery mode)
menuentry Ubuntu, with Linux 4.14.139-rt66
menuentry Ubuntu, with Linux 4.14.139-rt66 (recovery mode)
menuentry Memory test (memtest86+)
menuentry Memory test (memtest86+, serial console 115200)
menuentry Windows 7 (on /dev/sdc2)
menuentry Windows 7 (on /dev/sdc3)
```
From the output above, we'll need to generate a string with the pattern `"submenu_name>entry_name"`. In our case this would be
```
"Advanced options for Ubuntu>Ubuntu, with Linux 4.14.139-rt66"
```
**The double quotes and no spaces around the `>` are important!**
With this, we can setup the default grub entry and then update the grub menu entries. Don't forget this last step!
```bash
$ sudo sed -i 's/^GRUB_DEFAULT=.*/GRUB_DEFAULT="Advanced options for Ubuntu>Ubuntu, with Linux 4.14.139-rt66"/' /etc/default/grub
$ sudo update-grub
```
## Reboot the PC
After having performed the above mentioned steps, reboot the PC. It should boot into the correct
kernel automatically.
## Check for preemption capabilities
Make sure that the kernel does indeed support real-time scheduling:
```bash
$ uname -v | cut -d" " -f1-4
#1 SMP PREEMPT RT
```
## Optional: Disable CPU speed scaling
Many modern CPUs support changing their clock frequency dynamically depending on the currently
requested computation resources. In some cases this can lead to small interruptions in execution.
While the real-time scheduled controller thread should be unaffected by this, any external
components such as a visual servoing system might be interrupted for a short period on scaling
changes.
To check and modify the power saving mode, install cpufrequtils:
```bash
$ sudo apt install cpufrequtils
```
Run `cpufreq-info` to check available "governors" and the current CPU Frequency (`current CPU
frequency is XXX MHZ`). In the following we will set the governor to "performance".
```bash
$ sudo systemctl disable ondemand
$ sudo systemctl enable cpufrequtils
$ sudo sh -c 'echo "GOVERNOR=performance" > /etc/default/cpufrequtils'
$ sudo systemctl daemon-reload && sudo systemctl restart cpufrequtils
```
This disables the `ondemand` CPU scaling daemon, creates a `cpufrequtils` config file and restarts
the `cpufrequtils` service. Check with `cpufreq-info`.
For further information about governors, please see the [kernel
documentation](https://www.kernel.org/doc/Documentation/cpu-freq/governors.txt).

4
ur_robot_driver/doc/rosdoc.yaml Normal file
View File

@@ -0,0 +1,4 @@
- builder: doxygen
name: C++ API
output_dir: c++
file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'

48
ur_robot_driver/doc/setup_tool_communication.md Normal file
View File

@@ -0,0 +1,48 @@
# Setting up the tool communication on an e-Series robot
The Universal Robots e-Series provides an rs485 based interface at the tool flange that can be used
to attach an rs485-based device to the robot's tcp without the need to wire a separate cable along
the robot.
This driver enables forwarding this tool communication interface to an external machine for example
to start a device's ROS driver on a remote PC.
This document will guide you through installing the URCap needed for this and setting up your ROS
launch files to utilize the robot's tool communication.
## Robot setup
For setting up the robot, please install the **rs485-1.0.urcap** found in the **resources** folder.
Installing a URCap is explained in the [setup guide](install_urcap_e_series.md) for the **external-control** URCap.
After installing the URCap the robot will expose its tool communication device to the network.
## Setup the ROS side
In order to use the tool communication in ROS, simply pass the correct parameters to the bringup
launch files:
```bash
$ roslaunch ur_robot_driver ur<3|5|10>e_bringup.launch \
use_tool_communication:=true \
tool_voltage:=24 \ # can be 0, 12 or 24
tool_parity:=0 \ # 0: none, 1: odd, 2: even
tool_baud_rate:=115200 \
tool_stop_bits:=1 \
tool_rx_idle_chars:=1.5 \
tool_tx_idle_chars:=3.5 \
tool_device_name:=/tmp/ttyUR # remember that your user needs to have the rights to write that file handle
```
The `tool_device_name` is an arbitrary name for the device file at which the device will be
accessible in the local file system. Most ROS drivers for rs485 devices accept an argument to
specify the device file path. With the example above you could run the `rs485_node` from the package
`imaginary_drivers` using the following command:
```bash
$ rosrun imaginary_drivers rs485_node device:=/tmp/ttyUR
```
You can basically choose any device name, but your user has to have the correct rights to actually
create a new file handle inside this directory. Therefore, we didn't use the `/dev` folder in the
example, as users usually don't have the access rights to create new files there.
For all the other tool parameters seen above, please refer to the Universal Robots user manual.

376
ur_robot_driver/include/ur_robot_driver/comm/bin_parser.h Normal file
View File

@@ -0,0 +1,376 @@
/*
* Copyright 2019, FZI Forschungszentrum Informatik (refactor)
*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#pragma once
#include <assert.h>
#include <endian.h>
#include <inttypes.h>
#include <array>
#include <bitset>
#include <cstddef>
#include <cstring>
#include <string>
#include <memory>
#include "ur_robot_driver/log.h"
#include "ur_robot_driver/types.h"
#include "ur_robot_driver/exceptions.h"
namespace ur_driver
{
namespace comm
{
/*!
* \brief The BinParser class handles a byte buffer and functionality to iteratively parse the
* content.
*/
class BinParser
{
private:
uint8_t *buf_pos_, *buf_end_;
BinParser& parent_;
// Decode from network encoding (big endian) to host encoding
template <typename T>
T decode(T val)
{
return val;
}
uint16_t decode(uint16_t val)
{
return be16toh(val);
}
uint32_t decode(uint32_t val)
{
return be32toh(val);
}
uint64_t decode(uint64_t val)
{
return be64toh(val);
}
int16_t decode(int16_t val)
{
return be16toh(val);
}
int32_t decode(int32_t val)
{
return be32toh(val);
}
int64_t decode(int64_t val)
{
return be64toh(val);
}
public:
/*!
* \brief Creates a new BinParser object from a given buffer.
*
* \param buffer The byte buffer to parse
* \param buf_len Size of the buffer
*/
BinParser(uint8_t* buffer, size_t buf_len) : buf_pos_(buffer), buf_end_(buffer + buf_len), parent_(*this)
{
assert(buf_pos_ <= buf_end_);
}
/*!
* \brief Creates a new BinParser object for part of a buffer from a parent BinParser.
*
* \param parent Parent BinParser
* \param sub_len Size of the sub-buffer to parse
*/
BinParser(BinParser& parent, size_t sub_len)
: buf_pos_(parent.buf_pos_), buf_end_(parent.buf_pos_ + sub_len), parent_(parent)
{
assert(buf_pos_ <= buf_end_);
}
/*!
* \brief Deconstructor for the BinParser.
*/
~BinParser()
{
parent_.buf_pos_ = buf_pos_;
}
/*!
* \brief Parses the next bytes as given type without moving the buffer pointer.
*
* @tparam T Type to parse as
*
* \returns Value of the next bytes as type T
*/
template <typename T>
T peek()
{
if (buf_pos_ + sizeof(T) > buf_end_)
throw UrException("Could not parse received package. This can occur if the driver is started while the robot is "
"booting - please restart the driver once the robot has finished booting. "
"If the problem persists after the robot has booted, please contact the package maintainer.");
T val;
std::memcpy(&val, buf_pos_, sizeof(T));
return decode(val);
}
/*!
* \brief Parses the next bytes as given type.
*
* @tparam T Type to parse as
* \param val Reference to write the parsed value to
*/
template <typename T>
void parse(T& val)
{
val = peek<T>();
buf_pos_ += sizeof(T);
}
/*!
* \brief Parses the next bytes as a double.
*
* \param val Reference to write the parsed value to
*/
void parse(double& val)
{
uint64_t inner;
parse<uint64_t>(inner);
std::memcpy(&val, &inner, sizeof(double));
}
/*!
* \brief Parses the next bytes as a float.
*
* \param val Reference to write the parsed value to
*/
void parse(float& val)
{
uint32_t inner;
parse<uint32_t>(inner);
std::memcpy(&val, &inner, sizeof(float));
}
/*!
* \brief Parses the next byte as a bool.
*
* UR uses 1 byte for boolean values but sizeof(bool) is implementation defined, so we must ensure
* they're parsed as uint8_t on all compilers
*
*
* \param val Reference to write the parsed value to
*/
void parse(bool& val)
{
uint8_t inner;
parse<uint8_t>(inner);
val = inner != 0;
}
/*!
* \brief Parses the next bytes as a vector of 3 doubles.
*
* \param val Reference to write the parsed value to
*/
void parse(vector3d_t& val)
{
for (size_t i = 0; i < val.size(); ++i)
{
parse(val[i]);
}
}
/*!
* \brief Parses the next bytes as a vector of 6 doubles.
*
* \param val Reference to write the parsed value to
*/
void parse(vector6d_t& val)
{
for (size_t i = 0; i < val.size(); ++i)
{
parse(val[i]);
}
}
/*!
* \brief Parses the next bytes as a vector of 6 32 bit integers.
*
* \param val Reference to write the parsed value to
*/
void parse(vector6int32_t& val)
{
for (size_t i = 0; i < val.size(); ++i)
{
parse(val[i]);
}
}
/*!
* \brief Parses the next bytes as a vector of 6 unsigned 32 bit integers.
*
* \param val Reference to write the parsed value to
*/
void parse(vector6uint32_t& val)
{
for (size_t i = 0; i < val.size(); ++i)
{
parse(val[i]);
}
}
/*!
* \brief Writes the remaining bytes into a given buffer without parsing them.
*
* \param buffer The buffer to copy the remaining bytes to.
* \param buffer_length Reference to write the number of remaining bytes to.
*/
void rawData(std::unique_ptr<uint8_t>& buffer, size_t& buffer_length)
{
buffer_length = buf_end_ - buf_pos_;
buffer.reset(new uint8_t[buffer_length]);
memcpy(buffer.get(), buf_pos_, buffer_length);
consume();
}
/*!
* \brief Parses the remaining bytes as a string.
*
* \param val Reference to write the parsed value to
*/
void parseRemainder(std::string& val)
{
parse(val, size_t(buf_end_ - buf_pos_));
}
/*!
* \brief Parses a given number of bytes as a string.
*
* \param val Reference to write the parsed value to
* \param len Number of bytes to parse
*/
void parse(std::string& val, size_t len)
{
val.assign(reinterpret_cast<char*>(buf_pos_), len);
buf_pos_ += len;
}
/*!
* \brief Special string parse function that assumes uint8_t len followed by chars
*
* \param val Reference to write the parsed value to
*/
void parse(std::string& val)
{
uint8_t len;
parse(len);
parse(val, size_t(len));
}
/*!
* \brief Parses the next bytes as an array of a given type and size.
*
* @tparam T Type of the array
* @tparam N Number of elements in the array
* \param array Reference of an array to parse to.
*/
template <typename T, size_t N>
void parse(std::array<T, N>& array)
{
for (size_t i = 0; i < N; i++)
{
parse(array[i]);
}
}
/*!
* \brief Parses the next bytes as a value of a given type, but also copies it to a bitset.
*
* @tparam T Type to parse as
* @tparam N Size of the bitset to copy to
* \param set Reference to the bitset to copy the value to.
*/
template <typename T, size_t N>
void parse(std::bitset<N>& set)
{
T val;
parse(val);
set = std::bitset<N>(val);
}
/*!
* \brief Sets the current buffer position to the end of the buffer, finishing parsing.
*/
void consume()
{
buf_pos_ = buf_end_;
}
/*!
* \brief Moves the current buffer position ahead by a given amount.
*
* \param bytes Number of bytes to move the buffer position
*/
void consume(size_t bytes)
{
buf_pos_ += bytes;
}
/*!
* \brief Checks if at least a given number of bytes is still remaining unparsed in the buffer.
*
* \param bytes Number of bytes to check for
*
* \returns True, if at least the given number of bytes are unparsed, false otherwise.
*/
bool checkSize(size_t bytes)
{
return bytes <= size_t(buf_end_ - buf_pos_);
}
/*!
* \brief Checks if enough bytes for a given type remain unparsed in the buffer.
*
* @tparam T The type to check for
*
* \returns True, if enough bytes are unparsed, false otherwise.
*/
template <typename T>
bool checkSize(void)
{
return checkSize(T::SIZE);
}
/*!
* \brief Checks if no unparsed bytes remain in the buffer.
*
* \returns True, if all bytes were parsed, false otherwise.
*/
bool empty()
{
return buf_pos_ == buf_end_;
}
/*!
* \brief Logs debugging information about the BinParser object.
*/
void debug()
{
LOG_DEBUG("BinParser: %p - %p (%zu bytes)", buf_pos_, buf_end_, buf_end_ - buf_pos_);
}
};
} // namespace comm
} // namespace ur_driver

74
ur_robot_driver/include/ur_robot_driver/comm/package.h Normal file
View File

@@ -0,0 +1,74 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-08
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_PACKAGE_H_INCLUDED
#define UR_RTDE_DRIVER_PACKAGE_H_INCLUDED
#include "ur_robot_driver/comm/bin_parser.h"
namespace ur_driver
{
namespace comm
{
/*!
* \brief The URPackage a parent class. From that two implementations are inherited,
* one for the primary, one for the rtde interface (primary_interface::primaryPackage;
* rtde_interface::rtdePackage). The URPackage makes use of the template HeaderT.
*/
template <typename HeaderT>
class URPackage
{
public:
/*!
* \brief Creates a new URPackage object.
*/
URPackage() = default;
virtual ~URPackage() = default;
/*!
* \brief Sets the attributes of the package by parsing a serialized representation of the
* package.
*
* \param bp A parser containing a serialized version of the package
*
* \returns True, if the package was parsed successfully, false otherwise
*/
virtual bool parseWith(BinParser& bp) = 0;
/*!
* \brief Produces a human readable representation of the package object.
*
* \returns A string representing the object
*/
virtual std::string toString() const = 0;
private:
HeaderT header_;
};
} // namespace comm
} // namespace ur_driver
#endif // ifndef UR_RTDE_DRIVER_PACKAGE_H_INCLUDED

134
ur_robot_driver/include/ur_robot_driver/comm/package_serializer.h Normal file
View File

@@ -0,0 +1,134 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-04-10
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_PACKAGE_SERIALIZER_H_INCLUDED
#define UR_RTDE_DRIVER_PACKAGE_SERIALIZER_H_INCLUDED
#include <endian.h>
#include <cstring>
namespace ur_driver
{
namespace comm
{
/*!
* \brief A helper class to serialize packages. Contains methods for serializing all relevant
* datatypes.
*/
class PackageSerializer
{
public:
/*!
* \brief A generalized serialization method for arbitrary datatypes.
*
* @tparam T The type to serialize
* \param buffer The buffer to write the serialization into
* \param val The value to serialize
*
* \returns Size in byte of the serialization
*/
template <typename T>
static size_t serialize(uint8_t* buffer, T val)
{
size_t size = sizeof(T);
T tmp = encode(val);
std::memcpy(buffer, &tmp, size);
return size;
}
/*!
* \brief A serialization method for double values.
*
* \param buffer The buffer to write the serialization into.
* \param val The value to serialize.
*
* \returns Size in byte of the serialization.
*/
static size_t serialize(uint8_t* buffer, double val)
{
size_t size = sizeof(double);
uint64_t inner;
std::memcpy(&inner, &val, size);
inner = encode(inner);
std::memcpy(buffer, &inner, size);
return size;
}
/*!
* \brief A serialization method for strings.
*
* \param buffer The buffer to write the serialization into.
* \param val The string to serialize.
*
* \returns Size in byte of the serialization.
*/
static size_t serialize(uint8_t* buffer, std::string val)
{
const uint8_t* c_val = reinterpret_cast<const uint8_t*>(val.c_str());
for (size_t i = 0; i < val.size(); i++)
{
buffer[i] = c_val[i];
}
return val.size();
}
private:
template <typename T>
static T encode(T val)
{
return val;
}
static uint16_t encode(uint16_t val)
{
return htobe16(val);
}
static uint32_t encode(uint32_t val)
{
return htobe32(val);
}
static uint64_t encode(uint64_t val)
{
return htobe64(val);
}
static int16_t encode(int16_t val)
{
return htobe16(val);
}
static int32_t encode(int32_t val)
{
return htobe32(val);
}
static int64_t encode(int64_t val)
{
return htobe64(val);
}
};
} // namespace comm
} // namespace ur_driver
#endif // UR_RTDE_DRIVER_PACKAGE_SERIALIZER_H_INCLUDED

58
ur_robot_driver/include/ur_robot_driver/comm/parser.h Normal file
View File

@@ -0,0 +1,58 @@
/*
* Copyright 2019, FZI Forschungszentrum Informatik (refactor)
*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#pragma once
#include <vector>
#include "ur_robot_driver/comm/bin_parser.h"
#include "ur_robot_driver/comm/package.h"
namespace ur_driver
{
namespace comm
{
/*!
* \brief The parser is a general paser. The namsepace rtde_interface and primary_interface both
* iclude classes which inherit from it (rtdeParser and primaryParser).
* The parser functionality also embodies a factory function taking in an uint8.
*/
template <typename HeaderT>
class Parser
{
public:
Parser() = default;
virtual ~Parser() = default;
/*!
* \brief Declares the parse function.
*
* \param bp Instance of class binaryParser
* \param results A unique pointer
*/
virtual bool parse(BinParser& bp, std::vector<std::unique_ptr<URPackage<HeaderT>>>& results) = 0;
using _header_type = HeaderT;
private:
HeaderT header_;
// URProducer producer_;
};
} // namespace comm
} // namespace ur_driver

437
ur_robot_driver/include/ur_robot_driver/comm/pipeline.h Normal file
View File

@@ -0,0 +1,437 @@
/*
* Copyright 2019, FZI Forschungszentrum Informatik (templating)
*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#pragma once
#include "ur_robot_driver/comm/package.h"
#include "ur_robot_driver/log.h"
#include "ur_robot_driver/queue/readerwriterqueue.h"
#include <atomic>
#include <chrono>
#include <thread>
#include <vector>
#include <fstream>
namespace ur_driver
{
namespace comm
{
/*!
* \brief Parent class for for arbitrary consumers.
*
* @tparam T Type of the consumed products
*/
template <typename T>
class IConsumer
{
public:
/*!
* \brief Set-up functionality of the consumer.
*/
virtual void setupConsumer()
{
}
/*!
* \brief Fully tears down the consumer - by default no difference to stopping it.
*/
virtual void teardownConsumer()
{
stopConsumer();
}
/*!
* \brief Stops the consumer.
*/
virtual void stopConsumer()
{
}
/*!
* \brief Functionality for handling consumer timeouts.
*/
virtual void onTimeout()
{
}
/*!
* \brief Consumes a product, utilizing it's contents.
*
* \param product Shared pointer to the product to be consumed.
*
* \returns Success of the consumption.
*/
virtual bool consume(std::shared_ptr<T> product) = 0;
};
/*!
* \brief Consumer, that allows one product to be consumed by multiple arbitrary
* conusmers.
*
* @tparam T Type of the consumed products
*/
template <typename T>
class MultiConsumer : public IConsumer<T>
{
private:
std::vector<IConsumer<T>*> consumers_;
public:
/*!
* \brief Creates a new MultiConsumer object.
*
* \param consumers The list of consumers that should all consume given products
*/
MultiConsumer(std::vector<IConsumer<T>*> consumers) : consumers_(consumers)
{
}
/*!
* \brief Sets up all registered consumers.
*/
virtual void setupConsumer()
{
for (auto& con : consumers_)
{
con->setupConsumer();
}
}
/*!
* \brief Tears down all registered consumers.
*/
virtual void teardownConsumer()
{
for (auto& con : consumers_)
{
con->teardownConsumer();
}
}
/*!
* \brief Stops all registered consumers.
*/
virtual void stopConsumer()
{
for (auto& con : consumers_)
{
con->stopConsumer();
}
}
/*!
* \brief Triggers timeout functionality for all registered consumers.
*/
virtual void onTimeout()
{
for (auto& con : consumers_)
{
con->onTimeout();
}
}
/*!
* \brief Consumes a given product with all registered consumers.
*
* \param product Shared pointer to the product to be consumed.
*
* \returns Success of the consumption.
*/
bool consume(std::shared_ptr<T> product)
{
bool res = true;
for (auto& con : consumers_)
{
if (!con->consume(product))
res = false;
}
return res;
}
};
/*!
* \brief Parent class for arbitrary producers of packages.
*
* @tparam HeaderT Header type of the produced packages
*/
template <typename HeaderT>
class IProducer
{
public:
/*!
* \brief Set-up functionality of the producers.
*/
virtual void setupProducer()
{
}
/*!
* \brief Fully tears down the producer - by default no difference to stopping it.
*/
virtual void teardownProducer()
{
stopProducer();
}
/*!
* \brief Stops the producer.
*/
virtual void stopProducer()
{
}
/*!
* \brief Reads packages from some source and produces corresponding objects.
*
* \param products Vector of unique pointers to be filled with produced packages.
*
* \returns Success of the package production.
*/
virtual bool tryGet(std::vector<std::unique_ptr<URPackage<HeaderT>>>& products) = 0;
};
/*!
* \brief Parent class for notifiers.
*/
class INotifier
{
public:
/*!
* \brief Start notification.
*/
virtual void started(std::string name)
{
}
/*!
* \brief Stop notification.
*/
virtual void stopped(std::string name)
{
}
};
/*!
* \brief The Pipepline manages the production and optionally consumption of packages. Cyclically
* the producer is called and returned packages are saved in a queue. This queue is then either also
* cyclically utilized by the registered consumer or can be externally used.
*
* @tparam HeaderT Header type of the managed packages
*/
template <typename HeaderT>
class Pipeline
{
public:
typedef std::chrono::high_resolution_clock Clock;
typedef Clock::time_point Time;
using _package_type = URPackage<HeaderT>;
/*!
* \brief Creates a new Pipeline object, registering producer, consumer and notifier.
* Additionally, an empty queue is initialized.
*
* \param producer The producer to run in the pipeline
* \param consumer The consumer to run in the pipeline
* \param name The pipeline's name
* \param notifier The notifier to use
*/
Pipeline(IProducer<HeaderT>& producer, IConsumer<_package_type>& consumer, std::string name, INotifier& notifier)
: producer_(producer), consumer_(&consumer), name_(name), notifier_(notifier), queue_{ 32 }, running_{ false }
{
}
/*!
* \brief Creates a new Pipeline object, registering producer and notifier while no consumer is
* used. Additionally, an empty queue is initialized.
*
* \param producer The producer to run in the pipeline
* \param name The pipeline's name
* \param notifier The notifier to use
*/
Pipeline(IProducer<HeaderT>& producer, std::string name, INotifier& notifier)
: producer_(producer), consumer_(nullptr), name_(name), notifier_(notifier), queue_{ 32 }, running_{ false }
{
}
/*!
* \brief The Pipeline object's destructor, stopping the pipeline and joining all running threads.
*/
virtual ~Pipeline()
{
LOG_DEBUG("Destructing pipeline");
stop();
}
/*!
* \brief Starts the producer and, if existing, the consumer in new threads.
*/
void run()
{
if (running_)
return;
running_ = true;
producer_.setupProducer();
pThread_ = std::thread(&Pipeline::runProducer, this);
if (consumer_ != nullptr)
cThread_ = std::thread(&Pipeline::runConsumer, this);
notifier_.started(name_);
}
/*!
* \brief Stops the pipeline and all running threads.
*/
void stop()
{
if (!running_)
return;
LOG_DEBUG("Stopping pipeline! <%s>", name_.c_str());
running_ = false;
if (pThread_.joinable())
{
pThread_.join();
}
if (cThread_.joinable())
{
cThread_.join();
}
notifier_.stopped(name_);
}
/*!
* \brief Returns the next package in the queue. Can be used instead of registering a consumer.
*
* \param product Unique pointer to be set to the package
* \param timeout Time to wait if no package is in the queue before returning
*
* \returns
*/
bool getLatestProduct(std::unique_ptr<URPackage<HeaderT>>& product, std::chrono::milliseconds timeout)
{
return queue_.waitDequeTimed(product, timeout);
}
private:
IProducer<HeaderT>& producer_;
IConsumer<_package_type>* consumer_;
std::string name_;
INotifier& notifier_;
moodycamel::BlockingReaderWriterQueue<std::unique_ptr<_package_type>> queue_;
std::atomic<bool> running_;
std::thread pThread_, cThread_;
void runProducer()
{
LOG_DEBUG("Starting up producer");
std::ifstream realtime_file("/sys/kernel/realtime", std::ios::in);
bool has_realtime;
realtime_file >> has_realtime;
if (has_realtime)
{
const int max_thread_priority = sched_get_priority_max(SCHED_FIFO);
if (max_thread_priority != -1)
{
// We'll operate on the currently running thread.
pthread_t this_thread = pthread_self();
// struct sched_param is used to store the scheduling priority
struct sched_param params;
// We'll set the priority to the maximum.
params.sched_priority = max_thread_priority;
int ret = pthread_setschedparam(this_thread, SCHED_FIFO, &params);
if (ret != 0)
{
LOG_ERROR("Unsuccessful in setting producer thread realtime priority. Error code: %d", ret);
}
// Now verify the change in thread priority
int policy = 0;
ret = pthread_getschedparam(this_thread, &policy, &params);
if (ret != 0)
{
std::cout << "Couldn't retrieve real-time scheduling paramers" << std::endl;
}
// Check the correct policy was applied
if (policy != SCHED_FIFO)
{
LOG_ERROR("Producer thread: Scheduling is NOT SCHED_FIFO!");
}
else
{
LOG_INFO("Producer thread: SCHED_FIFO OK");
}
// Print thread scheduling priority
LOG_INFO("Thread priority is %d", params.sched_priority);
}
else
{
LOG_ERROR("Could not get maximum thread priority for producer thread");
}
}
else
{
LOG_WARN("No realtime capabilities found. Consider using a realtime system for better performance");
}
std::vector<std::unique_ptr<_package_type>> products;
while (running_)
{
if (!producer_.tryGet(products))
{
break;
}
for (auto& p : products)
{
if (!queue_.tryEnqueue(std::move(p)))
{
LOG_ERROR("Pipeline producer overflowed! <%s>", name_.c_str());
}
}
products.clear();
}
producer_.teardownProducer();
LOG_DEBUG("Pipeline producer ended! <%s>", name_.c_str());
running_ = false;
notifier_.stopped(name_);
}
void runConsumer()
{
consumer_->setupConsumer();
std::unique_ptr<_package_type> product;
while (running_)
{
// timeout was chosen because we should receive messages
// at roughly 125hz (every 8ms) and have to update
// the controllers (i.e. the consumer) with *at least* 125Hz
// So we update the consumer more frequently via onTimeout
if (!queue_.waitDequeTimed(product, std::chrono::milliseconds(8)))
{
consumer_->onTimeout();
continue;
}
if (!consumer_->consume(std::move(product)))
break;
}
consumer_->teardownConsumer();
LOG_DEBUG("Pipeline consumer ended! <%s>", name_.c_str());
running_ = false;
notifier_.stopped(name_);
}
};
} // namespace comm
} // namespace ur_driver

127
ur_robot_driver/include/ur_robot_driver/comm/producer.h Normal file
View File

@@ -0,0 +1,127 @@
/*
* Copyright 2019, FZI Forschungszentrum Informatik (templating)
*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#pragma once
#include <chrono>
#include "ur_robot_driver/comm/pipeline.h"
#include "ur_robot_driver/comm/parser.h"
#include "ur_robot_driver/comm/stream.h"
#include "ur_robot_driver/comm/package.h"
#include "ur_robot_driver/exceptions.h"
namespace ur_driver
{
namespace comm
{
/*!
* \brief A general producer for URPackages. Implements funcionality to produce packages by
* reading and parsing from a byte stream.
*
* @tparam HeaderT Header type of packages to produce.
*/
template <typename HeaderT>
class URProducer : public IProducer<HeaderT>
{
private:
URStream<HeaderT>& stream_;
Parser<HeaderT>& parser_;
std::chrono::seconds timeout_;
public:
/*!
* \brief Creates a URProducer object, registering a stream and a parser.
*
* \param stream The stream to read from
* \param parser The parser to use to interpret received byte information
*/
URProducer(URStream<HeaderT>& stream, Parser<HeaderT>& parser) : stream_(stream), parser_(parser), timeout_(1)
{
}
/*!
* \brief Triggers the stream to connect to the robot.
*/
void setupProducer()
{
timeval tv;
tv.tv_sec = 1;
tv.tv_usec = 0;
stream_.setReceiveTimeout(tv);
if (!stream_.connect())
{
throw UrException("Failed to connect to robot. Please check if the robot is booted and connected.");
}
}
/*!
* \brief Tears down the producer. Currently no special handling needed.
*/
void teardownProducer()
{
stopProducer();
}
/*!
* \brief Stops the producer. Currently no functionality needed.
*/
void stopProducer()
{
}
/*!
* \brief Attempts to read byte stream from the robot and parse it as a URPackage.
*
* \param products Unique pointer to hold the produced package
*
* \returns Success of reading and parsing the package
*/
bool tryGet(std::vector<std::unique_ptr<URPackage<HeaderT>>>& products)
{
// 4KB should be enough to hold any packet received from UR
uint8_t buf[4096];
size_t read = 0;
// expoential backoff reconnects
while (true)
{
if (stream_.read(buf, sizeof(buf), read))
{
// reset sleep amount
timeout_ = std::chrono::seconds(1);
break;
}
if (stream_.closed())
return false;
LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count());
std::this_thread::sleep_for(timeout_);
if (stream_.connect())
continue;
auto next = timeout_ * 2;
if (next <= std::chrono::seconds(120))
timeout_ = next;
}
BinParser bp(buf, read);
return parser_.parse(bp, products);
}
};
} // namespace comm
} // namespace ur_driver

141
ur_robot_driver/include/ur_robot_driver/comm/reverse_interface.h Normal file
View File

@@ -0,0 +1,141 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-04-11
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_REVERSE_INTERFACE_H_INCLUDED
#define UR_RTDE_DRIVER_REVERSE_INTERFACE_H_INCLUDED
#include "ur_robot_driver/comm/server.h"
#include "ur_robot_driver/types.h"
#include <cstring>
#include <endian.h>
namespace ur_driver
{
namespace comm
{
/*!
* \brief The ReverseInterface class handles communication to the robot. It starts a server and
* waits for the robot to connect via its URCaps program.
*/
class ReverseInterface
{
public:
ReverseInterface() = delete;
/*!
* \brief Creates a ReverseInterface object including a URServer.
*
* \param port Port the Server is started on
*/
ReverseInterface(uint32_t port) : server_(port)
{
if (!server_.bind())
{
throw std::runtime_error("Could not bind to server");
}
if (!server_.accept())
{
throw std::runtime_error("Failed to accept robot connection");
}
}
/*!
* \brief Disconnects possible clients so the reverse interface object can be safely destroyed.
*/
~ReverseInterface()
{
server_.disconnectClient();
}
/*!
* \brief Writes needed information to the robot to be read by the URCaps program.
*
* \param positions A vector of joint position targets for the robot
* \param type An additional integer used to command the program to end when needed
*
* \returns True, if the write was performed successfully, false otherwise.
*/
bool write(const vector6d_t* positions, const int32_t type = 2)
{
uint8_t buffer[sizeof(uint32_t) * 7];
uint8_t* b_pos = buffer;
int32_t val = htobe32(type);
b_pos += append(b_pos, val);
if (positions != nullptr)
{
for (auto const& pos : *positions)
{
int32_t val = static_cast<int32_t>(pos * MULT_JOINTSTATE);
val = htobe32(val);
b_pos += append(b_pos, val);
}
}
size_t written;
return server_.write(buffer, sizeof(buffer), written);
}
/*!
* \brief Reads a keepalive signal from the robot.
*
* \returns The received keepalive string or the empty string, if nothing was received
*/
std::string readKeepalive()
{
size_t buf_len = 16;
char buffer[buf_len];
bool read_successful = server_.readLine(buffer, buf_len);
if (read_successful)
{
return std::string(buffer);
}
else
{
return std::string("");
}
}
private:
URServer server_;
static const int32_t MULT_JOINTSTATE = 1000000;
template <typename T>
size_t append(uint8_t* buffer, T& val)
{
size_t s = sizeof(T);
std::memcpy(buffer, &val, s);
return s;
}
};
} // namespace comm
} // namespace ur_driver
#endif // UR_RTDE_DRIVER_REVERSE_INTERFACE_H_INCLUDED

138
ur_robot_driver/include/ur_robot_driver/comm/script_sender.h Normal file
View File

@@ -0,0 +1,138 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-05-22
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_SCRIPT_SENDER_H_INCLUDED
#define UR_RTDE_DRIVER_SCRIPT_SENDER_H_INCLUDED
#include "ur_robot_driver/comm/server.h"
#include "ur_robot_driver/log.h"
namespace ur_driver
{
namespace comm
{
/*!
* \brief The ScriptSender class starts a URServer for a robot to connect to and waits for a
* request to receive a program. This program is then delivered to the requesting robot.
*/
class ScriptSender
{
public:
ScriptSender() = delete;
/*!
* \brief Creates a ScriptSender object, including a new URServer and not yet started thread.
*
* \param port Port to start the server on
* \param program Program to send to the robot upon request
*/
ScriptSender(uint32_t port, const std::string& program) : server_(port), script_thread_(), program_(program)
{
if (!server_.bind())
{
throw std::runtime_error("Could not bind to server");
}
}
/*!
* \brief Starts the thread that handles program requests by a robot.
*/
void start()
{
script_thread_ = std::thread(&ScriptSender::runScriptSender, this);
}
private:
URServer server_;
std::thread script_thread_;
std::string program_;
const std::string PROGRAM_REQUEST_ = std::string("request_program\n");
void runScriptSender()
{
while (true)
{
if (!server_.accept())
{
throw std::runtime_error("Failed to accept robot connection");
}
if (requestRead())
{
LOG_INFO("Robot requested program");
sendProgram();
}
server_.disconnectClient();
}
}
bool requestRead()
{
size_t buf_len = 1024;
char buffer[buf_len];
bool read_successful = server_.readLine(buffer, buf_len);
if (read_successful)
{
if (std::string(buffer) == PROGRAM_REQUEST_)
{
return true;
}
else
{
LOG_WARN("Received unexpected message on script request port ");
}
}
else
{
LOG_WARN("Could not read on script request port");
}
return false;
}
void sendProgram()
{
size_t len = program_.size();
const uint8_t* data = reinterpret_cast<const uint8_t*>(program_.c_str());
size_t written;
if (server_.write(data, len, written))
{
LOG_INFO("Sent program to robot");
}
else
{
LOG_ERROR("Could not send program to robot");
}
}
};
} // namespace comm
} // namespace ur_driver
#endif // UR_RTDE_DRIVER_SCRIPT_SENDER_H_INCLUDED

104
ur_robot_driver/include/ur_robot_driver/comm/server.h Normal file
View File

@@ -0,0 +1,104 @@
/*
* Copyright 2019, FZI Forschungszentrum Informatik (refactor)
*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#pragma once
#include <netdb.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <atomic>
#include <cstdlib>
#include <mutex>
#include <string>
#include "ur_robot_driver/comm/tcp_socket.h"
namespace ur_driver
{
namespace comm
{
#define MAX_SERVER_BUF_LEN 50
/*!
* \brief The URServer class abstracts communication with the robot. It opens a socket on a given
* port and waits for a robot to connect, at which point two way communication can be established.
*/
class URServer : private comm::TCPSocket
{
private:
int port_;
comm::TCPSocket client_;
protected:
virtual bool open(int socket_fd, struct sockaddr* address, size_t address_len);
public:
/*!
* \brief Creates a URServer object with a given port.
*
* \param port The port to open a socket on
*/
URServer(int port);
/*!
* \brief Closes the socket to allow for destruction of the object.
*/
~URServer();
/*!
* \brief Getter for the server IP.
*
* \returns The IP of the server
*/
std::string getIP();
/*!
* \brief Binds to server's port, setting up a socket if possible.
*
* \returns Success of setting up the socket
*/
bool bind();
/*!
* \brief Waits for a robot to connect to the socket.
*
* \returns True, if a robot successfully connected, false otherwise.
*/
bool accept();
/*!
* \brief Triggers a disconnect of the currently connected robot.
*/
void disconnectClient();
/*!
* \brief Reads the byte-stream from the robot to the next linebreak.
*
* \param buffer The buffer to write the received bytes to
* \param buf_len Size of the buffer
*
* \returns True if a successful read occurred, false otherwise
*/
bool readLine(char* buffer, size_t buf_len);
/*!
* \brief Writes a buffer to the robot.
*
* \param buf The buffer to write from
* \param buf_len The length to write
* \param written A reference used to indicate how many bytes were written
*
* \returns Success of the write
*/
bool write(const uint8_t* buf, size_t buf_len, size_t& written);
};
} // namespace comm
} // namespace ur_driver

70
ur_robot_driver/include/ur_robot_driver/comm/shell_consumer.h Normal file
View File

@@ -0,0 +1,70 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_SHELL_CONSUMER_H_INCLUDED
#define UR_RTDE_DRIVER_SHELL_CONSUMER_H_INCLUDED
#include "ur_robot_driver/log.h"
#include "ur_robot_driver/comm/pipeline.h"
#include "ur_robot_driver/comm/package.h"
namespace ur_driver
{
namespace comm
{
/*!
* \brief The ShellConsumer class is a simple consumer that writes a readable representation to
* the logging info channel.
*
* @tparam HeaderT Header type of the packages to consume
*/
template <typename HeaderT>
class ShellConsumer : public IConsumer<URPackage<HeaderT>>
{
public:
ShellConsumer() = default;
virtual ~ShellConsumer() = default;
/*!
* \brief Consumes a package, writing a human readable representation to the logging.
*
* \param product The package to consume
*
* \returns True if the output was successful
*/
virtual bool consume(std::shared_ptr<URPackage<HeaderT>> product)
{
LOG_INFO("%s", product->toString().c_str());
return true;
}
private:
/* data */
};
} // namespace comm
} // namespace ur_driver
#endif // ifndef UR_RTDE_DRIVER_SHELL_CONSUMER_H_INCLUDED

158
ur_robot_driver/include/ur_robot_driver/comm/stream.h Normal file
View File

@@ -0,0 +1,158 @@
/*
* Copyright 2019, FZI Forschungszentrum Informatik (templating)
*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#pragma once
#include <netdb.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <atomic>
#include <mutex>
#include <string>
#include "ur_robot_driver/log.h"
#include "ur_robot_driver/comm/tcp_socket.h"
namespace ur_driver
{
namespace comm
{
/*!
* \brief The stream is an abstraction of the TCPSocket that offers reading a full UR data package
* out of the socket. This means, it has to have some knowledge about the package structure to
* peek at the field defining the package length. This is why it is templated with the package
* header type.
*/
template <typename HeaderT>
class URStream : public TCPSocket
{
public:
/*!
* \brief Creates a new URStream object. Note, that this does not immediately open the socket,
* that has to be done separately by calling the connect() function.
*
* \param host IP address of the remote host
* \param port Port on which the socket shall be connected
*/
URStream(const std::string& host, int port) : host_(host), port_(port)
{
}
/*!
* \brief Connects to the configured socket.
*
* \returns True on success, false if connection could not be established
*/
bool connect()
{
return TCPSocket::setup(host_, port_);
}
/*!
* \brief Disconnects from the configured socket.
*/
void disconnect()
{
LOG_DEBUG("Disconnecting from %s:%d", host_.c_str(), port_);
TCPSocket::close();
}
/*!
* \brief Returns whether the underlying socket is currently closed
*/
bool closed()
{
return getState() == SocketState::Closed;
}
/*!
* \brief Reads a full UR package out of a socket. For this, it looks into the package and reads
* the byte length from the socket directly. It returns as soon as all bytes for the package are
* read from the socket.
*
* \param[out] buf The byte buffer where the content shall be stored
* \param[in] buf_len Number of bytes allocated for the buffer
* \param[out] read Number of bytes actually read from the socket
*
* \returns True on success, false on error, e.g. the buffer is smaller than the package size
*/
bool read(uint8_t* buf, const size_t buf_len, size_t& read);
/*!
* \brief Writes directly to the underlying socket (with a mutex guard)
*
* \param[in] buf Byte stream that should be sent
* \param[in] buf_len Number of bytes in buffer
* \param[out] written Number of bytes actually written to the socket
*
* \returns False if sending went wrong
*/
bool write(const uint8_t* buf, const size_t buf_len, size_t& written);
protected:
virtual bool open(int socket_fd, struct sockaddr* address, size_t address_len)
{
return ::connect(socket_fd, address, address_len) == 0;
}
private:
std::string host_;
int port_;
std::mutex write_mutex_, read_mutex_;
};
template <typename HeaderT>
bool URStream<HeaderT>::write(const uint8_t* buf, const size_t buf_len, size_t& written)
{
std::lock_guard<std::mutex> lock(write_mutex_);
return TCPSocket::write(buf, buf_len, written);
}
template <typename HeaderT>
bool URStream<HeaderT>::read(uint8_t* buf, const size_t buf_len, size_t& total)
{
std::lock_guard<std::mutex> lock(read_mutex_);
bool initial = true;
uint8_t* buf_pos = buf;
size_t remainder = sizeof(typename HeaderT::_package_size_type);
size_t read = 0;
while (remainder > 0 && TCPSocket::read(buf_pos, remainder, read))
{
TCPSocket::setOptions(getSocketFD());
if (initial)
{
remainder = HeaderT::getPackageLength(buf);
if (remainder >= (buf_len - sizeof(typename HeaderT::_package_size_type)))
{
LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len);
return false;
}
initial = false;
}
total += read;
buf_pos += read;
remainder -= read;
}
return remainder == 0;
}
} // namespace comm
} // namespace ur_driver

150
ur_robot_driver/include/ur_robot_driver/comm/tcp_socket.h Normal file
View File

@@ -0,0 +1,150 @@
/*
* Copyright 2019, FZI Forschungszentrum Informatik (refactor)
*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#pragma once
#include <netdb.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <atomic>
#include <mutex>
#include <string>
#include <memory>
namespace ur_driver
{
namespace comm
{
/*!
* \brief State the socket can be in
*/
enum class SocketState
{
Invalid, ///< Socket is initialized or setup failed
Connected, ///< Socket is connected and ready to use
Disconnected, ///< Socket is disconnected and cannot be used
Closed ///< Connection to socket got closed
};
/*!
* \brief Class for TCP socket abstraction
*/
class TCPSocket
{
private:
std::atomic<int> socket_fd_;
std::atomic<SocketState> state_;
std::unique_ptr<timeval> recv_timeout_;
protected:
virtual bool open(int socket_fd, struct sockaddr* address, size_t address_len)
{
return false;
}
virtual void setOptions(int socket_fd);
bool setup(std::string& host, int port);
public:
/*!
* \brief Creates a TCPSocket object
*/
TCPSocket();
virtual ~TCPSocket();
/*!
* \brief Getter for the state of the socket.
*
* \returns Returns the current state of the socket
*/
SocketState getState()
{
return state_;
}
/*!
* \brief Getter for the file descriptor of the socket.
*
* \returns The file descriptor of the socket
*/
int getSocketFD()
{
return socket_fd_;
}
/*!
* \brief Setter for the file descriptor of the socket.
*
* \param socket_fd The new value
*
* \returns False, if the socket is in state connected
*/
bool setSocketFD(int socket_fd);
/*!
* \brief Determines the IP address of the local machine
*
* \returns The IP address of the local machine.
*/
std::string getIP() const;
/*!
* \brief Reads one byte from the socket
*
* \param[out] character Target buffer
*
* \returns True on success, false otherwise
*/
bool read(char* character);
/*!
* \brief Reads data from the socket
*
* \param[out] buf Buffer where the data shall be stored
* \param[in] buf_len Number of bytes allocated for the buffer
* \param[out] read Number of bytes actually read
*
* \returns True on success, false otherwise
*/
bool read(uint8_t* buf, const size_t buf_len, size_t& read);
/*!
* \brief Writes to the socket
*
* \param[in] buf Buffer of bytes to write
* \param[in] buf_len Number of bytes in the buffer
* \param[out] written Number of bytes actually written
*
* \returns True on success, false otherwise
*/
bool write(const uint8_t* buf, const size_t buf_len, size_t& written);
/*!
* \brief Closes the connection to the socket.
*/
void close();
/*!
* \brief Setup Receive timeout used for this socket.
*
* \param timeout Timeout used for setting things up
*/
void setReceiveTimeout(const timeval& timeout);
};
} // namespace comm
} // namespace ur_driver

104
ur_robot_driver/include/ur_robot_driver/exceptions.h Normal file
View File

@@ -0,0 +1,104 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-06-11
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_EXCEPTIONS_H_INCLUDED
#define UR_RTDE_DRIVER_EXCEPTIONS_H_INCLUDED
#include <stdexcept>
#include <sstream>
namespace ur_driver
{
/*!
* \brief Our base class for exceptions. Specialized exceptions should inherit from those.
*/
class UrException : virtual public std::runtime_error
{
public:
explicit UrException() : std::runtime_error("")
{
}
explicit UrException(const std::string& what_arg) : std::runtime_error(what_arg)
{
}
explicit UrException(const char* what_arg) : std::runtime_error(what_arg)
{
}
virtual ~UrException() = default;
private:
/* data */
};
/*!
* \brief A specialized exception representing detection of a not supported UR control version.
*/
class VersionMismatch : public UrException
{
public:
explicit VersionMismatch() : VersionMismatch("", 0, 0)
{
}
explicit VersionMismatch(const std::string& text, const uint32_t version_req, const uint32_t version_actual)
: std::runtime_error(text)
{
version_required_ = version_req;
version_actual_ = version_actual;
std::stringstream ss;
ss << text << "(Required version: " << version_required_ << ", actual version: " << version_actual_ << ")";
text_ = ss.str();
}
virtual ~VersionMismatch() = default;
virtual const char* what() const noexcept override
{
return text_.c_str();
}
private:
uint32_t version_required_;
uint32_t version_actual_;
std::string text_;
};
/*!
* \brief A specialized exception representing that communication to the tool is not possible.
*/
class ToolCommNotAvailable : public VersionMismatch
{
public:
explicit ToolCommNotAvailable() : ToolCommNotAvailable("", 0, 0)
{
}
explicit ToolCommNotAvailable(const std::string& text, const uint32_t version_req, const uint32_t version_actual)
: std::runtime_error(text), VersionMismatch(text, version_req, version_actual)
{
}
};
} // namespace ur_driver
#endif // ifndef UR_RTDE_DRIVER_EXCEPTIONS_H_INCLUDED

39
ur_robot_driver/include/ur_robot_driver/log.h Normal file
View File

@@ -0,0 +1,39 @@
/*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#pragma once
#include <inttypes.h>
#ifdef ROS_BUILD
#include <ros/ros.h>
#define LOG_DEBUG ROS_DEBUG
#define LOG_WARN ROS_WARN
#define LOG_INFO ROS_INFO
#define LOG_ERROR ROS_ERROR
#define LOG_FATAL ROS_FATAL
#else
#define LOG_DEBUG(format, ...) printf("[DEBUG]: " format "\n", ##__VA_ARGS__)
#define LOG_WARN(format, ...) printf("[WARNING]: " format "\n", ##__VA_ARGS__)
#define LOG_INFO(format, ...) printf("[INFO]: " format "\n", ##__VA_ARGS__)
#define LOG_ERROR(format, ...) printf("[ERROR]: " format "\n", ##__VA_ARGS__)
#define LOG_FATAL(format, ...) printf("[FATAL]: " format "\n", ##__VA_ARGS__)
#endif

84
ur_robot_driver/include/ur_robot_driver/primary/package_header.h Normal file
View File

@@ -0,0 +1,84 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \author Lea Steffen steffen@fzi.de
* \date 2019-04-01
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_PRIMARY_INTERFACE_H_INCLUDED
#define UR_RTDE_DRIVER_PRIMARY_INTERFACE_H_INCLUDED
#include <inttypes.h>
#include <cstddef>
#include <endian.h>
#include "ur_robot_driver/types.h"
namespace ur_driver
{
namespace primary_interface
{
static const int UR_PRIMARY_PORT = 30001;
static const int UR_SECONDARY_PORT = 30002;
/*!
* \brief Possible RobotPackage types
*/
enum class RobotPackageType : int8_t
{
DISCONNECT = -1,
ROBOT_STATE = 16,
ROBOT_MESSAGE = 20,
HMC_MESSAGE = 22,
MODBUS_INFO_MESSAGE = 5,
SAFETY_SETUP_BROADCAST_MESSAGE = 23,
SAFETY_COMPLIANCE_TOLERANCES_MESSAGE = 24,
PROGRAM_STATE_MESSAGE = 25
};
/*!
* \brief This class represents the header for primary packages.
*/
class PackageHeader
{
public:
PackageHeader() = default;
virtual ~PackageHeader() = default;
using _package_size_type = int32_t;
/*!
* \brief Reads a buffer, interpreting the next bytes as the size of the contained package.
*
* \param buf The given byte stream containing a serialized package
*
* \returns The size of the given serialized package
*/
static size_t getPackageLength(uint8_t* buf)
{
return be32toh(*(reinterpret_cast<_package_size_type*>(buf)));
}
};
} // namespace primary_interface
} // namespace ur_driver
#endif // ifndef UR_RTDE_DRIVER_PRIMARY_INTERFACE_H_INCLUDED

79
ur_robot_driver/include/ur_robot_driver/primary/primary_package.h Normal file
View File

@@ -0,0 +1,79 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Lea Steffen steffen@fzi.de
* \date 2019-04-01
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_PRIMARY_PACKAGE_H_INCLUDED
#define UR_RTDE_DRIVER_PRIMARY_PACKAGE_H_INCLUDED
#include "ur_robot_driver/primary/package_header.h"
#include "ur_robot_driver/comm/package.h"
namespace ur_driver
{
namespace primary_interface
{
/*!
* \brief The PrimaryPackage is solely an abstraction level.
* It inherits form the URPackage and is also a parent class for primary_interface::RobotMessage,
* primary_interface::RobotState
*/
class PrimaryPackage : public comm::URPackage<PackageHeader>
{
public:
/*!
* \brief Creates a new PrimaryPackage object.
*/
PrimaryPackage() : buffer_length_(0)
{
}
virtual ~PrimaryPackage() = default;
/*!
* \brief Sets the attributes of the package by parsing a serialized representation of the
* package.
*
* \param bp A parser containing a serialized version of the package
*
* \returns True, if the package was parsed successfully, false otherwise
*/
virtual bool parseWith(comm::BinParser& bp);
/*!
* \brief Produces a human readable representation of the package object.
*
* \returns A string representing the object
*/
virtual std::string toString() const;
protected:
std::unique_ptr<uint8_t> buffer_;
size_t buffer_length_;
};
} // namespace primary_interface
} // namespace ur_driver
#endif /* UR_RTDE_DRIVER_PRIMARY_PACKAGE_H_INCLUDED */

186
ur_robot_driver/include/ur_robot_driver/primary/primary_parser.h Normal file
View File

@@ -0,0 +1,186 @@
/*
* Copyright 2019, FZI Forschungszentrum Informatik (refactor)
*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#pragma once
#include <vector>
#include "ur_robot_driver/comm/bin_parser.h"
#include "ur_robot_driver/comm/pipeline.h"
#include "ur_robot_driver/comm/parser.h"
#include "ur_robot_driver/primary/package_header.h"
#include "ur_robot_driver/primary/robot_state.h"
#include "ur_robot_driver/primary/robot_message.h"
//#include "ur_robot_driver/primary/robot_state/robot_mode_data.h"
#include "ur_robot_driver/primary/robot_state/kinematics_info.h"
//#include "ur_robot_driver/primary/robot_state/master_board.h"
#include "ur_robot_driver/primary/robot_message/version_message.h"
namespace ur_driver
{
namespace primary_interface
{
/*!
* \brief The primary specific parser. Interprets a given byte stream as serialized primary
* packages and parses it accordingly.
*/
class PrimaryParser : public comm::Parser<PackageHeader>
{
public:
PrimaryParser() = default;
virtual ~PrimaryParser() = default;
/*!
* \brief Uses the given BinParser to create package objects from the contained serialization.
*
* \param bp A BinParser holding one or more serialized primary packages
* \param results A vector of pointers to created primary package objects
*
* \returns True, if the byte stream could successfully be parsed as primary packages, false
* otherwise
*/
bool parse(comm::BinParser& bp, std::vector<std::unique_ptr<comm::URPackage<PackageHeader>>>& results)
{
int32_t packet_size;
RobotPackageType type;
bp.parse(packet_size);
bp.parse(type);
switch (type)
{
case RobotPackageType::ROBOT_STATE:
{
while (!bp.empty())
{
if (!bp.checkSize(sizeof(uint32_t)))
{
LOG_ERROR("Failed to read sub-package length, there's likely a parsing error");
return false;
}
uint32_t sub_size = bp.peek<uint32_t>();
if (!bp.checkSize(static_cast<size_t>(sub_size)))
{
LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size);
return false;
}
// deconstruction of a sub parser will increment the position of the parent parser
comm::BinParser sbp(bp, sub_size);
sbp.consume(sizeof(sub_size));
RobotStateType type;
sbp.parse(type);
std::unique_ptr<PrimaryPackage> packet(stateFromType(type));
if (packet == nullptr)
{
sbp.consume();
// TODO: create robot state type here
continue;
}
if (!packet->parseWith(sbp))
{
LOG_ERROR("Sub-package parsing of type %d failed!", static_cast<int>(type));
return false;
}
results.push_back(std::move(packet));
if (!sbp.empty())
{
LOG_ERROR("Sub-package of type %d was not parsed completely!", static_cast<int>(type));
sbp.debug();
return false;
}
}
break;
}
case RobotPackageType::ROBOT_MESSAGE:
{
uint64_t timestamp;
uint8_t source;
RobotMessagePackageType message_type;
bp.parse(timestamp);
bp.parse(source);
bp.parse(message_type);
std::unique_ptr<PrimaryPackage> packet(messageFromType(message_type, timestamp, source));
if (!packet->parseWith(bp))
{
LOG_ERROR("Package parsing of type %d failed!", static_cast<int>(message_type));
return false;
}
results.push_back(std::move(packet));
return true;
break;
}
default:
{
LOG_DEBUG("Invalid robot package type recieved: %u", static_cast<uint8_t>(type));
bp.consume();
return true;
}
}
return true;
}
private:
RobotState* stateFromType(RobotStateType type)
{
switch (type)
{
/*case robot_state_type::ROBOT_MODE_DATA:
// SharedRobotModeData* rmd = new SharedRobotModeData();
//return new rmd;
case robot_state_type::MASTERBOARD_DATA:
return new MBD;*/
case RobotStateType::KINEMATICS_INFO:
return new KinematicsInfo(type);
default:
return new RobotState(type);
}
}
RobotMessage* messageFromType(RobotMessagePackageType type, uint64_t timestamp, uint8_t source)
{
switch (type)
{
/*case robot_state_type::ROBOT_MODE_DATA:
// SharedRobotModeData* rmd = new SharedRobotModeData();
//return new rmd;
case robot_state_type::MASTERBOARD_DATA:
return new MBD;*/
case RobotMessagePackageType::ROBOT_MESSAGE_VERSION:
return new VersionMessage(timestamp, source);
default:
return new RobotMessage(timestamp, source);
}
}
};
} // namespace primary_interface
} // namespace ur_driver

95
ur_robot_driver/include/ur_robot_driver/primary/robot_message.h Normal file
View File

@@ -0,0 +1,95 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Lea Steffen steffen@fzi.de
* \date 2019-04-01
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_ROBOT_MESSAGE_H_INCLUDED
#define UR_RTDE_DRIVER_ROBOT_MESSAGE_H_INCLUDED
#include "ur_robot_driver/primary/primary_package.h"
namespace ur_driver
{
namespace primary_interface
{
/*!
* \brief Possible RobotMessage types
*/
enum class RobotMessagePackageType : uint8_t
{
ROBOT_MESSAGE_TEXT = 0,
ROBOT_MESSAGE_PROGRAM_LABEL = 1,
PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2,
ROBOT_MESSAGE_VERSION = 3,
ROBOT_MESSAGE_SAFETY_MODE = 5,
ROBOT_MESSAGE_ERROR_CODE = 6,
ROBOT_MESSAGE_KEY = 7,
ROBOT_MESSAGE_REQUEST_VALUE = 9,
ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10
};
/*!
* \brief The RobotMessage class is a parent class for the different received robot messages.
*/
class RobotMessage : public PrimaryPackage
{
public:
/*!
* \brief Creates a new RobotMessage object to be filled from a package.
*
* \param timestamp Timestamp of the package
* \param source The package's source
*/
RobotMessage(const uint64_t timestamp, const uint8_t source) : timestamp_(timestamp), source_(source)
{
}
virtual ~RobotMessage() = default;
/*!
* \brief Sets the attributes of the package by parsing a serialized representation of the
* package.
*
* \param bp A parser containing a serialized version of the package
*
* \returns True, if the package was parsed successfully, false otherwise
*/
virtual bool parseWith(comm::BinParser& bp);
/*!
* \brief Produces a human readable representation of the package object.
*
* \returns A string representing the object
*/
virtual std::string toString() const;
uint64_t timestamp_;
uint8_t source_;
RobotMessagePackageType message_type_;
};
} // namespace primary_interface
} // namespace ur_driver
#endif /* UR_RTDE_DRIVER_ROBOT_MESSAGE_H_INCLUDED */

83
ur_robot_driver/include/ur_robot_driver/primary/robot_message/version_message.h Normal file
View File

@@ -0,0 +1,83 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-08
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_VERSION_MESSAGE_H_INCLUDED
#define UR_RTDE_DRIVER_VERSION_MESSAGE_H_INCLUDED
#include "ur_robot_driver/primary/robot_message.h"
namespace ur_driver
{
namespace primary_interface
{
/*!
* \brief The VersionMessage class handles the version messages sent via the primary UR interface.
*/
class VersionMessage : public RobotMessage
{
public:
VersionMessage() = delete;
/*!
* \brief Creates a new VersionMessage object to be filled from a package.
*
* \param timestamp Timestamp of the package
* \param source The package's source
*/
VersionMessage(uint64_t timestamp, uint8_t source) : RobotMessage(timestamp, source)
{
}
virtual ~VersionMessage() = default;
/*!
* \brief Sets the attributes of the package by parsing a serialized representation of the
* package.
*
* \param bp A parser containing a serialized version of the package
*
* \returns True, if the package was parsed successfully, false otherwise
*/
virtual bool parseWith(comm::BinParser& bp);
/*!
* \brief Produces a human readable representation of the package object.
*
* \returns A string representing the object
*/
virtual std::string toString() const;
int8_t project_name_length_;
std::string project_name_;
uint8_t major_version_;
uint8_t minor_version_;
int32_t svn_version_;
int32_t build_number_;
std::string build_date_;
};
} // namespace primary_interface
} // namespace ur_driver
#endif // ifndef UR_RTDE_DRIVER_VERSION_MESSAGE_H_INCLUDED

107
ur_robot_driver/include/ur_robot_driver/primary/robot_state.h Normal file
View File

@@ -0,0 +1,107 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Lea Steffen steffen@fzi.de
* \date 2019-04-01
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_ROBOT_STATE_H_INCLUDED
#define UR_RTDE_DRIVER_ROBOT_STATE_H_INCLUDED
#include <sstream>
#include "ur_robot_driver/primary/primary_package.h"
#include "ur_robot_driver/primary/package_header.h"
namespace ur_driver
{
namespace primary_interface
{
/*!
* \brief Possible RobotState types
*/
enum class RobotStateType : uint8_t
{
ROBOT_MODE_DATA = 0,
JOINT_DATA = 1,
TOOL_DATA = 2,
MASTERBOARD_DATA = 3,
CARTESIAN_INFO = 4,
KINEMATICS_INFO = 5,
CONFIGURATION_DATA = 6,
FORCE_MODE_DATA = 7,
ADDITIONAL_INFO = 8,
CALIBRATION_DATA = 9
};
/*!
* \brief Base class for a RobotState data packages will be used directly.
*/
class RobotState : public PrimaryPackage
{
public:
RobotState() = delete;
/*!
* \brief Creates a new RobotState object, setting the type of state message.
*
* \param type The type of state message
*/
RobotState(const RobotStateType type) : state_type_(type)
{
}
virtual ~RobotState() = default;
/*!
* \brief Sets the attributes of the package by parsing a serialized representation of the
* package.
*
* \param bp A parser containing a serialized version of the package
*
* \returns True, if the package was parsed successfully, false otherwise
*/
virtual bool parseWith(comm::BinParser& bp)
{
return PrimaryPackage::parseWith(bp);
}
/*!
* \brief Produces a human readable representation of the package object.
*
* \returns A string representing the object
*/
virtual std::string toString() const
{
std::stringstream ss;
ss << "Type: " << static_cast<int>(state_type_) << std::endl;
ss << PrimaryPackage::toString();
return ss.str();
}
private:
RobotStateType state_type_;
};
} // namespace primary_interface
} // namespace ur_driver
#endif /* UR_RTDE_DRIVER_ROBOT_STATE_H_INCLUDED */

91
ur_robot_driver/include/ur_robot_driver/primary/robot_state/kinematics_info.h Normal file
View File

@@ -0,0 +1,91 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-08
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_KINEMATICS_INFO_H_INCLUDED
#define UR_RTDE_DRIVER_KINEMATICS_INFO_H_INCLUDED
#include "ur_robot_driver/types.h"
#include "ur_robot_driver/primary/robot_state.h"
namespace ur_driver
{
namespace primary_interface
{
/*!
* \brief This messages contains information about the robot's calibration. The DH parameters are
* a combination between the perfect model parameters and the correction deltas as noted in the
* configuration files on the robot controller.
*/
class KinematicsInfo : public RobotState
{
public:
KinematicsInfo() = delete;
/*!
* \brief Creates a new KinematicsInfo object.
*
* \param type The type of RobotState message received
*/
KinematicsInfo(const RobotStateType type) : RobotState(type)
{
}
virtual ~KinematicsInfo() = default;
/*!
* \brief Sets the attributes of the package by parsing a serialized representation of the
* package.
*
* \param bp A parser containing a serialized version of the package
*
* \returns True, if the package was parsed successfully, false otherwise
*/
virtual bool parseWith(comm::BinParser& bp);
/*!
* \brief Produces a human readable representation of the package object.
*
* \returns A string representing the object
*/
virtual std::string toString() const;
/*!
* \brief Calculates a hash value of the parameters to allow for identification of a calibration.
*
* \returns A hash value of the parameters
*/
std::string toHash() const;
vector6uint32_t checksum_;
vector6d_t dh_theta_;
vector6d_t dh_a_;
vector6d_t dh_d_;
vector6d_t dh_alpha_;
uint32_t calibration_status_; // According to the docs this should be uint8_t, but then I have 3 bytes left.
};
} // namespace primary_interface
} // namespace ur_driver
#endif // ifndef UR_RTDE_DRIVER_KINEMATICS_INFO_H_INCLUDED

117
ur_robot_driver/include/ur_robot_driver/primary/robot_state/master_board.h Normal file
View File

@@ -0,0 +1,117 @@
/*
* Copyright 2019, FZI Forschungszentrum Informatik (refactor)
*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#pragma once
#include <inttypes.h>
#include <bitset>
#include <cstddef>
#include "ur_robot_driver/primary/robot_state.h"
namespace ur_driver
{
namespace primary_interface
{
class SharedMasterBoardData
{
public:
virtual bool parseWith(BinParser& bp);
int8_t analog_input_range0;
int8_t analog_input_range1;
double analog_input0;
double analog_input1;
int8_t analog_output_domain0;
int8_t analog_output_domain1;
double analog_output0;
double analog_output1;
float master_board_temperature;
float robot_voltage_48V;
float robot_current;
float master_IO_current;
bool euromap67_interface_installed;
// euromap fields are dynamic so don't include in SIZE
int32_t euromap_input_bits;
int32_t euromap_output_bits;
static const size_t SIZE = sizeof(double) * 4 + sizeof(int8_t) * 4 + sizeof(float) * 4 + sizeof(uint8_t);
static const size_t EURO_SIZE = sizeof(int32_t) * 2;
};
class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket
{
public:
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URStatePacketConsumer& consumer);
std::bitset<10> digital_input_bits;
std::bitset<10> digital_output_bits;
uint8_t master_safety_state;
bool master_on_off_state;
// euromap fields are dynamic so don't include in SIZE
int16_t euromap_voltage;
int16_t euromap_current;
static const size_t SIZE = SharedMasterBoardData::SIZE + sizeof(int16_t) * 2 + sizeof(uint8_t) * 2;
static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE + sizeof(int16_t) * 2;
};
class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket
{
public:
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URStatePacketConsumer& consumer);
std::bitset<18> digital_input_bits;
std::bitset<18> digital_output_bits;
uint8_t safety_mode;
bool in_reduced_mode;
// euromap fields are dynamic so don't include in SIZE
float euromap_voltage;
float euromap_current;
static const size_t SIZE = SharedMasterBoardData::SIZE + sizeof(int32_t) * 2 + sizeof(uint8_t) * 2 +
sizeof(uint32_t); // UR internal field we skip
static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE + sizeof(float) * 2;
};
class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1
{
public:
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URStatePacketConsumer& consumer);
uint8_t operational_mode_selector_input;
uint8_t three_position_enabling_device_input;
static const size_t SIZE = MasterBoardData_V3_0__1::SIZE + sizeof(uint8_t) * 2;
};
} // namespace primary_interface
} // namespace ur_driver

120
ur_robot_driver/include/ur_robot_driver/primary/robot_state/robot_mode_data.h Normal file
View File

@@ -0,0 +1,120 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
// Copyright 2015, 2016 Thomas Timm Andersen (original version)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-08
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIBVER_ROBOT_MODE_DATA_H_INCLUDED
#define UR_RTDE_DRIBVER_ROBOT_MODE_DATA_H_INCLUDED
#include "ur_robot_driver/primary/robot_state.h"
namespace ur_driver
{
namespace primary_interface
{
class SharedRobotModeData
{
public:
virtual bool parseWith(comm::BinParser& bp);
uint64_t timestamp;
bool physical_robot_connected;
bool real_robot_enabled;
bool robot_power_on;
bool emergency_stopped;
bool protective_stopped; // AKA security_stopped
bool program_running;
bool program_paused;
static const size_t SIZE = sizeof(uint64_t) + sizeof(uint8_t) * 6;
};
enum class robot_mode_V3_X : uint8_t
{
DISCONNECTED = 0,
CONFIRM_SAFETY = 1,
BOOTING = 2,
POWER_OFF = 3,
POWER_ON = 4,
IDLE = 5,
BACKDRIVE = 6,
RUNNING = 7,
UPDATING_FIRMWARE = 8
};
enum class robot_control_mode_V3_X : uint8_t
{
POSITION = 0,
TEACH = 1,
FORCE = 2,
TORQUE = 3
};
class RobotModeData_V3_0__1 : public SharedRobotModeData, public RobotState
{
public:
virtual bool parseWith(comm::BinParser& bp);
robot_mode_V3_X robot_mode;
robot_control_mode_V3_X control_mode;
double target_speed_fraction;
double speed_scaling;
static const size_t SIZE = SharedRobotModeData::SIZE + sizeof(uint8_t) + sizeof(robot_mode_V3_X) +
sizeof(robot_control_mode_V3_X) + sizeof(double) + sizeof(double);
static_assert(RobotModeData_V3_0__1::SIZE == 33, "RobotModeData_V3_0__1 has missmatched size");
};
class RobotModeData_V3_2 : public RobotModeData_V3_0__1
{
public:
virtual bool parseWith(comm::BinParser& bp);
// virtual bool consumeWith(URStatePacketConsumer& consumer);
double target_speed_fraction_limit;
static const size_t SIZE = RobotModeData_V3_0__1::SIZE + sizeof(double);
static_assert(RobotModeData_V3_2::SIZE == 41, "RobotModeData_V3_2 has missmatched size");
};
class RobotModeData_V3_5 : public RobotModeData_V3_2
{
public:
virtual bool parseWith(comm::BinParser& bp);
// virtual bool consumeWith(URStatePacketConsumer& consumer);
unsigned char unknown_internal_use;
static const size_t SIZE = RobotModeData_V3_2::SIZE + sizeof(unsigned char);
static_assert(RobotModeData_V3_5::SIZE == 42, "RobotModeData_V3_5 has missmatched size");
};
} // namespace primary_interface
} // namespace ur_driver
#endif // ifndef UR_RTDE_DRIBVER_ROBOT_MODE_DATA_H_INCLUDED

28
ur_robot_driver/include/ur_robot_driver/queue/LICENSE.md Normal file
View File

@@ -0,0 +1,28 @@
This license applies to all the code in this repository except that written by third
parties, namely the files in benchmarks/ext, which have their own licenses, and Jeff
Preshing's semaphore implementation (used in the blocking queue) which has a zlib
license (embedded in atomicops.h).
Simplified BSD License:
Copyright (c) 2013-2015, Cameron Desrochers
All rights reserved.
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.
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.

738
ur_robot_driver/include/ur_robot_driver/queue/atomicops.h Normal file
View File

@@ -0,0 +1,738 @@
// ©2013-2016 Cameron Desrochers.
// Distributed under the simplified BSD license (see the license file that
// should have come with this header).
// Uses Jeff Preshing's semaphore implementation (under the terms of its
// separate zlib license, embedded below).
#pragma once
// Provides portable (VC++2010+, Intel ICC 13, GCC 4.7+, and anything C++11 compliant) implementation
// of low-level memory barriers, plus a few semi-portable utility macros (for inlining and alignment).
// Also has a basic atomic type (limited to hardware-supported atomics with no memory ordering guarantees).
// Uses the AE_* prefix for macros (historical reasons), and the "moodycamel" namespace for symbols.
#include <cassert>
#include <cerrno>
#include <cstdint>
#include <ctime>
#include <type_traits>
// Platform detection
#if defined(__INTEL_COMPILER)
#define AE_ICC
#elif defined(_MSC_VER)
#define AE_VCPP
#elif defined(__GNUC__)
#define AE_GCC
#endif
#if defined(_M_IA64) || defined(__ia64__)
#define AE_ARCH_IA64
#elif defined(_WIN64) || defined(__amd64__) || defined(_M_X64) || defined(__x86_64__)
#define AE_ARCH_X64
#elif defined(_M_IX86) || defined(__i386__)
#define AE_ARCH_X86
#elif defined(_M_PPC) || defined(__powerpc__)
#define AE_ARCH_PPC
#else
#define AE_ARCH_UNKNOWN
#endif
// AE_UNUSED
#define AE_UNUSED(x) ((void)x)
// AE_FORCEINLINE
#if defined(AE_VCPP) || defined(AE_ICC)
#define AE_FORCEINLINE __forceinline
#elif defined(AE_GCC)
//#define AE_FORCEINLINE __attribute__((always_inline))
#define AE_FORCEINLINE inline
#else
#define AE_FORCEINLINE inline
#endif
// AE_ALIGN
#if defined(AE_VCPP) || defined(AE_ICC)
#define AE_ALIGN(x) __declspec(align(x))
#elif defined(AE_GCC)
#define AE_ALIGN(x) __attribute__((aligned(x)))
#else
// Assume GCC compliant syntax...
#define AE_ALIGN(x) __attribute__((aligned(x)))
#endif
// Portable atomic fences implemented below:
namespace moodycamel
{
enum memory_order
{
memory_order_relaxed,
memory_order_acquire,
memory_order_release,
memory_order_acq_rel,
memory_order_seq_cst,
// memory_order_sync: Forces a full sync:
// #LoadLoad, #LoadStore, #StoreStore, and most significantly, #StoreLoad
memory_order_sync = memory_order_seq_cst
};
} // end namespace moodycamel
#if (defined(AE_VCPP) && (_MSC_VER < 1700 || defined(__cplusplus_cli))) || defined(AE_ICC)
// VS2010 and ICC13 don't support std::atomic_*_fence, implement our own fences
#include <intrin.h>
#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86)
#define AeFullSync _mm_mfence
#define AeLiteSync _mm_mfence
#elif defined(AE_ARCH_IA64)
#define AeFullSync __mf
#define AeLiteSync __mf
#elif defined(AE_ARCH_PPC)
#include <ppcintrinsics.h>
#define AeFullSync __sync
#define AeLiteSync __lwsync
#endif
#ifdef AE_VCPP
#pragma warning(push)
#pragma warning(disable : 4365) // Disable erroneous 'conversion from long to unsigned int, signed/unsigned mismatch'
// error when using `assert`
#ifdef __cplusplus_cli
#pragma managed(push, off)
#endif
#endif
namespace moodycamel
{
AE_FORCEINLINE void compilerFence(memory_order order)
{
switch (order)
{
case memory_order_relaxed:
break;
case memory_order_acquire:
_ReadBarrier();
break;
case memory_order_release:
_WriteBarrier();
break;
case memory_order_acq_rel:
_ReadWriteBarrier();
break;
case memory_order_seq_cst:
_ReadWriteBarrier();
break;
default:
assert(false);
}
}
// x86/x64 have a strong memory model -- all loads and stores have
// acquire and release semantics automatically (so only need compiler
// barriers for those).
#if defined(AE_ARCH_X86) || defined(AE_ARCH_X64)
AE_FORCEINLINE void fence(memory_order order)
{
switch (order)
{
case memory_order_relaxed:
break;
case memory_order_acquire:
_ReadBarrier();
break;
case memory_order_release:
_WriteBarrier();
break;
case memory_order_acq_rel:
_ReadWriteBarrier();
break;
case memory_order_seq_cst:
_ReadWriteBarrier();
AeFullSync();
_ReadWriteBarrier();
break;
default:
assert(false);
}
}
#else
AE_FORCEINLINE void fence(memory_order order)
{
// Non-specialized arch, use heavier memory barriers everywhere just in case :-(
switch (order)
{
case memory_order_relaxed:
break;
case memory_order_acquire:
_ReadBarrier();
AeLiteSync();
_ReadBarrier();
break;
case memory_order_release:
_WriteBarrier();
AeLiteSync();
_WriteBarrier();
break;
case memory_order_acq_rel:
_ReadWriteBarrier();
AeLiteSync();
_ReadWriteBarrier();
break;
case memory_order_seq_cst:
_ReadWriteBarrier();
AeFullSync();
_ReadWriteBarrier();
break;
default:
assert(false);
}
}
#endif
} // end namespace moodycamel
#else
// Use standard library of atomics
#include <atomic>
namespace moodycamel
{
AE_FORCEINLINE void compilerFence(memory_order order)
{
switch (order)
{
case memory_order_relaxed:
break;
case memory_order_acquire:
std::atomic_signal_fence(std::memory_order_acquire);
break;
case memory_order_release:
std::atomic_signal_fence(std::memory_order_release);
break;
case memory_order_acq_rel:
std::atomic_signal_fence(std::memory_order_acq_rel);
break;
case memory_order_seq_cst:
std::atomic_signal_fence(std::memory_order_seq_cst);
break;
default:
assert(false);
}
}
AE_FORCEINLINE void fence(memory_order order)
{
switch (order)
{
case memory_order_relaxed:
break;
case memory_order_acquire:
std::atomic_thread_fence(std::memory_order_acquire);
break;
case memory_order_release:
std::atomic_thread_fence(std::memory_order_release);
break;
case memory_order_acq_rel:
std::atomic_thread_fence(std::memory_order_acq_rel);
break;
case memory_order_seq_cst:
std::atomic_thread_fence(std::memory_order_seq_cst);
break;
default:
assert(false);
}
}
} // end namespace moodycamel
#endif
#if !defined(AE_VCPP) || (_MSC_VER >= 1700 && !defined(__cplusplus_cli))
#define AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC
#endif
#ifdef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC
#include <atomic>
#endif
#include <utility>
// WARNING: *NOT* A REPLACEMENT FOR std::atomic. READ CAREFULLY:
// Provides basic support for atomic variables -- no memory ordering guarantees are provided.
// The guarantee of atomicity is only made for types that already have atomic load and store guarantees
// at the hardware level -- on most platforms this generally means aligned pointers and integers (only).
namespace moodycamel
{
template <typename T>
class WeakAtomic
{
public:
WeakAtomic()
{
}
#ifdef AE_VCPP
#pragma warning(disable : 4100) // Get rid of (erroneous) 'unreferenced formal parameter' warning
#endif
template <typename U>
WeakAtomic(U&& x) : value_(std::forward<U>(x))
{
}
#ifdef __cplusplus_cli
// Work around bug with universal reference/nullptr combination that only appears when /clr is on
WeakAtomic(nullptr_t) : value_(nullptr)
{
}
#endif
WeakAtomic(WeakAtomic const& other) : value_(other.value_)
{
}
WeakAtomic(WeakAtomic&& other) : value_(std::move(other.value_))
{
}
#ifdef AE_VCPP
#pragma warning(default : 4100)
#endif
AE_FORCEINLINE operator T() const
{
return load();
}
#ifndef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC
template <typename U>
AE_FORCEINLINE WeakAtomic const& operator=(U&& x)
{
value_ = std::forward<U>(x);
return *this;
}
AE_FORCEINLINE WeakAtomic const& operator=(WeakAtomic const& other)
{
value_ = other.value_;
return *this;
}
AE_FORCEINLINE T load() const
{
return value_;
}
AE_FORCEINLINE T fetchAddAcquire(T increment)
{
#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86)
if (sizeof(T) == 4)
return _InterlockedExchangeAdd((long volatile*)&value_, (long)increment);
#if defined(_M_AMD64)
else if (sizeof(T) == 8)
return _InterlockedExchangeAdd64((long long volatile*)&value_, (long long)increment);
#endif
#else
#error Unsupported platform
#endif
assert(false && "T must be either a 32 or 64 bit type");
return value_;
}
AE_FORCEINLINE T fetchAddRelease(T increment)
{
#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86)
if (sizeof(T) == 4)
return _InterlockedExchangeAdd((long volatile*)&value_, (long)increment);
#if defined(_M_AMD64)
else if (sizeof(T) == 8)
return _InterlockedExchangeAdd64((long long volatile*)&value_, (long long)increment);
#endif
#else
#error Unsupported platform
#endif
assert(false && "T must be either a 32 or 64 bit type");
return value_;
}
#else
template <typename U>
AE_FORCEINLINE WeakAtomic const& operator=(U&& x)
{
value_.store(std::forward<U>(x), std::memory_order_relaxed);
return *this;
}
AE_FORCEINLINE WeakAtomic const& operator=(WeakAtomic const& other)
{
value_.store(other.value_.load(std::memory_order_relaxed), std::memory_order_relaxed);
return *this;
}
AE_FORCEINLINE T load() const
{
return value_.load(std::memory_order_relaxed);
}
AE_FORCEINLINE T fetchAddAcquire(T increment)
{
return value_.fetch_add(increment, std::memory_order_acquire);
}
AE_FORCEINLINE T fetchAddRelease(T increment)
{
return value_.fetch_add(increment, std::memory_order_release);
}
#endif
private:
#ifndef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC
// No std::atomic support, but still need to circumvent compiler optimizations.
// `volatile` will make memory access slow, but is guaranteed to be reliable.
volatile T value_;
#else
std::atomic<T> value_;
#endif
};
} // end namespace moodycamel
// Portable single-producer, single-consumer semaphore below:
#if defined(_WIN32)
// Avoid including windows.h in a header; we only need a handful of
// items, so we'll redeclare them here (this is relatively safe since
// the API generally has to remain stable between Windows versions).
// I know this is an ugly hack but it still beats polluting the global
// namespace with thousands of generic names or adding a .cpp for nothing.
extern "C" {
struct _SECURITY_ATTRIBUTES;
__declspec(dllimport) void* __stdcall CreateSemaphoreW(_SECURITY_ATTRIBUTES* lpSemaphoreAttributes, long lInitialCount,
long lMaximumCount, const wchar_t* lpName);
__declspec(dllimport) int __stdcall CloseHandle(void* hObject);
__declspec(dllimport) unsigned long __stdcall WaitForSingleObject(void* hHandle, unsigned long dwMilliseconds);
__declspec(dllimport) int __stdcall ReleaseSemaphore(void* hSemaphore, long lReleaseCount, long* lpPreviousCount);
}
#elif defined(__MACH__)
#include <mach/mach.h>
#elif defined(__unix__)
#include <semaphore.h>
#endif
namespace moodycamel
{
// Code in the spsc_sema namespace below is an adaptation of Jeff Preshing's
// portable + lightweight semaphore implementations, originally from
// https://github.com/preshing/cpp11-on-multicore/blob/master/common/sema.h
// LICENSE:
// Copyright (c) 2015 Jeff Preshing
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
//
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
//
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgement in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
namespace spsc_sema
{
#if defined(_WIN32)
class Semaphore
{
private:
void* m_hSema;
Semaphore(const Semaphore& other);
Semaphore& operator=(const Semaphore& other);
public:
Semaphore(int initialCount = 0)
{
assert(initialCount >= 0);
const long maxLong = 0x7fffffff;
m_hSema = CreateSemaphoreW(nullptr, initialCount, maxLong, nullptr);
}
~Semaphore()
{
CloseHandle(m_hSema);
}
void wait()
{
const unsigned long infinite = 0xffffffff;
WaitForSingleObject(m_hSema, infinite);
}
bool tryWait()
{
const unsigned long RC_WAIT_TIMEOUT = 0x00000102;
return WaitForSingleObject(m_hSema, 0) != RC_WAIT_TIMEOUT;
}
bool timedWait(std::uint64_t usecs)
{
const unsigned long RC_WAIT_TIMEOUT = 0x00000102;
return WaitForSingleObject(m_hSema, (unsigned long)(usecs / 1000)) != RC_WAIT_TIMEOUT;
}
void signal(int count = 1)
{
ReleaseSemaphore(m_hSema, count, nullptr);
}
};
#elif defined(__MACH__)
//---------------------------------------------------------
// Semaphore (Apple iOS and OSX)
// Can't use POSIX semaphores due to http://lists.apple.com/archives/darwin-kernel/2009/Apr/msg00010.html
//---------------------------------------------------------
class Semaphore
{
private:
semaphore_t sema_;
Semaphore(const Semaphore& other);
Semaphore& operator=(const Semaphore& other);
public:
Semaphore(int initialCount = 0)
{
assert(initialCount >= 0);
semaphore_create(mach_task_self(), &sema_, SYNC_POLICY_FIFO, initialCount);
}
~Semaphore()
{
semaphore_destroy(mach_task_self(), sema_);
}
void wait()
{
semaphore_wait(sema_);
}
bool tryWait()
{
return timedWait(0);
}
bool timedWait(std::int64_t timeout_usecs)
{
mach_timespec_t ts;
ts.tv_sec = timeout_usecs / 1000000;
ts.tv_nsec = (timeout_usecs % 1000000) * 1000;
// added in OSX 10.10:
// https://developer.apple.com/library/prerelease/mac/documentation/General/Reference/APIDiffsMacOSX10_10SeedDiff/modules/Darwin.html
kern_return_t rc = semaphore_timedwait(sema_, ts);
return rc != KERN_OPERATION_TIMED_OUT;
}
void signal()
{
semaphore_signal(sema_);
}
void signal(int count)
{
while (count-- > 0)
{
semaphore_signal(sema_);
}
}
};
#elif defined(__unix__)
//---------------------------------------------------------
// Semaphore (POSIX, Linux)
//---------------------------------------------------------
class Semaphore
{
private:
sem_t sema_;
Semaphore(const Semaphore& other);
Semaphore& operator=(const Semaphore& other);
public:
Semaphore(int initialCount = 0)
{
assert(initialCount >= 0);
sem_init(&sema_, 0, initialCount);
}
~Semaphore()
{
sem_destroy(&sema_);
}
void wait()
{
// http://stackoverflow.com/questions/2013181/gdb-causes-sem-wait-to-fail-with-eintr-error
int rc;
do
{
rc = sem_wait(&sema_);
} while (rc == -1 && errno == EINTR);
}
bool tryWait()
{
int rc;
do
{
rc = sem_trywait(&sema_);
} while (rc == -1 && errno == EINTR);
return !(rc == -1 && errno == EAGAIN);
}
bool timedWait(std::uint64_t usecs)
{
struct timespec ts;
const int usecs_in_1_sec = 1000000;
const int nsecs_in_1_sec = 1000000000;
clock_gettime(CLOCK_REALTIME, &ts);
ts.tv_sec += usecs / usecs_in_1_sec;
ts.tv_nsec += (usecs % usecs_in_1_sec) * 1000;
// sem_timedwait bombs if you have more than 1e9 in tv_nsec
// so we have to clean things up before passing it in
if (ts.tv_nsec > nsecs_in_1_sec)
{
ts.tv_nsec -= nsecs_in_1_sec;
++ts.tv_sec;
}
int rc;
do
{
rc = sem_timedwait(&sema_, &ts);
} while (rc == -1 && errno == EINTR);
return !(rc == -1 && errno == ETIMEDOUT);
}
void signal()
{
sem_post(&sema_);
}
void signal(int count)
{
while (count-- > 0)
{
sem_post(&sema_);
}
}
};
#else
#error Unsupported platform! (No semaphore wrapper available)
#endif
//---------------------------------------------------------
// LightweightSemaphore
//---------------------------------------------------------
class LightweightSemaphore
{
public:
typedef std::make_signed<std::size_t>::type ssize_t;
private:
WeakAtomic<ssize_t> count_;
Semaphore sema_;
bool waitWithPartialSpinning(std::int64_t timeout_usecs = -1)
{
ssize_t old_count;
// Is there a better way to set the initial spin count?
// If we lower it to 1000, testBenaphore becomes 15x slower on my Core i7-5930K Windows PC,
// as threads start hitting the kernel semaphore.
int spin = 10000;
while (--spin >= 0)
{
if (count_.load() > 0)
{
count_.fetchAddAcquire(-1);
return true;
}
compilerFence(memory_order_acquire); // Prevent the compiler from collapsing the loop.
}
old_count = count_.fetchAddAcquire(-1);
if (old_count > 0)
return true;
if (timeout_usecs < 0)
{
sema_.wait();
return true;
}
if (sema_.timedWait(timeout_usecs))
return true;
// At this point, we've timed out waiting for the semaphore, but the
// count is still decremented indicating we may still be waiting on
// it. So we have to re-adjust the count, but only if the semaphore
// wasn't signaled enough times for us too since then. If it was, we
// need to release the semaphore too.
while (true)
{
old_count = count_.fetchAddRelease(1);
if (old_count < 0)
return false; // successfully restored things to the way they were
// Oh, the producer thread just signaled the semaphore after all. Try again:
old_count = count_.fetchAddAcquire(-1);
if (old_count > 0 && sema_.tryWait())
return true;
}
}
public:
LightweightSemaphore(ssize_t initialCount = 0) : count_(initialCount)
{
assert(initialCount >= 0);
}
bool tryWait()
{
if (count_.load() > 0)
{
count_.fetchAddAcquire(-1);
return true;
}
return false;
}
void wait()
{
if (!tryWait())
waitWithPartialSpinning();
}
bool wait(std::int64_t timeout_usecs)
{
return tryWait() || waitWithPartialSpinning(timeout_usecs);
}
void signal(ssize_t count = 1)
{
assert(count >= 0);
ssize_t old_count = count_.fetchAddRelease(count);
assert(old_count >= -1);
if (old_count < 0)
{
sema_.signal(1);
}
}
ssize_t availableApprox() const
{
ssize_t count = count_.load();
return count > 0 ? count : 0;
}
};
} // end namespace spsc_sema
} // end namespace moodycamel
#if defined(AE_VCPP) && (_MSC_VER < 1700 || defined(__cplusplus_cli))
#pragma warning(pop)
#ifdef __cplusplus_cli
#pragma managed(pop)
#endif
#endif

865
ur_robot_driver/include/ur_robot_driver/queue/readerwriterqueue.h Normal file
View File

@@ -0,0 +1,865 @@
// ©2013-2016 Cameron Desrochers.
// Distributed under the simplified BSD license (see the license file that
// should have come with this header).
#pragma once
#include <cassert>
#include <cstdint>
#include <cstdlib> // For malloc/free/abort & size_t
#include <new>
#include <stdexcept>
#include <type_traits>
#include <utility>
#include "atomicops.h"
#if __cplusplus > 199711L || _MSC_VER >= 1700 // C++11 or VS2012
#include <chrono>
#endif
// A lock-free queue for a single-consumer, single-producer architecture.
// The queue is also wait-free in the common path (except if more memory
// needs to be allocated, in which case malloc is called).
// Allocates memory sparingly (O(lg(n) times, amortized), and only once if
// the original maximum size estimate is never exceeded.
// Tested on x86/x64 processors, but semantics should be correct for all
// architectures (given the right implementations in atomicops.h), provided
// that aligned integer and pointer accesses are naturally atomic.
// Note that there should only be one consumer thread and producer thread;
// Switching roles of the threads, or using multiple consecutive threads for
// one role, is not safe unless properly synchronized.
// Using the queue exclusively from one thread is fine, though a bit silly.
#ifndef MOODYCAMEL_CACHE_LINE_SIZE
#define MOODYCAMEL_CACHE_LINE_SIZE 64
#endif
#ifndef MOODYCAMEL_EXCEPTIONS_ENABLED
#if (defined(_MSC_VER) && defined(_CPPUNWIND)) || (defined(__GNUC__) && defined(__EXCEPTIONS)) || \
(!defined(_MSC_VER) && !defined(__GNUC__))
#define MOODYCAMEL_EXCEPTIONS_ENABLED
#endif
#endif
#ifdef AE_VCPP
#pragma warning(push)
#pragma warning(disable : 4324) // structure was padded due to __declspec(align())
#pragma warning(disable : 4820) // padding was added
#pragma warning(disable : 4127) // conditional expression is constant
#endif
namespace moodycamel
{
template <typename T, size_t MAX_BLOCK_SIZE = 512>
class ReaderWriterQueue
{
// Design: Based on a queue-of-queues. The low-level queues are just
// circular buffers with front and tail indices indicating where the
// next element to dequeue is and where the next element can be enqueued,
// respectively. Each low-level queue is called a "block". Each block
// wastes exactly one element's worth of space to keep the design simple
// (if front == tail then the queue is empty, and can't be full).
// The high-level queue is a circular linked list of blocks; again there
// is a front and tail, but this time they are pointers to the blocks.
// The front block is where the next element to be dequeued is, provided
// the block is not empty. The back block is where elements are to be
// enqueued, provided the block is not full.
// The producer thread owns all the tail indices/pointers. The consumer
// thread owns all the front indices/pointers. Both threads read each
// other's variables, but only the owning thread updates them. E.g. After
// the consumer reads the producer's tail, the tail may change before the
// consumer is done dequeuing_ an object, but the consumer knows the tail
// will never go backwards, only forwards.
// If there is no room to enqueue an object, an additional block (of
// equal size to the last block) is added. Blocks are never removed.
public:
// Constructs a queue that can hold maxSize elements without further
// allocations. If more than MAX_BLOCK_SIZE elements are requested,
// then several blocks of MAX_BLOCK_SIZE each are reserved (including
// at least one extra buffer block).
explicit ReaderWriterQueue(size_t max_size = 15)
#ifndef NDEBUG
: enqueuing_(false), dequeuing_(false)
#endif
{
assert(max_size > 0);
assert(MAX_BLOCK_SIZE == ceilToPow2(MAX_BLOCK_SIZE) && "MAX_BLOCK_SIZE must be a power of 2");
assert(MAX_BLOCK_SIZE >= 2 && "MAX_BLOCK_SIZE must be at least 2");
Block* first_block = nullptr;
largest_block_size_ = ceilToPow2(max_size + 1); // We need a spare slot to fit maxSize elements in the block
if (largest_block_size_ > MAX_BLOCK_SIZE * 2)
{
// We need a spare block in case the producer is writing to a different block the consumer is reading from, and
// wants to enqueue the maximum number of elements. We also need a spare element in each block to avoid the
// ambiguity
// between front == tail meaning "empty" and "full".
// So the effective number of slots that are guaranteed to be usable at any time is the block size - 1 times the
// number of blocks - 1. Solving for maxSize and applying a ceiling to the division gives us (after simplifying):
size_t initial_block_count = (max_size + MAX_BLOCK_SIZE * 2 - 3) / (MAX_BLOCK_SIZE - 1);
largest_block_size_ = MAX_BLOCK_SIZE;
Block* last_block = nullptr;
for (size_t i = 0; i != initial_block_count; ++i)
{
auto block = makeBlock(largest_block_size_);
if (block == nullptr)
{
#ifdef MOODYCAMEL_EXCEPTIONS_ENABLED
throw std::bad_alloc();
#else
abort();
#endif
}
if (first_block == nullptr)
{
first_block = block;
}
else
{
last_block->next = block;
}
last_block = block;
block->next = first_block;
}
}
else
{
first_block = makeBlock(largest_block_size_);
if (first_block == nullptr)
{
#ifdef MOODYCAMEL_EXCEPTIONS_ENABLED
throw std::bad_alloc();
#else
abort();
#endif
}
first_block->next = first_block;
}
front_block_ = first_block;
tail_block_ = first_block;
// Make sure the reader/writer threads will have the initialized memory setup above:
fence(memory_order_sync);
}
// Note: The queue should not be accessed concurrently while it's
// being deleted. It's up to the user to synchronize this.
~ReaderWriterQueue()
{
// Make sure we get the latest version of all variables from other CPUs:
fence(memory_order_sync);
// Destroy any remaining objects in queue and free memory
Block* front_block = front_block_;
Block* block = front_block;
do
{
Block* next_block = block->next;
size_t block_front = block->front;
size_t block_tail = block->tail;
for (size_t i = block_front; i != block_tail; i = (i + 1) & block->sizeMask)
{
auto element = reinterpret_cast<T*>(block->data + i * sizeof(T));
element->~T();
(void)element;
}
auto raw_block = block->rawThis;
block->~Block();
std::free(raw_block);
block = next_block;
} while (block != front_block);
}
// Enqueues a copy of element if there is room in the queue.
// Returns true if the element was enqueued, false otherwise.
// Does not allocate memory.
AE_FORCEINLINE bool tryEnqueue(T const& element)
{
return innerEnqueue<CannotAlloc>(element);
}
// Enqueues a moved copy of element if there is room in the queue.
// Returns true if the element was enqueued, false otherwise.
// Does not allocate memory.
AE_FORCEINLINE bool tryEnqueue(T&& element)
{
return innerEnqueue<CannotAlloc>(std::forward<T>(element));
}
// Enqueues a copy of element on the queue.
// Allocates an additional block of memory if needed.
// Only fails (returns false) if memory allocation fails.
AE_FORCEINLINE bool enqueue(T const& element)
{
return innerEnqueue<CanAlloc>(element);
}
// Enqueues a moved copy of element on the queue.
// Allocates an additional block of memory if needed.
// Only fails (returns false) if memory allocation fails.
AE_FORCEINLINE bool enqueue(T&& element)
{
return innerEnqueue<CanAlloc>(std::forward<T>(element));
}
// Attempts to dequeue an element; if the queue is empty,
// returns false instead. If the queue has at least one element,
// moves front to result using operator=, then returns true.
template <typename U>
bool tryDequeue(U& result)
{
#ifndef NDEBUG
ReentrantGuard guard(this->dequeuing_);
#endif
// High-level pseudocode:
// Remember where the tail block is
// If the front block has an element in it, dequeue it
// Else
// If front block was the tail block when we entered the function, return false
// Else advance to next block and dequeue the item there
// Note that we have to use the value of the tail block from before we check if the front
// block is full or not, in case the front block is empty and then, before we check if the
// tail block is at the front block or not, the producer fills up the front block *and
// moves on*, which would make us skip a filled block. Seems unlikely, but was consistently
// reproducible in practice.
// In order to avoid overhead in the common case, though, we do a double-checked pattern
// where we have the fast path if the front block is not empty, then read the tail block,
// then re-read the front block and check if it's not empty again, then check if the tail
// block has advanced.
Block* front_block = front_block_.load();
size_t block_tail = front_block->localTail;
size_t block_front = front_block->front.load();
if (block_front != block_tail || block_front != (front_block->localTail = front_block->tail.load()))
{
fence(memory_order_acquire);
non_empty_front_block:
// Front block not empty, dequeue from here
auto element = reinterpret_cast<T*>(front_block->data + block_front * sizeof(T));
result = std::move(*element);
element->~T();
block_front = (block_front + 1) & front_block->sizeMask;
fence(memory_order_release);
front_block->front = block_front;
}
else if (front_block != tail_block_.load())
{
fence(memory_order_acquire);
front_block = front_block_.load();
block_tail = front_block->localTail = front_block->tail.load();
block_front = front_block->front.load();
fence(memory_order_acquire);
if (block_front != block_tail)
{
// Oh look, the front block isn't empty after all
goto non_empty_front_block;
}
// Front block is empty but there's another block ahead, advance to it
Block* next_block = front_block->next;
// Don't need an acquire fence here since next can only ever be set on the tail_block,
// and we're not the tail_block, and we did an acquire earlier after reading tail_block which
// ensures next is up-to-date on this CPU in case we recently were at tail_block.
size_t next_block_front = next_block->front.load();
size_t next_block_tail = next_block->localTail = next_block->tail.load();
fence(memory_order_acquire);
// Since the tail_block is only ever advanced after being written to,
// we know there's for sure an element to dequeue on it
assert(next_block_front != next_block_tail);
AE_UNUSED(next_block_tail);
// We're done with this block, let the producer use it if it needs
fence(memory_order_release); // Expose possibly pending changes to front_block->front from last dequeue
front_block_ = front_block = next_block;
compilerFence(memory_order_release); // Not strictly needed
auto element = reinterpret_cast<T*>(front_block->data + next_block_front * sizeof(T));
result = std::move(*element);
element->~T();
next_block_front = (next_block_front + 1) & front_block->sizeMask;
fence(memory_order_release);
front_block->front = next_block_front;
}
else
{
// No elements in current block and no other block to advance to
return false;
}
return true;
}
// Returns a pointer to the front element in the queue (the one that
// would be removed next by a call to `tryDequeue` or `pop`). If the
// queue appears empty at the time the method is called, nullptr is
// returned instead.
// Must be called only from the consumer thread.
T* peek()
{
#ifndef NDEBUG
ReentrantGuard guard(this->dequeuing_);
#endif
// See tryDequeue() for reasoning
Block* front_block = front_block_.load();
size_t block_tail = front_block->localTail;
size_t block_front = front_block->front.load();
if (block_front != block_tail || block_front != (front_block->localTail = front_block->tail.load()))
{
fence(memory_order_acquire);
non_empty_front_block:
return reinterpret_cast<T*>(front_block->data + block_front * sizeof(T));
}
else if (front_block != tail_block_.load())
{
fence(memory_order_acquire);
front_block = front_block_.load();
block_tail = front_block->localTail = front_block->tail.load();
block_front = front_block->front.load();
fence(memory_order_acquire);
if (block_front != block_tail)
{
goto non_empty_front_block;
}
Block* next_block = front_block->next;
size_t next_block_front = next_block->front.load();
fence(memory_order_acquire);
assert(next_block_front != next_block->tail.load());
return reinterpret_cast<T*>(next_block->data + next_block_front * sizeof(T));
}
return nullptr;
}
// Removes the front element from the queue, if any, without returning it.
// Returns true on success, or false if the queue appeared empty at the time
// `pop` was called.
bool pop()
{
#ifndef NDEBUG
ReentrantGuard guard(this->dequeuing_);
#endif
// See tryDequeue() for reasoning
Block* front_block = front_block_.load();
size_t block_tail = front_block->localTail;
size_t block_front = front_block->front.load();
if (block_front != block_tail || block_front != (front_block->localTail = front_block->tail.load()))
{
fence(memory_order_acquire);
non_empty_front_block:
auto element = reinterpret_cast<T*>(front_block->data + block_front * sizeof(T));
element->~T();
block_front = (block_front + 1) & front_block->sizeMask;
fence(memory_order_release);
front_block->front = block_front;
}
else if (front_block != tail_block_.load())
{
fence(memory_order_acquire);
front_block = front_block_.load();
block_tail = front_block->localTail = front_block->tail.load();
block_front = front_block->front.load();
fence(memory_order_acquire);
if (block_front != block_tail)
{
goto non_empty_front_block;
}
// Front block is empty but there's another block ahead, advance to it
Block* next_block = front_block->next;
size_t next_block_front = next_block->front.load();
size_t next_block_tail = next_block->localTail = next_block->tail.load();
fence(memory_order_acquire);
assert(next_block_front != next_block_tail);
AE_UNUSED(next_block_tail);
fence(memory_order_release);
front_block_ = front_block = next_block;
compilerFence(memory_order_release);
auto element = reinterpret_cast<T*>(front_block->data + next_block_front * sizeof(T));
element->~T();
next_block_front = (next_block_front + 1) & front_block->sizeMask;
fence(memory_order_release);
front_block->front = next_block_front;
}
else
{
// No elements in current block and no other block to advance to
return false;
}
return true;
}
// Returns the approximate number of items currently in the queue.
// Safe to call from both the producer and consumer threads.
inline size_t sizeApprox() const
{
size_t result = 0;
Block* front_block = front_block_.load();
Block* block = front_block;
do
{
fence(memory_order_acquire);
size_t block_front = block->front.load();
size_t block_tail = block->tail.load();
result += (block_tail - block_front) & block->sizeMask;
block = block->next.load();
} while (block != front_block);
return result;
}
private:
enum AllocationMode
{
CanAlloc,
CannotAlloc
};
template <AllocationMode canAlloc, typename U>
bool innerEnqueue(U&& element)
{
#ifndef NDEBUG
ReentrantGuard guard(this->enqueuing_);
#endif
// High-level pseudocode (assuming we're allowed to alloc a new block):
// If room in tail block, add to tail
// Else check next block
// If next block is not the head block, enqueue on next block
// Else create a new block and enqueue there
// Advance tail to the block we just enqueued to
Block* tail_block = tail_block_.load();
size_t block_front = tail_block->localFront;
size_t block_tail = tail_block->tail.load();
size_t next_block_tail = (block_tail + 1) & tail_block->sizeMask;
if (next_block_tail != block_front || next_block_tail != (tail_block->localFront = tail_block->front.load()))
{
fence(memory_order_acquire);
// This block has room for at least one more element
char* location = tail_block->data + block_tail * sizeof(T);
new (location) T(std::forward<U>(element));
fence(memory_order_release);
tail_block->tail = next_block_tail;
}
else
{
fence(memory_order_acquire);
if (tail_block->next.load() != front_block_)
{
// Note that the reason we can't advance to the front_block and start adding new entries there
// is because if we did, then dequeue would stay in that block, eventually reading the new values,
// instead of advancing to the next full block (whose values were enqueued first and so should be
// consumed first).
fence(memory_order_acquire); // Ensure we get latest writes if we got the latest front_block
// tail_block is full, but there's a free block ahead, use it
Block* tail_block_next = tail_block->next.load();
size_t next_block_front = tail_block_next->localFront = tail_block_next->front.load();
next_block_tail = tail_block_next->tail.load();
fence(memory_order_acquire);
// This block must be empty since it's not the head block and we
// go through the blocks in a circle
assert(next_block_front == next_block_tail);
tail_block_next->localFront = next_block_front;
char* location = tail_block_next->data + next_block_tail * sizeof(T);
new (location) T(std::forward<U>(element));
tail_block_next->tail = (next_block_tail + 1) & tail_block_next->sizeMask;
fence(memory_order_release);
tail_block_ = tail_block_next;
}
else if (canAlloc == CanAlloc)
{
// tail_block is full and there's no free block ahead; create a new block
auto new_block_size = largest_block_size_ >= MAX_BLOCK_SIZE ? largest_block_size_ : largest_block_size_ * 2;
auto new_block = makeBlock(new_block_size);
if (new_block == nullptr)
{
// Could not allocate a block!
return false;
}
largest_block_size_ = new_block_size;
new (new_block->data) T(std::forward<U>(element));
assert(new_block->front == 0);
new_block->tail = new_block->localTail = 1;
new_block->next = tail_block->next.load();
tail_block->next = new_block;
// Might be possible for the dequeue thread to see the new tail_block->next
// *without* seeing the new tail_block value, but this is OK since it can't
// advance to the next block until tail_block is set anyway (because the only
// case where it could try to read the next is if it's already at the tail_block,
// and it won't advance past tail_block in any circumstance).
fence(memory_order_release);
tail_block_ = new_block;
}
else if (canAlloc == CannotAlloc)
{
// Would have had to allocate a new block to enqueue, but not allowed
return false;
}
else
{
assert(false && "Should be unreachable code");
return false;
}
}
return true;
}
// Disable copying
ReaderWriterQueue(ReaderWriterQueue const&)
{
}
// Disable assignment
ReaderWriterQueue& operator=(ReaderWriterQueue const&)
{
}
AE_FORCEINLINE static size_t ceilToPow2(size_t x)
{
// From http://graphics.stanford.edu/~seander/bithacks.html#RoundUpPowerOf2
--x;
x |= x >> 1;
x |= x >> 2;
x |= x >> 4;
for (size_t i = 1; i < sizeof(size_t); i <<= 1)
{
x |= x >> (i << 3);
}
++x;
return x;
}
template <typename U>
static AE_FORCEINLINE char* alignFor(char* ptr)
{
const std::size_t alignment = std::alignment_of<U>::value;
return ptr + (alignment - (reinterpret_cast<std::uintptr_t>(ptr) % alignment)) % alignment;
}
private:
#ifndef NDEBUG
struct ReentrantGuard
{
ReentrantGuard(bool& in_section) : in_section_(in_section)
{
assert(!in_section_ && "ReaderWriterQueue does not support enqueuing_ or dequeuing_ elements from other "
"elements' "
"ctors and dtors");
in_section_ = true;
}
~ReentrantGuard()
{
in_section_ = false;
}
private:
ReentrantGuard& operator=(ReentrantGuard const&);
private:
bool& in_section_;
};
#endif
struct Block
{
// Avoid false-sharing by putting highly contended variables on their own cache lines
WeakAtomic<size_t> front; // (Atomic) Elements are read from here
size_t localTail; // An uncontended shadow copy of tail, owned by the consumer
char cachelineFiller0_[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(WeakAtomic<size_t>) - sizeof(size_t)];
WeakAtomic<size_t> tail; // (Atomic) Elements are enqueued here
size_t localFront;
char cachelineFiller1_[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(WeakAtomic<size_t>) - sizeof(size_t)]; // next isn't
// very
// contended, but
// we don't want
// it on the same
// cache line as
// tail (which
// is)
WeakAtomic<Block*> next; // (Atomic)
char* data; // Contents (on heap) are aligned to T's alignment
const size_t sizeMask;
// size must be a power of two (and greater than 0)
Block(size_t const& _size, char* _rawThis, char* _data)
: front(0)
, localTail(0)
, tail(0)
, localFront(0)
, next(nullptr)
, data(_data)
, sizeMask(_size - 1)
, rawThis(_rawThis)
{
}
private:
// C4512 - Assignment operator could not be generated
Block& operator=(Block const&);
public:
char* rawThis;
};
static Block* makeBlock(size_t capacity)
{
// Allocate enough memory for the block itself, as well as all the elements it will contain
auto size = sizeof(Block) + std::alignment_of<Block>::value - 1;
size += sizeof(T) * capacity + std::alignment_of<T>::value - 1;
auto new_block_raw = static_cast<char*>(std::malloc(size));
if (new_block_raw == nullptr)
{
return nullptr;
}
auto new_block_aligned = alignFor<Block>(new_block_raw);
auto new_block_data = alignFor<T>(new_block_aligned + sizeof(Block));
return new (new_block_aligned) Block(capacity, new_block_raw, new_block_data);
}
private:
WeakAtomic<Block*> front_block_; // (Atomic) Elements are enqueued to this block
char cachelineFiller_[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(WeakAtomic<Block*>)];
WeakAtomic<Block*> tail_block_; // (Atomic) Elements are dequeued from this block
size_t largest_block_size_;
#ifndef NDEBUG
bool enqueuing_;
bool dequeuing_;
#endif
};
// Like ReaderWriterQueue, but also providees blocking operations
template <typename T, size_t MAX_BLOCK_SIZE = 512>
class BlockingReaderWriterQueue
{
private:
typedef ::moodycamel::ReaderWriterQueue<T, MAX_BLOCK_SIZE> ReaderWriterQueue;
public:
explicit BlockingReaderWriterQueue(size_t maxSize = 15) : inner_(maxSize)
{
}
// Enqueues a copy of element if there is room in the queue.
// Returns true if the element was enqueued, false otherwise.
// Does not allocate memory.
AE_FORCEINLINE bool tryEnqueue(T const& element)
{
if (inner_.tryEnqueue(element))
{
sema_.signal();
return true;
}
return false;
}
// Enqueues a moved copy of element if there is room in the queue.
// Returns true if the element was enqueued, false otherwise.
// Does not allocate memory.
AE_FORCEINLINE bool tryEnqueue(T&& element)
{
if (inner_.tryEnqueue(std::forward<T>(element)))
{
sema_.signal();
return true;
}
return false;
}
// Enqueues a copy of element on the queue.
// Allocates an additional block of memory if needed.
// Only fails (returns false) if memory allocation fails.
AE_FORCEINLINE bool enqueue(T const& element)
{
if (inner_.enqueue(element))
{
sema_.signal();
return true;
}
return false;
}
// Enqueues a moved copy of element on the queue.
// Allocates an additional block of memory if needed.
// Only fails (returns false) if memory allocation fails.
AE_FORCEINLINE bool enqueue(T&& element)
{
if (inner_.enqueue(std::forward<T>(element)))
{
sema_.signal();
return true;
}
return false;
}
// Attempts to dequeue an element; if the queue is empty,
// returns false instead. If the queue has at least one element,
// moves front to result using operator=, then returns true.
template <typename U>
bool tryDequeue(U& result)
{
if (sema_.tryWait())
{
bool success = inner_.tryDequeue(result);
assert(success);
AE_UNUSED(success);
return true;
}
return false;
}
// Attempts to dequeue an element; if the queue is empty,
// waits until an element is available, then dequeues it.
template <typename U>
void waitDequeue(U& result)
{
sema_.wait();
bool success = inner_.tryDequeue(result);
AE_UNUSED(result);
assert(success);
AE_UNUSED(success);
}
// Attempts to dequeue an element; if the queue is empty,
// waits until an element is available up to the specified timeout,
// then dequeues it and returns true, or returns false if the timeout
// expires before an element can be dequeued.
// Using a negative timeout indicates an indefinite timeout,
// and is thus functionally equivalent to calling waitDequeue.
template <typename U>
bool waitDequeTimed(U& result, std::int64_t timeout_usecs)
{
if (!sema_.wait(timeout_usecs))
{
return false;
}
bool success = inner_.tryDequeue(result);
AE_UNUSED(result);
assert(success);
AE_UNUSED(success);
return true;
}
#if __cplusplus > 199711L || _MSC_VER >= 1700
// Attempts to dequeue an element; if the queue is empty,
// waits until an element is available up to the specified timeout,
// then dequeues it and returns true, or returns false if the timeout
// expires before an element can be dequeued.
// Using a negative timeout indicates an indefinite timeout,
// and is thus functionally equivalent to calling waitDequeue.
template <typename U, typename Rep, typename Period>
inline bool waitDequeTimed(U& result, std::chrono::duration<Rep, Period> const& timeout)
{
return waitDequeTimed(result, std::chrono::duration_cast<std::chrono::microseconds>(timeout).count());
}
#endif
// Returns a pointer to the front element in the queue (the one that
// would be removed next by a call to `tryDequeue` or `pop`). If the
// queue appears empty at the time the method is called, nullptr is
// returned instead.
// Must be called only from the consumer thread.
AE_FORCEINLINE T* peek()
{
return inner_.peek();
}
// Removes the front element from the queue, if any, without returning it.
// Returns true on success, or false if the queue appeared empty at the time
// `pop` was called.
AE_FORCEINLINE bool pop()
{
if (sema_.tryWait())
{
bool result = inner_.pop();
assert(result);
AE_UNUSED(result);
return true;
}
return false;
}
// Returns the approximate number of items currently in the queue.
// Safe to call from both the producer and consumer threads.
AE_FORCEINLINE size_t sizeApprox() const
{
return sema_.availableApprox();
}
private:
// Disable copying & assignment
BlockingReaderWriterQueue(ReaderWriterQueue const&)
{
}
BlockingReaderWriterQueue& operator=(ReaderWriterQueue const&)
{
}
private:
ReaderWriterQueue inner_;
spsc_sema::LightweightSemaphore sema_;
};
} // end namespace moodycamel
#ifdef AE_VCPP
#pragma warning(pop)
#endif

255
ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h Normal file
View File

@@ -0,0 +1,255 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-11
*
*/
//----------------------------------------------------------------------
#ifndef UR_DRIVER_HARDWARE_INTERFACE_H_INCLUDED
#define UR_DRIVER_HARDWARE_INTERFACE_H_INCLUDED
#include <hardware_interface/robot_hw.h>
#include <hardware_interface/force_torque_sensor_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <algorithm>
#include <std_msgs/Bool.h>
#include <std_msgs/Float64.h>
#include <std_msgs/String.h>
#include <std_srvs/Trigger.h>
#include <realtime_tools/realtime_publisher.h>
#include "tf2_msgs/TFMessage.h"
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <ur_msgs/IOStates.h>
#include <ur_msgs/ToolDataMsg.h>
#include <ur_msgs/SetIO.h>
#include "ur_msgs/SetSpeedSliderFraction.h"
#include <ur_controllers/speed_scaling_interface.h>
#include <ur_controllers/scaled_joint_command_interface.h>
#include "ur_robot_driver/ur/ur_driver.h"
namespace ur_driver
{
/*!
* \brief Possible states for robot control
*/
enum class PausingState
{
PAUSED,
RUNNING,
RAMPUP
};
/*!
* \brief The HardwareInterface class handles the interface between the ROS system and the main
* driver. It contains the read and write methods of the main control loop and registers various ROS
* topics and services.
*/
class HardwareInterface : public hardware_interface::RobotHW
{
public:
/*!
* \brief Creates a new HardwareInterface object.
*/
HardwareInterface();
virtual ~HardwareInterface() = default;
/*!
* \brief Handles the setup functionality for the ROS interface. This includes parsing ROS
* parameters, creating interfaces, starting the main driver and advertising ROS services.
*
* \param root_nh Root level ROS node handle
* \param robot_hw_nh ROS node handle for the robot namespace
*
* \returns True, if the setup was performed successfully
*/
virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) override;
/*!
* \brief Read method of the control loop. Reads a RTDE package from the robot and handles and
* publishes the information as needed.
*
* \param time Current time
* \param period Duration of current control loop iteration
*/
virtual void read(const ros::Time& time, const ros::Duration& period) override;
/*!
* \brief Write method of the control loop. Writes target joint positions to the robot to be read
* by its URCaps program.
*
* \param time Current time
* \param period Duration of current control loop iteration
*/
virtual void write(const ros::Time& time, const ros::Duration& period) override;
/*!
* \brief Preparation to start and stop loaded controllers.
*
* \param start_list List of controllers to start
* \param stop_list List of controllers to stop
*
* \returns True, if the controllers can be switched
*/
virtual bool prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list) override;
/*!
* \brief Starts and stops controllers.
*
* \param start_list List of controllers to start
* \param stop_list List of controllers to stop
*/
virtual void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list) override;
/*!
* \brief Getter for the current control frequency
*
* \returns The used control frequency
*/
uint32_t getControlFrequency() const;
/*!
* \brief Checks if the URCaps program is running on the robot.
*
* \returns True, if the program is currently running, false otherwise.
*/
bool isRobotProgramRunning() const;
/*!
* \brief Callback to handle a change in the current state of the URCaps program running on the
* robot.
*
* \param program_running The new state of the program
*/
void handleRobotProgramState(bool program_running);
/*!
* \brief Checks if a reset of the ROS controllers is necessary.
*
* \returns Necessity of ROS controller reset
*/
bool shouldResetControllers();
protected:
/*!
* \brief Transforms force-torque measurements reported from the robot from base to tool frame.
*
* Requires extractToolPose() to be run first.
*/
void transformForceTorque();
/*!
* \brief Stores the raw tool pose data from the robot in a transformation msg
*
* \param timestamp Timestamp of read data
*/
void extractToolPose(const ros::Time& timestamp);
/*!
* \brief Publishes the tool pose to the tf system
*
* Requires extractToolPose() to be run first.
*/
void publishPose();
void publishIOData();
void publishToolData();
bool stopControl(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
template <typename T>
void readData(const std::unique_ptr<rtde_interface::DataPackage>& data_pkg, const std::string& var_name, T& data);
template <typename T, size_t N>
void readBitsetData(const std::unique_ptr<rtde_interface::DataPackage>& data_pkg, const std::string& var_name,
std::bitset<N>& data);
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);
void commandCallback(const std_msgs::StringConstPtr& msg);
std::unique_ptr<UrDriver> ur_driver_;
ros::ServiceServer deactivate_srv_;
hardware_interface::JointStateInterface js_interface_;
ur_controllers::ScaledPositionJointInterface spj_interface_;
hardware_interface::PositionJointInterface pj_interface_;
ur_controllers::SpeedScalingInterface speedsc_interface_;
// hardware_interface::VelocityJointInterface vj_interface_;
hardware_interface::ForceTorqueSensorInterface fts_interface_;
vector6d_t joint_position_command_;
// std::vector<double> joint_velocity_command_;
vector6d_t joint_positions_;
vector6d_t joint_velocities_;
vector6d_t joint_efforts_;
vector6d_t fts_measurements_;
vector6d_t tcp_pose_;
std::bitset<18> actual_dig_out_bits_;
std::bitset<18> actual_dig_in_bits_;
std::array<double, 2> standard_analog_input_;
std::array<double, 2> standard_analog_output_;
std::bitset<4> analog_io_types_;
uint32_t tool_mode_;
std::bitset<2> tool_analog_input_types_;
std::array<double, 2> tool_analog_input_;
int32_t tool_output_voltage_;
double tool_output_current_;
double tool_temperature_;
tf2::Vector3 tcp_force_;
tf2::Vector3 tcp_torque_;
geometry_msgs::TransformStamped tcp_transform_;
double speed_scaling_;
double target_speed_fraction_;
double speed_scaling_combined_;
std::vector<std::string> joint_names_;
std::unique_ptr<realtime_tools::RealtimePublisher<tf2_msgs::TFMessage>> tcp_pose_pub_;
std::unique_ptr<realtime_tools::RealtimePublisher<ur_msgs::IOStates>> io_pub_;
std::unique_ptr<realtime_tools::RealtimePublisher<ur_msgs::ToolDataMsg>> tool_data_pub_;
ros::ServiceServer set_speed_slider_srv_;
ros::ServiceServer set_io_srv_;
ros::ServiceServer resend_robot_program_srv_;
ros::Subscriber command_sub_;
uint32_t runtime_state_;
bool position_controller_running_;
PausingState pausing_state_;
double pausing_ramp_up_increment_;
std::string tcp_link_;
bool robot_program_running_;
ros::Publisher program_state_pub_;
bool controller_reset_necessary_;
bool controllers_initialized_;
std::string robot_ip_;
std::string tf_prefix_;
};
} // namespace ur_driver
#endif // ifndef UR_DRIVER_HARDWARE_INTERFACE_H_INCLUDED

89
ur_robot_driver/include/ur_robot_driver/rtde/control_package_pause.h Normal file
View File

@@ -0,0 +1,89 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_CONTROL_PACKAGE_PAUSE_H_INCLUDED
#define UR_RTDE_DRIVER_CONTROL_PACKAGE_PAUSE_H_INCLUDED
#include "ur_robot_driver/rtde/rtde_package.h"
namespace ur_driver
{
namespace rtde_interface
{
/*!
* \brief This class handles the robot's response to a requested stop in RTDE data package
* communication.
*/
class ControlPackagePause : public RTDEPackage
{
public:
/*!
* \brief Creates a new ControlPackagePause object.
*/
ControlPackagePause() : RTDEPackage(PackageType::RTDE_CONTROL_PACKAGE_PAUSE)
{
}
virtual ~ControlPackagePause() = default;
/*!
* \brief Sets the attributes of the package by parsing a serialized representation of the
* package.
*
* \param bp A parser containing a serialized version of the package
*
* \returns True, if the package was parsed successfully, false otherwise
*/
virtual bool parseWith(comm::BinParser& bp);
/*!
* \brief Produces a human readable representation of the package object.
*
* \returns A string representing the object
*/
virtual std::string toString() const;
uint8_t accepted_;
};
/*!
* \brief This class is used to request a stop in RTDE data package communication.
*/
class ControlPackagePauseRequest : public RTDEPackage
{
public:
/*!
* \brief Creates a new ControlPackagePauseRequest object.
*/
ControlPackagePauseRequest() : RTDEPackage(PackageType::RTDE_CONTROL_PACKAGE_PAUSE)
{
}
virtual ~ControlPackagePauseRequest() = default;
};
} // namespace rtde_interface
} // namespace ur_driver
#endif // UR_RTDE_DRIVER_CONTROL_PACKAGE_PAUSE_H_INCLUDED

104
ur_robot_driver/include/ur_robot_driver/rtde/control_package_setup_inputs.h Normal file
View File

@@ -0,0 +1,104 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_CONTROL_PACKAGE_SETUP_INPUTS_H_INCLUDED
#define UR_RTDE_DRIVER_CONTROL_PACKAGE_SETUP_INPUTS_H_INCLUDED
#include "ur_robot_driver/rtde/rtde_package.h"
namespace ur_driver
{
namespace rtde_interface
{
/*!
* \brief This class handles the robot's response to a requested input recipe setup.
*/
class ControlPackageSetupInputs : public RTDEPackage
{
public:
/*!
* \brief Creates a new ControlPackageSetupInputs object.
*/
ControlPackageSetupInputs() : RTDEPackage(PackageType::RTDE_CONTROL_PACKAGE_SETUP_INPUTS)
{
}
virtual ~ControlPackageSetupInputs() = default;
/*!
* \brief Sets the attributes of the package by parsing a serialized representation of the
* package.
*
* \param bp A parser containing a serialized version of the package
*
* \returns True, if the package was parsed successfully, false otherwise
*/
virtual bool parseWith(comm::BinParser& bp);
/*!
* \brief Produces a human readable representation of the package object.
*
* \returns A string representing the object
*/
virtual std::string toString() const;
uint8_t input_recipe_id_;
std::string variable_types_;
};
/*!
* \brief This class is used to setup the input recipe as part of the initial RTDE handshake.
*/
class ControlPackageSetupInputsRequest : public RTDEPackage
{
public:
/*!
* \brief Creates a new ControlPackageSetupInputsRequest object.
*/
ControlPackageSetupInputsRequest() : RTDEPackage(PackageType::RTDE_CONTROL_PACKAGE_SETUP_INPUTS)
{
}
virtual ~ControlPackageSetupInputsRequest() = default;
/*!
* \brief Generates a serialized package.
*
* \param buffer Buffer to fill with the serialization
* \param variable_names The input recipe to set
*
* \returns The total size of the serialized package
*/
static size_t generateSerializedRequest(uint8_t* buffer, std::vector<std::string> variable_names);
std::string variable_names_;
private:
static const PackageType PACKAGE_TYPE = PackageType::RTDE_CONTROL_PACKAGE_SETUP_INPUTS;
};
} // namespace rtde_interface
} // namespace ur_driver
#endif // UR_RTDE_DRIVER_CONTROL_PACKAGE_SETUP_INPUTS_H_INCLUDED

118
ur_robot_driver/include/ur_robot_driver/rtde/control_package_setup_outputs.h Normal file
View File

@@ -0,0 +1,118 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_CONTROL_PACKAGE_SETUP_OUTPUTS_H_INCLUDED
#define UR_RTDE_DRIVER_CONTROL_PACKAGE_SETUP_OUTPUTS_H_INCLUDED
#include "ur_robot_driver/rtde/rtde_package.h"
#include "ur_robot_driver/rtde/package_header.h"
namespace ur_driver
{
namespace rtde_interface
{
/*!
* \brief This class handles the robot's response to a requested output recipe setup.
*/
class ControlPackageSetupOutputs : public RTDEPackage
{
public:
/*!
* \brief Creates a new ControlPackageSetupOutputs object.
*/
ControlPackageSetupOutputs() : RTDEPackage(PackageType::RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS)
{
}
virtual ~ControlPackageSetupOutputs() = default;
/*!
* \brief Sets the attributes of the package by parsing a serialized representation of the
* package.
*
* \param bp A parser containing a serialized version of the package
*
* \returns True, if the package was parsed successfully, false otherwise
*/
virtual bool parseWith(comm::BinParser& bp);
/*!
* \brief Produces a human readable representation of the package object.
*
* \returns A string representing the object
*/
virtual std::string toString() const;
uint8_t output_recipe_id_;
std::string variable_types_;
};
/*!
* \brief This class is used to setup the output recipe as part of the initial RTDE handshake.
*/
class ControlPackageSetupOutputsRequest : public RTDEPackage
{
public:
/*!
* \brief Creates a new ControlPackageSetupOutputsRequest object.
*/
ControlPackageSetupOutputsRequest() : RTDEPackage(PackageType::RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS)
{
}
virtual ~ControlPackageSetupOutputsRequest() = default;
/*!
* \brief Generates a serialized package.
*
* \param buffer Buffer to fill with the serialization
* \param output_frequency Frequency of data packages to be sent by the robot
* \param variable_names The output recipe to set
*
* \returns The total size of the serialized package
*/
static size_t generateSerializedRequest(uint8_t* buffer, double output_frequency,
std::vector<std::string> variable_names);
/*!
* \brief Generates a serialized package.
*
* \param buffer Buffer to fill with the serialization
* \param variable_names The output recipe to set
*
* \returns The total size of the serialized package
*/
static size_t generateSerializedRequest(uint8_t* buffer, std::vector<std::string> variable_names);
double output_frequency_;
std::string variable_names_;
private:
static const PackageType PACKAGE_TYPE = PackageType::RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS;
};
} // namespace rtde_interface
} // namespace ur_driver
#endif // UR_RTDE_DRIVER_CONTROL_PACKAGE_SETUP_OUTPUTS_H_INCLUDED

101
ur_robot_driver/include/ur_robot_driver/rtde/control_package_start.h Normal file
View File

@@ -0,0 +1,101 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_CONTROL_PACKAGE_START_H_INCLUDED
#define UR_RTDE_DRIVER_CONTROL_PACKAGE_START_H_INCLUDED
#include "ur_robot_driver/rtde/rtde_package.h"
namespace ur_driver
{
namespace rtde_interface
{
/*!
* \brief This class handles the robot's response to a requested start in RTDE data package
* communication.
*/
class ControlPackageStart : public RTDEPackage
{
public:
/*!
* \brief Creates a new ControlPackageStart object.
*/
ControlPackageStart() : RTDEPackage(PackageType::RTDE_CONTROL_PACKAGE_START)
{
}
virtual ~ControlPackageStart() = default;
/*!
* \brief Sets the attributes of the package by parsing a serialized representation of the
* package.
*
* \param bp A parser containing a serialized version of the package
*
* \returns True, if the package was parsed successfully, false otherwise
*/
virtual bool parseWith(comm::BinParser& bp);
/*!
* \brief Produces a human readable representation of the package object.
*
* \returns A string representing the object
*/
virtual std::string toString() const;
uint8_t accepted_;
};
/*!
* \brief This class is used to request a stop in RTDE data package communication.
*/
class ControlPackageStartRequest : public RTDEPackage
{
public:
/*!
* \brief Creates a new ControlPackageStartRequest object.
*/
ControlPackageStartRequest() : RTDEPackage(PackageType::RTDE_CONTROL_PACKAGE_START)
{
}
virtual ~ControlPackageStartRequest() = default;
/*!
* \brief Generates a serialized package.
*
* \param buffer Buffer to fill with the serialization
*
* \returns The total size of the serialized package
*/
static size_t generateSerializedRequest(uint8_t* buffer);
private:
static const uint16_t PAYLOAD_SIZE = 0;
};
} // namespace rtde_interface
} // namespace ur_driver
#endif // UR_RTDE_DRIVER_CONTROL_PACKAGE_START_H_INCLUDED

237
ur_robot_driver/include/ur_robot_driver/rtde/data_package.h Normal file
View File

@@ -0,0 +1,237 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Lea Steffen steffen@fzi.de
* \date 2019-04-01
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_DATA_PACKAGE_H_INCLUDED
#define UR_RTDE_DRIVER_DATA_PACKAGE_H_INCLUDED
#include <unordered_map>
#include "ur_robot_driver/types.h"
#include "ur_robot_driver/rtde/rtde_package.h"
#include <boost/variant.hpp>
namespace ur_driver
{
namespace rtde_interface
{
/*!
* \brief Possible values for the runtime state
*/
enum class RUNTIME_STATE : uint32_t
{
STOPPING = 0,
STOPPED = 1,
PLAYING = 2,
PAUSING = 3,
PAUSED = 4,
RESUMING = 5
};
/*!
* \brief The DataPackage class handles communication in the form of RTDE data packages both to and
* from the robot. It contains functionality to parse and serialize packages for arbitrary recipes.
*/
class DataPackage : public RTDEPackage
{
public:
using _rtde_type_variant = boost::variant<bool, uint8_t, uint32_t, uint64_t, int32_t, double, vector3d_t, vector6d_t,
vector6int32_t, vector6uint32_t, std::string>;
DataPackage() = delete;
/*!
* \brief Creates a new DataPackage object, based on a given recipe.
*
* \param recipe The used recipe
*/
DataPackage(const std::vector<std::string>& recipe) : RTDEPackage(PackageType::RTDE_DATA_PACKAGE), recipe_(recipe)
{
}
virtual ~DataPackage() = default;
/*!
* \brief Initializes to contained list with empty values based on the recipe.
*/
void initEmpty();
/*!
* \brief Sets the attributes of the package by parsing a serialized representation of the
* package.
*
* \param bp A parser containing a serialized version of the package
*
* \returns True, if the package was parsed successfully, false otherwise
*/
virtual bool parseWith(comm::BinParser& bp);
/*!
* \brief Produces a human readable representation of the package object.
*
* \returns A string representing the object
*/
virtual std::string toString() const;
/*!
* \brief Serializes the package.
*
* \param buffer Buffer to fill with the serialization
*
* \returns The total size of the serialized package
*/
size_t serializePackage(uint8_t* buffer);
/*!
* \brief Get a data field from the DataPackage.
*
* The data package contains a lot of different data fields, depending on the recipe.
*
* \param name The string identifier for the data field as used in the documentation.
* \param val Target variable. Make sure, it's the correct type.
* \exception boost::bad_get if the type under given \p name does not match the template type T.
*
* \returns True on success, false if the field cannot be found inside the package.
*/
template <typename T>
bool getData(const std::string& name, T& val)
{
if (data_.find(name) != data_.end())
{
val = boost::strict_get<T>(data_[name]);
}
else
{
return false;
}
return true;
}
/*!
* \brief Get a data field from the DataPackage as bitset
*
* The data package contains a lot of different data fields, depending on the recipe.
*
* \param name The string identifier for the data field as used in the documentation.
* \param val Target variable. Make sure, it's the correct type.
* \exception boost::bad_get if the type under given \p name does not match the template type T.
*
* \returns True on success, false if the field cannot be found inside the package.
*/
template <typename T, size_t N>
bool getData(const std::string& name, std::bitset<N>& val)
{
static_assert(sizeof(T) * 8 >= N, "Bitset is too large for underlying variable");
if (data_.find(name) != data_.end())
{
val = std::bitset<N>(boost::strict_get<T>(data_[name]));
}
else
{
return false;
}
return true;
}
/*!
* \brief Set a data field in the DataPackage.
*
* The data package contains a lot of different data fields, depending on the recipe.
*
* \param name The string identifier for the data field as used in the documentation.
* \param val Value to set. Make sure, it's the correct type.
*
* \returns True on success, false if the field cannot be found inside the package.
*/
template <typename T>
bool setData(const std::string& name, T& val)
{
if (data_.find(name) != data_.end())
{
data_[name] = val;
}
else
{
return false;
}
return true;
}
/*!
* \brief Setter of the recipe id value used to identify the used recipe to the robot.
*
* \param recipe_id The new value
*/
void setRecipeID(const uint8_t& recipe_id)
{
recipe_id_ = recipe_id;
}
private:
// Const would be better here
static std::unordered_map<std::string, _rtde_type_variant> g_type_list;
uint8_t recipe_id_;
std::unordered_map<std::string, _rtde_type_variant> data_;
std::vector<std::string> recipe_;
struct ParseVisitor : public boost::static_visitor<>
{
template <typename T>
void operator()(T& d, comm::BinParser& bp) const
{
bp.parse(d);
}
};
struct StringVisitor : public boost::static_visitor<std::string>
{
template <typename T>
std::string operator()(T& d) const
{
std::stringstream ss;
ss << d;
return ss.str();
}
};
struct SizeVisitor : public boost::static_visitor<uint16_t>
{
template <typename T>
uint16_t operator()(T& d) const
{
return sizeof(d);
}
};
struct SerializeVisitor : public boost::static_visitor<size_t>
{
template <typename T>
size_t operator()(T& d, uint8_t* buffer) const
{
return comm::PackageSerializer::serialize(buffer, d);
}
};
};
} // namespace rtde_interface
} // namespace ur_driver
#endif // ifndef UR_RTDE_DRIVER_DATA_PACKAGE_H_INCLUDED

106
ur_robot_driver/include/ur_robot_driver/rtde/get_urcontrol_version.h Normal file
View File

@@ -0,0 +1,106 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_GET_URCONTROL_VERSION_H_INCLUDED
#define UR_RTDE_DRIVER_GET_URCONTROL_VERSION_H_INCLUDED
#include "ur_robot_driver/rtde/rtde_package.h"
#include "ur_robot_driver/ur/version_information.h"
namespace ur_driver
{
namespace rtde_interface
{
/*!
* \brief This class handles the package detailing the UR control version sent by the robot.
*/
class GetUrcontrolVersion : public RTDEPackage
{
public:
/*!
* \brief Creates a new GetUrcontrolVersion object.
*/
GetUrcontrolVersion() : RTDEPackage(PackageType::RTDE_GET_URCONTROL_VERSION)
{
}
virtual ~GetUrcontrolVersion() = default;
/*!
* \brief Sets the attributes of the package by parsing a serialized representation of the
* package.
*
* \param bp A parser containing a serialized version of the package
*
* \returns True, if the package was parsed successfully, false otherwise
*/
virtual bool parseWith(comm::BinParser& bp);
/*!
* \brief Produces a human readable representation of the package object.
*
* \returns A string representing the object
*/
virtual std::string toString() const;
VersionInformation version_information_;
uint32_t major_;
uint32_t minor_;
uint32_t bugfix_;
uint32_t build_;
};
/*!
* \brief This class is used to request the used UR control version from the robot.
*/
class GetUrcontrolVersionRequest : public RTDEPackage
{
public:
/*!
* \brief Creates a new GetUrcontrolVersionRequest object.
*/
GetUrcontrolVersionRequest() : RTDEPackage(PackageType::RTDE_GET_URCONTROL_VERSION)
{
}
virtual ~GetUrcontrolVersionRequest() = default;
/*!
* \brief Generates a serialized package.
*
* \param buffer Buffer to fill with the serialization
*
* \returns The total size of the serialized package
*/
static size_t generateSerializedRequest(uint8_t* buffer);
private:
static const uint16_t PAYLOAD_SIZE = 0;
};
} // namespace rtde_interface
} // namespace ur_driver
#endif // UR_RTDE_DRIVER_GET_URCONTROL_VERSION_H_INCLUDED

100
ur_robot_driver/include/ur_robot_driver/rtde/package_header.h Normal file
View File

@@ -0,0 +1,100 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Lea Steffen steffen@fzi.de
* \date 2019-04-01
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_RTDE__HEADER_H_INCLUDED
#define UR_RTDE_DRIVER_RTDE__HEADER_H_INCLUDED
#include <cstddef>
#include <endian.h>
#include "ur_robot_driver/types.h"
#include "ur_robot_driver/comm/package_serializer.h"
namespace ur_driver
{
namespace rtde_interface
{
/*!
* \brief Possible package types
*/
enum class PackageType : uint8_t
{
RTDE_REQUEST_PROTOCOL_VERSION = 86, // ascii V
RTDE_GET_URCONTROL_VERSION = 118, // ascii v
RTDE_TEXT_MESSAGE = 77, // ascii M
RTDE_DATA_PACKAGE = 85, // ascii U
RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS = 79, // ascii O
RTDE_CONTROL_PACKAGE_SETUP_INPUTS = 73, // ascii I
RTDE_CONTROL_PACKAGE_START = 83, // ascii S
RTDE_CONTROL_PACKAGE_PAUSE = 80 // ascii P
};
/*!
* \brief This class represents the header for RTDE packages.
*/
class PackageHeader
{
public:
PackageHeader() = default;
virtual ~PackageHeader() = default;
using _package_size_type = uint16_t;
/*!
* \brief Reads a buffer, interpreting the next bytes as the size of the contained package.
*
* \param buf The given byte stream containing a serialized package
*
* \returns The size of the given serialized package
*/
static size_t getPackageLength(uint8_t* buf)
{
return be16toh(*(reinterpret_cast<_package_size_type*>(buf)));
}
/*!
* \brief Creates a serialization of a header based on given values.
*
* \param buffer The buffer to write the serialization to
* \param package_type The type of the package
* \param payload_length The length of the package's payload
*
* \returns
*/
static size_t serializeHeader(uint8_t* buffer, PackageType package_type, uint16_t payload_length)
{
uint16_t header_size = sizeof(_package_size_type) + sizeof(PackageType);
uint16_t size = header_size + payload_length;
comm::PackageSerializer::serialize(buffer, size);
comm::PackageSerializer::serialize(buffer + sizeof(size), package_type);
return header_size;
}
};
} // namespace rtde_interface
} // namespace ur_driver
#endif // #ifndef UR_RTDE_DRIVER_RTDE__HEADER_H_INCLUDED

106
ur_robot_driver/include/ur_robot_driver/rtde/request_protocol_version.h Normal file
View File

@@ -0,0 +1,106 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_REQUEST_PROTOCOL_VERSION_H_INCLUDED
#define UR_RTDE_DRIVER_REQUEST_PROTOCOL_VERSION_H_INCLUDED
#include "ur_robot_driver/rtde/rtde_package.h"
#include "ur_robot_driver/rtde/package_header.h"
namespace ur_driver
{
namespace rtde_interface
{
/*!
* \brief This class handles the robot's response after trying to set the used RTDE protocol version.
*/
class RequestProtocolVersion : public RTDEPackage
{
public:
/*!
* \brief Creates a new RequestProtocolVersion object.
*/
RequestProtocolVersion() : RTDEPackage(PackageType::RTDE_REQUEST_PROTOCOL_VERSION)
{
}
virtual ~RequestProtocolVersion() = default;
/*!
* \brief Sets the attributes of the package by parsing a serialized representation of the
* package.
*
* \param bp A parser containing a serialized version of the package
*
* \returns True, if the package was parsed successfully, false otherwise
*/
virtual bool parseWith(comm::BinParser& bp);
/*!
* \brief Produces a human readable representation of the package object.
*
* \returns A string representing the object
*/
virtual std::string toString() const;
uint8_t accepted_;
};
/*!
* \brief This class handles producing a request towards the robot to use a specific RTDE protocol
* version.
*/
class RequestProtocolVersionRequest : public RTDEPackage
{
public:
/*!
* \brief Creates a new RequestProtocolVersionRequest object.
*/
RequestProtocolVersionRequest() : RTDEPackage(PackageType::RTDE_REQUEST_PROTOCOL_VERSION)
{
}
virtual ~RequestProtocolVersionRequest() = default;
/*!
* \brief Generates a serialized package.
*
* \param buffer Buffer to fill with the serialization
* \param version RTDE protocol version to request usage of
*
* \returns The total size of the serialized package
*/
static size_t generateSerializedRequest(uint8_t* buffer, uint16_t version);
uint16_t protocol_version_;
private:
static const uint16_t PAYLOAD_SIZE = sizeof(uint16_t);
};
} // namespace rtde_interface
} // namespace ur_driver
#endif // UR_RTDE_DRIVER_REQUEST_PROTOCOL_VERSION_H_INCLUDED

150
ur_robot_driver/include/ur_robot_driver/rtde/rtde_client.h Normal file
View File

@@ -0,0 +1,150 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-04-10
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_RTDE_CLIENT_H_INCLUDED
#define UR_RTDE_DRIVER_RTDE_CLIENT_H_INCLUDED
#include "ur_robot_driver/comm/pipeline.h"
#include "ur_robot_driver/rtde/package_header.h"
#include "ur_robot_driver/rtde/rtde_package.h"
#include "ur_robot_driver/comm/stream.h"
#include "ur_robot_driver/rtde/rtde_parser.h"
#include "ur_robot_driver/comm/producer.h"
#include "ur_robot_driver/rtde/data_package.h"
#include "ur_robot_driver/rtde/request_protocol_version.h"
#include "ur_robot_driver/rtde/control_package_setup_outputs.h"
#include "ur_robot_driver/rtde/control_package_start.h"
#include "ur_robot_driver/log.h"
#include "ur_robot_driver/rtde/rtde_writer.h"
static const int UR_RTDE_PORT = 30004;
static const std::string PIPELINE_NAME = "RTDE Data Pipeline";
namespace ur_driver
{
namespace rtde_interface
{
/*!
* \brief The RTDEClient class manages communication over the RTDE interface. It contains the RTDE
* handshake and read and write functionality to and from the robot.
*/
class RTDEClient
{
public:
RTDEClient() = delete;
/*!
* \brief Creates a new RTDEClient object, including a used URStream and Pipeline to handle the
* communication with the robot.
*
* \param robot_ip The IP of the robot
* \param notifier The notifier to use in the pipeline
* \param output_recipe_file Path to the file containing the output recipe
* \param input_recipe_file Path to the file containing the input recipe
*/
RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& output_recipe_file,
const std::string& input_recipe_file);
~RTDEClient() = default;
/*!
* \brief Sets up RTDE communication with the robot. The handshake includes negotiation of the
* used protocol version and setting of input and output recipes.
*
* \returns Success of the handshake
*/
bool init();
/*!
* \brief Triggers the robot to start sending RTDE data packages in the negotiated format.
*
* \returns Success of the requested start
*/
bool start();
/*!
* \brief Reads the pipeline to fetch the next data package.
*
* \param timeout Time to wait if no data package is currently in the queue
*
* \returns Unique ptr to the package, if a package was fetched successfully, nullptr otherwise
*/
std::unique_ptr<rtde_interface::DataPackage> getDataPackage(std::chrono::milliseconds timeout);
/*!
* \brief Getter for the frequency the robot will publish RTDE data packages with.
*
* \returns The used frequency
*/
double getMaxFrequency() const
{
return max_frequency_;
}
/*!
* \brief Getter for the UR control version received from the robot.
*
* \returns The VersionInformation received from the robot
*/
VersionInformation getVersion()
{
return urcontrol_version_;
}
/*!
* \brief Returns the IP address (of the machine running this driver) used for the socket connection.
*
* \returns The IP address as a string (e.g. "192.168.0.1")
*/
std::string getIP() const;
/*!
* \brief Getter for the RTDE writer, which is used to send data via the RTDE interface to the
* robot.
*
* \returns A reference to the used RTDEWriter
*/
RTDEWriter& getWriter();
private:
comm::URStream<PackageHeader> stream_;
std::vector<std::string> recipe_;
std::vector<std::string> input_recipe_;
RTDEParser parser_;
comm::URProducer<PackageHeader> prod_;
comm::Pipeline<PackageHeader> pipeline_;
RTDEWriter writer_;
VersionInformation urcontrol_version_;
double max_frequency_;
constexpr static const double CB3_MAX_FREQUENCY = 125.0;
constexpr static const double URE_MAX_FREQUENCY = 500.0;
std::vector<std::string> readRecipe(const std::string& recipe_file);
};
} // namespace rtde_interface
} // namespace ur_driver
#endif // UR_RTDE_DRIVER_RTDE_CLIENT_H_INCLUDED

79
ur_robot_driver/include/ur_robot_driver/rtde/rtde_package.h Normal file
View File

@@ -0,0 +1,79 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Lea Steffen steffen@fzi.de
* \date 2019-04-01
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_RTDE_PACKAGE_H_INCLUDED
#define UR_RTDE_DRIVER_RTDE_PACKAGE_H_INCLUDED
#include "ur_robot_driver/rtde/package_header.h"
#include "ur_robot_driver/comm/package.h"
namespace ur_driver
{
namespace rtde_interface
{
/*!
* brief The RTDEPackage is solely an abstraction level.
* It inherits form the URPackage and is also a parent class for rtde_interface::TextMessage.
*/
class RTDEPackage : public comm::URPackage<PackageHeader>
{
public:
/*!
* \brief Creates a new RTDEPackage object.
*/
RTDEPackage() = delete;
RTDEPackage(const PackageType type) : type_(type)
{
}
virtual ~RTDEPackage() = default;
/*!
* \brief Sets the attributes of the package by parsing a serialized representation of the
* package.
*
* \param bp A parser containing a serialized version of the package
*
* \returns True, if the package was parsed successfully, false otherwise
*/
virtual bool parseWith(comm::BinParser& bp);
/*!
* \brief Produces a human readable representation of the package object.
*
* \returns A string representing the object
*/
virtual std::string toString() const;
protected:
std::unique_ptr<uint8_t> buffer_;
size_t buffer_length_;
PackageType type_;
};
} // namespace rtde_interface
} // namespace ur_driver
#endif // UR_RTDE_DRIVER_RTDE_PACKAGE_H_INCLUDED

159
ur_robot_driver/include/ur_robot_driver/rtde/rtde_parser.h Normal file
View File

@@ -0,0 +1,159 @@
/*
* Copyright 2019, FZI Forschungszentrum Informatik (refactor)
*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#pragma once
#include <vector>
#include "ur_robot_driver/comm/parser.h"
#include "ur_robot_driver/comm/bin_parser.h"
#include "ur_robot_driver/comm/pipeline.h"
#include "ur_robot_driver/rtde/control_package_pause.h"
#include "ur_robot_driver/rtde/control_package_setup_inputs.h"
#include "ur_robot_driver/rtde/control_package_setup_outputs.h"
#include "ur_robot_driver/rtde/control_package_start.h"
#include "ur_robot_driver/rtde/data_package.h"
#include "ur_robot_driver/rtde/get_urcontrol_version.h"
#include "ur_robot_driver/rtde/package_header.h"
#include "ur_robot_driver/rtde/request_protocol_version.h"
#include "ur_robot_driver/rtde/text_message.h"
namespace ur_driver
{
namespace rtde_interface
{
/*!
* \brief The RTDE specific parser. Interprets a given byte stream as serialized RTDE packages
* and parses it accordingly.
*/
class RTDEParser : public comm::Parser<PackageHeader>
{
public:
RTDEParser() = delete;
/*!
* \brief Creates a new RTDEParser object, registering the used recipe.
*
* \param recipe The recipe used in RTDE data communication
*/
RTDEParser(const std::vector<std::string>& recipe) : recipe_(recipe), protocol_version_(1)
{
}
virtual ~RTDEParser() = default;
/*!
* \brief Uses the given BinParser to create package objects from the contained serialization.
*
* \param bp A BinParser holding one or more serialized RTDE packages
* \param results A vector of pointers to created RTDE package objects
*
* \returns True, if the byte stream could successfully be parsed as RTDE packages, false
* otherwise
*/
bool parse(comm::BinParser& bp, std::vector<std::unique_ptr<comm::URPackage<PackageHeader>>>& results)
{
PackageHeader::_package_size_type size;
PackageType type;
bp.parse(size);
bp.parse(type);
if (!bp.checkSize(size - sizeof(size) - sizeof(type)))
{
LOG_ERROR("Buffer len shorter than expected packet length");
return false;
}
switch (type)
{
case PackageType::RTDE_DATA_PACKAGE:
{
std::unique_ptr<RTDEPackage> package(new DataPackage(recipe_));
if (!package->parseWith(bp))
{
LOG_ERROR("Package parsing of type %d failed!", static_cast<int>(type));
return false;
}
results.push_back(std::move(package));
break;
}
default:
{
std::unique_ptr<RTDEPackage> package(packageFromType(type));
if (!package->parseWith(bp))
{
LOG_ERROR("Package parsing of type %d failed!", static_cast<int>(type));
return false;
}
results.push_back(std::move(package));
break;
}
}
if (!bp.empty())
{
LOG_ERROR("Package of type %d was not parsed completely!", static_cast<int>(type));
bp.debug();
return false;
}
return true;
}
void setProtocolVersion(uint16_t protocol_version)
{
protocol_version_ = protocol_version;
}
private:
std::vector<std::string> recipe_;
RTDEPackage* packageFromType(PackageType type)
{
switch (type)
{
case PackageType::RTDE_TEXT_MESSAGE:
return new TextMessage(protocol_version_);
break;
case PackageType::RTDE_GET_URCONTROL_VERSION:
return new GetUrcontrolVersion;
break;
case PackageType::RTDE_REQUEST_PROTOCOL_VERSION:
return new RequestProtocolVersion;
break;
case PackageType::RTDE_CONTROL_PACKAGE_PAUSE:
return new ControlPackagePause;
break;
case PackageType::RTDE_CONTROL_PACKAGE_SETUP_INPUTS:
return new ControlPackageSetupInputs;
break;
case PackageType::RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS:
return new ControlPackageSetupOutputs;
break;
case PackageType::RTDE_CONTROL_PACKAGE_START:
return new ControlPackageStart;
break;
default:
return new RTDEPackage(type);
}
}
uint16_t protocol_version_;
};
} // namespace rtde_interface
} // namespace ur_driver

128
ur_robot_driver/include/ur_robot_driver/rtde/rtde_writer.h Normal file
View File

@@ -0,0 +1,128 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-07-25
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_RTDE_WRITER_H_INCLUDED
#define UR_RTDE_DRIVER_RTDE_WRITER_H_INCLUDED
#include "ur_robot_driver/rtde/package_header.h"
#include "ur_robot_driver/rtde/rtde_package.h"
#include "ur_robot_driver/rtde/data_package.h"
#include "ur_robot_driver/comm/stream.h"
#include "ur_robot_driver/queue/readerwriterqueue.h"
#include <thread>
namespace ur_driver
{
namespace rtde_interface
{
/*!
* \brief The RTDEWriter class offers an abstraction layer to send data to the robot via the RTDE
* interface. Several simple to use functions to create data packages to send exist, which are
* then sent to the robot in an additional thread.
*/
class RTDEWriter
{
public:
RTDEWriter() = delete;
/*!
* \brief Creates a new RTDEWriter object using a given URStream and recipe.
*
* \param stream The URStream to use for communication with the robot
* \param recipe The recipe to use for communication
*/
RTDEWriter(comm::URStream<PackageHeader>* stream, const std::vector<std::string>& recipe);
~RTDEWriter() = default;
/*!
* \brief Starts the writer thread, which periodically clears the queue to write packages to the
* robot.
*
* \param recipe_id The recipe id to use, so the robot correctly identifies the used recipe
*/
void init(uint8_t recipe_id);
/*!
* \brief The writer thread loop, continually serializing and sending packages to the robot.
*/
void run();
/*!
* \brief Creates a package to request setting a new value for the speed slider.
*
* \param speed_slider_fraction The new speed slider fraction as a value between 0.0 and 1.0
*
* \returns Success of the package creation
*/
bool sendSpeedSlider(double speed_slider_fraction);
/*!
* \brief Creates a package to request setting a new value for one of the standard digital output pins.
*
* \param output_pin The pin to change
* \param value The new value
*
* \returns Success of the package creation
*/
bool sendStandardDigitalOutput(uint8_t output_pin, bool value);
/*!
* \brief Creates a package to request setting a new value for one of the configurable digital output pins.
*
* \param output_pin The pin to change
* \param value The new value
*
* \returns Success of the package creation
*/
bool sendConfigurableDigitalOutput(uint8_t output_pin, bool value);
/*!
* \brief Creates a package to request setting a new value for one of the tool output pins.
*
* \param output_pin The pin to change
* \param value The new value
*
* \returns Success of the package creation
*/
bool sendToolDigitalOutput(uint8_t output_pin, bool value);
/*!
* \brief Creates a package to request setting a new value for one of the standard analog output pins.
*
* \param output_pin The pin to change
* \param value The new value
*
* \returns Success of the package creation
*/
bool sendStandardAnalogOutput(uint8_t output_pin, double value);
private:
uint8_t pinToMask(uint8_t pin);
comm::URStream<PackageHeader>* stream_;
std::vector<std::string> recipe_;
uint8_t recipe_id_;
moodycamel::BlockingReaderWriterQueue<std::unique_ptr<DataPackage>> queue_;
std::thread writer_thread_;
};
} // namespace rtde_interface
} // namespace ur_driver
#endif // UR_RTDE_DRIVER_RTDE_WRITER_H_INCLUDED

84
ur_robot_driver/include/ur_robot_driver/rtde/text_message.h Normal file
View File

@@ -0,0 +1,84 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Lea Steffen steffen@fzi.de
* \date 2019-04-01
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_TEXT_MESSAGE_H_INCLUDED
#define UR_RTDE_DRIVER_TEXT_MESSAGE_H_INCLUDED
#include "ur_robot_driver/rtde/rtde_package.h"
namespace ur_driver
{
namespace rtde_interface
{
/*!
* \brief This class handles RTDE text messages sent by the robot.
*/
class TextMessage : public RTDEPackage
{
public:
/*!
* \brief Creates a new TextMessage object.
*
* \param protocol_version Protocol version used for the RTDE communication
*/
TextMessage(uint16_t protocol_version)
: RTDEPackage(PackageType::RTDE_TEXT_MESSAGE), protocol_version_(protocol_version)
{
}
virtual ~TextMessage() = default;
/*!
* \brief Sets the attributes of the package by parsing a serialized representation of the
* package.
*
* \param bp A parser containing a serialized version of the package
*
* \returns True, if the package was parsed successfully, false otherwise
*/
virtual bool parseWith(comm::BinParser& bp);
/*!
* \brief Produces a human readable representation of the package object.
*
* \returns A string representing the object
*/
virtual std::string toString() const;
uint8_t message_length_;
std::string message_;
uint8_t source_length_;
std::string source_;
uint8_t warning_level_;
uint8_t message_type_;
uint16_t protocol_version_;
};
} // namespace rtde_interface
} // namespace ur_driver
#endif // UR_RTDE_DRIVER_TEXT_MESSAGE_H_INCLUDED

49
ur_robot_driver/include/ur_robot_driver/types.h Normal file
View File

@@ -0,0 +1,49 @@
/*
* Copyright 2019, FZI Forschungszentrum Informatik (refactor)
*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#pragma once
#include <inttypes.h>
#include <array>
#include <iostream>
namespace ur_driver
{
using vector3d_t = std::array<double, 3>;
using vector6d_t = std::array<double, 6>;
using vector6int32_t = std::array<int32_t, 6>;
using vector6uint32_t = std::array<uint32_t, 6>;
template <class T, std::size_t N>
std::ostream& operator<<(std::ostream& out, const std::array<T, N>& item)
{
out << "[";
for (size_t i = 0; i < item.size(); ++i)
{
out << item[i];
if (i != item.size() - 1)
{
out << ", ";
}
}
out << "]";
return out;
}
} // namespace ur_driver

104
ur_robot_driver/include/ur_robot_driver/ur/calibration_checker.h Normal file
View File

@@ -0,0 +1,104 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-06-14
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_UR_CALIBRATION_CHECKER_H_INCLUDED
#define UR_RTDE_DRIVER_UR_CALIBRATION_CHECKER_H_INCLUDED
#include <ur_robot_driver/comm/pipeline.h>
#include <ur_robot_driver/primary/robot_state/kinematics_info.h>
namespace ur_driver
{
/*!
* \brief The CalibrationChecker class consumes primary packages ignoring all but KinematicsInfo
* packages. These are then checked against the used kinematics to see if the correct calibration
* is used.
*/
class CalibrationChecker : public comm::IConsumer<comm::URPackage<primary_interface::PackageHeader>>
{
public:
/*!
* \brief Creates a new CalibrationChecker object with an expected hash calculated from the used
* kinematics.
*
* \param expected_hash The expected kinematics hash
*/
CalibrationChecker(const std::string& expected_hash);
virtual ~CalibrationChecker() = default;
/*!
* \brief Empty setup function, as no setup is needed.
*/
virtual void setupConsumer()
{
}
/*!
* \brief Tears down the consumer.
*/
virtual void teardownConsumer()
{
}
/*!
* \brief Stops the consumer.
*/
virtual void stopConsumer()
{
}
/*!
* \brief Handles timeouts.
*/
virtual void onTimeout()
{
}
/*!
* \brief Consumes a package, checking its hash if it is a KinematicsInfo package. If the hash
* does not match the expected hash, an error is logged.
*
* \param product The package to consume
*
* \returns True, if the package was consumed correctly
*/
virtual bool consume(std::shared_ptr<comm::URPackage<primary_interface::PackageHeader>> product);
/*!
* \brief Used to make sure the calibration check is not performed several times.
*
* \returns True, if the calibration was already checked, false otherwise
*/
bool isChecked()
{
return checked_;
}
private:
std::string expected_hash_;
bool checked_;
};
} // namespace ur_driver
#endif // ifndef UR_RTDE_DRIVER_UR_CALIBRATION_CHECKER_H_INCLUDED

241
ur_robot_driver/include/ur_robot_driver/ur/tool_communication.h Normal file
View File

@@ -0,0 +1,241 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-06-06
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_UR_TOOL_COMMUNICATION_H_INCLUDED
#define UR_RTDE_DRIVER_UR_TOOL_COMMUNICATION_H_INCLUDED
#include "ur_robot_driver/types.h"
#include <set>
namespace ur_driver
{
/*!
* \brief Possible values for the tool voltage
*/
enum class ToolVoltage : int
{
OFF = 0, ///< 0V
_12V = 12, ///< 12V
_24V = 24 ///< 24V
};
/*!
* \brief Possible values for th parity flag
*/
enum class Parity : int
{
NONE = 0,
ODD = 1,
EVEN = 2
};
/*!
* \brief Helper class that represents a numeric value with a lower and an upper boundary.
*
* @tparam T any type for which a comparison exists.
*/
template <class T>
class Limited
{
public:
Limited() = delete;
~Limited() = default;
using Datatype = T;
/*!
* \brief Create a new Limited object
*
* \param lower Lower boundary used for this Limited object
* \param upper Upper boundary used for this Limited object
*/
Limited(const T lower, const T upper) : lower_(lower), upper_(upper)
{
data_ = lower_;
}
/*!
* \brief Set the data field with a given value
*
* If the given value is out of the configured range, an exception is thrown.
*
* \param data
*/
void setData(const T data)
{
if (data >= lower_ && data <= upper_)
{
data_ = data;
}
else
{
throw std::runtime_error("Given data is out of range");
}
}
/*!
* \brief Returns the data stored in this object
*/
T getData() const
{
return data_;
}
private:
T data_;
const T lower_;
const T upper_;
};
/*!
* \brief Class holding a tool communication configuration
*/
class ToolCommSetup
{
public:
ToolCommSetup();
~ToolCommSetup() = default;
using StopBitsT = Limited<uint32_t>;
using RxIdleCharsT = Limited<float>;
using TxIdleCharsT = Limited<float>;
/*!
* \brief Setup the tool voltage that will be configured on the robot. This will not immediately
* change values on the robot, it will only be stored inside the ToolCommSetup object.
*/
void setToolVoltage(const ToolVoltage tool_voltage)
{
tool_voltage_ = tool_voltage;
}
/*!
* \brief Return the tool voltage currently stored
*/
ToolVoltage getToolVoltage() const
{
return tool_voltage_;
}
/*!
* \brief Setup the tool communication parity that will be configured on the robot. This will not immediately
* change values on the robot, it will only be stored inside the ToolCommSetup object.
*/
void setParity(const Parity parity)
{
parity_ = parity;
}
/*!
* \brief Return the parity currently stored
*/
Parity getParity() const
{
return parity_;
}
/*!
* \brief Setup the tool communication baud rate that will be configured on the robot. This will not immediately
* change values on the robot, it will only be stored inside the ToolCommSetup object.
*
* \param baud_rate must be one of baud_rates_allowed_ or an exception will be thrown
*/
void setBaudRate(const uint32_t baud_rate);
/*!
* \brief Return the baud rate currently stored
*/
uint32_t getBaudRate() const
{
return baud_rate_;
};
/*!
* \brief Setup the tool communication number of stop bits that will be configured on the robot. This will not
* immediately change values on the robot, it will only be stored inside the ToolCommSetup object.
*
* \param stop_bits must be inside [1,2] or this will throw an exception.
*/
void setStopBits(const StopBitsT::Datatype stop_bits)
{
stop_bits_.setData(stop_bits);
}
/*!
* \brief Return the number of stop bits currently stored
*/
StopBitsT::Datatype getStopBits() const
{
return stop_bits_.getData();
}
/*!
* \brief Setup the tool communication number of idle chars for the rx channel that will be configured on the robot.
* This will not immediately change values on the robot, it will only be stored inside the ToolCommSetup object.
*
* \param rx_idle_chars must be inside [1.0, 40] or this will throw an exception.
*/
void setRxIdleChars(const RxIdleCharsT::Datatype rx_idle_chars)
{
rx_idle_chars_.setData(rx_idle_chars);
}
/*!
* \brief Return the number of rx idle chars currently stored
*/
RxIdleCharsT::Datatype getRxIdleChars() const
{
return rx_idle_chars_.getData();
}
/*!
* \brief Setup the tool communication number of idle chars for the tx channel that will be configured on the robot.
* This will not immediately change values on the robot, it will only be stored inside the ToolCommSetup object.
*
* \param tx_idle_chars must be inside [0.0, 40] or this will throw an exception.
*/
void setTxIdleChars(const TxIdleCharsT::Datatype tx_idle_chars)
{
tx_idle_chars_.setData(tx_idle_chars);
}
/*!
* \brief Return the number of tx idle chars currently stored
*/
TxIdleCharsT::Datatype getTxIdleChars() const
{
return tx_idle_chars_.getData();
}
private:
const std::set<uint32_t> baud_rates_allowed_{ 9600, 19200, 38400, 57600, 115200, 10000000, 2000000, 5000000 };
ToolVoltage tool_voltage_;
Parity parity_;
uint32_t baud_rate_;
StopBitsT stop_bits_;
RxIdleCharsT rx_idle_chars_;
TxIdleCharsT tx_idle_chars_;
};
} // namespace ur_driver
#endif // ifndef UR_RTDE_DRIVER_UR_TOOL_COMMUNICATION_H_INCLUDED

194
ur_robot_driver/include/ur_robot_driver/ur/ur_driver.h Normal file
View File

@@ -0,0 +1,194 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-11
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_UR_UR_DRIVER_H_INCLUDED
#define UR_RTDE_DRIVER_UR_UR_DRIVER_H_INCLUDED
#include "ur_robot_driver/rtde/rtde_client.h"
#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/primary/robot_message/version_message.h"
#include "ur_robot_driver/rtde/rtde_writer.h"
namespace ur_driver
{
/*!
* \brief This is the main class for interfacing the driver.
*
* It sets up all the necessary socket connections and handles the data exchange with the robot.
* Use this classes methods to access and write data.
*/
class UrDriver
{
public:
/*!
* \brief Constructs a new UrDriver object.
*
* \param robot_ip IP-address under which the robot is reachable.
* \param script_file URScript file that should be sent to the robot.
* \param output_recipe_file Filename where the output recipe is stored in.
* \param input_recipe_file Filename where the input recipe is stored in.
* \param handle_program_state Function handle to a callback on program state changes.
* \param headless_mode Parameter to control if the driver should be started in headless mode.
* \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.
*/
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 = "");
/*!
* \brief Constructs a new UrDriver object.
*
* \param robot_ip IP-address under which the robot is reachable.
* \param script_file URScript file that should be sent to the robot.
* \param output_recipe_file Filename where the output recipe is stored in.
* \param input_recipe_file Filename where the input recipe is stored in.
* \param handle_program_state Function handle to a callback on program state changes.
* \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.
*/
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 = "")
: UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode,
std::unique_ptr<ToolCommSetup>{}, calibration_checksum)
{
}
virtual ~UrDriver() = default;
/*!
* \brief Access function to receive the latest data package sent from the robot through RTDE
* interface.
*
* \returns The latest data package on success, a nullptr if no package can be found inside a preconfigured time
* window.
*/
std::unique_ptr<rtde_interface::DataPackage> getDataPackage();
uint32_t getControlFrequency() const
{
return rtde_frequency_;
}
/*!
* \brief Writes a joint command together with a keepalive signal onto the socket being sent to
* the robot.
*
* \param values Desired joint positions
*
* \returns True on successful write.
*/
bool writeJointCommand(const vector6d_t& values);
/*!
* \brief Write a keepalive signal only.
*
* This signals the robot that the connection is still
* active in times when no commands are to be sent (e.g. no controller is active.)
*
* \returns True on successful write.
*/
bool writeKeepalive();
/*!
* \brief Sends a stop command to the socket interface which will signal the program running on
* the robot to no longer listen for commands sent from the remote pc.
*
* \returns True on successful write.
*/
bool stopControl();
/*!
* \brief Starts the watchdog checking if the URCaps program is running on the robot and it is
* ready to receive control commands.
*/
void startWatchdog();
/*!
* \brief Checks if the kinematics information in the used model fits the actual robot.
*
* \param checksum Hash of the used kinematics information
*/
void checkCalibration(const std::string& checksum);
/*!
* \brief Getter for the RTDE writer used to write to the robot's RTDE interface.
*
* \returns The active RTDE writer
*/
rtde_interface::RTDEWriter& getRTDEWriter();
/*!
* \brief Sends a custom script program to the robot.
*
* The given code must be valid according the UR Scripting Manual.
*
* \param program URScript code that shall be executed by the robot.
*
* \returns true on successful upload, false otherwise.
*/
bool sendScript(const std::string& program);
/*!
* \brief Sends the external control program to the robot.
*
* Only for use in headless mode, as it replaces the use of the URCaps program.
*
* \returns true on successful upload, false otherwise
*/
bool sendRobotProgram();
private:
std::string readScriptFile(const std::string& filename);
std::string readKeepalive();
int rtde_frequency_;
comm::INotifier notifier_;
std::unique_ptr<rtde_interface::RTDEClient> rtde_client_;
std::unique_ptr<comm::ReverseInterface> reverse_interface_;
std::unique_ptr<comm::ScriptSender> script_sender_;
std::unique_ptr<comm::URStream<ur_driver::primary_interface::PackageHeader>> primary_stream_;
std::unique_ptr<comm::URStream<ur_driver::primary_interface::PackageHeader>> secondary_stream_;
double servoj_time_;
uint32_t servoj_gain_;
double servoj_lookahead_time_;
std::thread watchdog_thread_;
bool reverse_interface_active_;
uint32_t reverse_port_;
std::function<void(bool)> handle_program_state_;
std::string robot_ip_;
bool in_headless_mode_;
std::string full_robot_program_;
};
} // namespace ur_driver
#endif // ifndef UR_RTDE_DRIVER_UR_UR_DRIVER_H_INCLUDED

54
ur_robot_driver/include/ur_robot_driver/ur/version_information.h Normal file
View File

@@ -0,0 +1,54 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-06-11
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_UR_VERSION_INFORMATION_H_INCLUDED
#define UR_RTDE_DRIVER_UR_VERSION_INFORMATION_H_INCLUDED
#include <ur_robot_driver/types.h>
namespace ur_driver
{
/*!
* \brief Struct containing a robot's version information
*/
struct VersionInformation
{
VersionInformation()
{
major = 0;
minor = 0;
bugfix = 0;
build = 0;
}
uint32_t major; ///< Major version number
uint32_t minor; ///< Minor version number
uint32_t bugfix; ///< Bugfix version number
uint32_t build; ///< Build number
};
} // namespace ur_driver
#endif // ifndef UR_RTDE_DRIVER_UR_VERSION_INFORMATION_H_INCLUDED

28
ur_robot_driver/launch/ur10_bringup.launch Normal file
View File

@@ -0,0 +1,28 @@
<?xml version="1.0"?>
<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="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."/>
<arg name="controller_config_file" default="$(find ur_robot_driver)/config/ur10_controllers.yaml" doc="Config file used for defining the ROS-Control controllers."/>
<arg name="robot_description_file" default="$(find ur_description)/launch/ur10_upload.launch" doc="Robot description launch file."/>
<arg name="kinematics_config" default="$(find ur_description)/config/ur10_default.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="limited" default="false" doc="Use the description in limited mode (Every axis rotates from -PI to PI)"/>
<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."/>
<include file="$(find ur_robot_driver)/launch/ur_common.launch">
<arg name="debug" value="$(arg debug)"/>
<arg name="use_tool_communication" value="false"/>
<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="kinematics_config" value="$(arg kinematics_config)"/>
<arg name="limited" value="$(arg limited)"/>
<arg name="tf_prefix" value="$(arg tf_prefix)"/>
<arg name="controllers" value="$(arg controllers)"/>
<arg name="stopped_controllers" value="$(arg stopped_controllers)"/>
<arg name="headless_mode" value="$(arg headless_mode)"/>
</include>
</launch>

46
ur_robot_driver/launch/ur10e_bringup.launch Normal file
View File

@@ -0,0 +1,46 @@
<?xml version="1.0"?>
<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="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."/>
<arg name="controller_config_file" default="$(find ur_robot_driver)/config/ur10e_controllers.yaml" doc="Config file used for defining the ROS-Control controllers."/>
<arg name="robot_description_file" default="$(find ur_e_description)/launch/ur10e_upload.launch" doc="Robot description launch file."/>
<arg name="kinematics_config" default="$(find ur_e_description)/config/ur10e_default.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="use_tool_communication" default="false" doc="On e-Series robots tool communication can be enabled with this argument"/>
<arg name="tool_voltage" default="0" doc="Tool voltage set at the beginning of the UR program. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_parity" default="0" doc="Parity configuration used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_baud_rate" default="115200" doc="Baud rate used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_stop_bits" default="1" doc="Number of stop bits used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_rx_idle_chars" default="1.5" doc="Number of idle chars in RX channel used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_tx_idle_chars" default="3.5" doc="Number of idle chars in TX channel used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<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="limited" default="false" doc="Use the description in limited mode (Every axis rotates from -PI to PI)"/>
<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."/>
<include file="$(find ur_robot_driver)/launch/ur_common.launch">
<arg name="debug" value="$(arg debug)"/>
<arg name="use_tool_communication" value="$(arg use_tool_communication)"/>
<arg name="controller_config_file" value="$(arg controller_config_file)"/>
<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="limited" value="$(arg limited)"/>
<arg name="tf_prefix" value="$(arg tf_prefix)"/>
<arg name="controllers" value="$(arg controllers)"/>
<arg name="stopped_controllers" value="$(arg stopped_controllers)"/>
<arg name="headless_mode" value="$(arg headless_mode)"/>
<arg name="tool_voltage" value="$(arg tool_voltage)"/>
<arg name="tool_parity" value="$(arg tool_parity)"/>
<arg name="tool_baud_rate" value="$(arg tool_baud_rate)"/>
<arg name="tool_stop_bits" value="$(arg tool_stop_bits)"/>
<arg name="tool_rx_idle_chars" value="$(arg tool_rx_idle_chars)"/>
<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)"/>
</include>
</launch>

28
ur_robot_driver/launch/ur3_bringup.launch Normal file
View File

@@ -0,0 +1,28 @@
<?xml version="1.0"?>
<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="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."/>
<arg name="controller_config_file" default="$(find ur_robot_driver)/config/ur3_controllers.yaml" doc="Config file used for defining the ROS-Control controllers."/>
<arg name="robot_description_file" default="$(find ur_description)/launch/ur3_upload.launch" doc="Robot description launch file."/>
<arg name="kinematics_config" default="$(find ur_description)/config/ur3_default.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="limited" default="false" doc="Use the description in limited mode (Every axis rotates from -PI to PI)"/>
<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."/>
<include file="$(find ur_robot_driver)/launch/ur_common.launch">
<arg name="debug" value="$(arg debug)"/>
<arg name="use_tool_communication" value="false"/>
<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="kinematics_config" value="$(arg kinematics_config)"/>
<arg name="limited" value="$(arg limited)"/>
<arg name="tf_prefix" value="$(arg tf_prefix)"/>
<arg name="controllers" value="$(arg controllers)"/>
<arg name="stopped_controllers" value="$(arg stopped_controllers)"/>
<arg name="headless_mode" value="$(arg headless_mode)"/>
</include>
</launch>

45
ur_robot_driver/launch/ur3e_bringup.launch Normal file
View File

@@ -0,0 +1,45 @@
<?xml version="1.0"?>
<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="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."/>
<arg name="controller_config_file" default="$(find ur_robot_driver)/config/ur3e_controllers.yaml" doc="Config file used for defining the ROS-Control controllers."/>
<arg name="robot_description_file" default="$(find ur_e_description)/launch/ur3e_upload.launch" doc="Robot description launch file."/>
<arg name="kinematics_config" default="$(find ur_e_description)/config/ur3e_default.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="use_tool_communication" default="false" doc="On e-Series robots tool communication can be enabled with this argument"/>
<arg name="tool_voltage" default="0" doc="Tool voltage set at the beginning of the UR program. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_parity" default="0" doc="Parity configuration used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_baud_rate" default="115200" doc="Baud rate used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_stop_bits" default="1" doc="Number of stop bits used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_rx_idle_chars" default="1.5" doc="Number of idle chars in RX channel used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_tx_idle_chars" default="3.5" doc="Number of idle chars in TX channel used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<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="limited" default="false" doc="Use the description in limited mode (Every axis rotates from -PI to PI)"/>
<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."/>
<include file="$(find ur_robot_driver)/launch/ur_common.launch">
<arg name="debug" value="$(arg debug)"/>
<arg name="use_tool_communication" value="$(arg use_tool_communication)"/>
<arg name="controller_config_file" value="$(arg controller_config_file)"/>
<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="limited" value="$(arg limited)"/>
<arg name="tf_prefix" value="$(arg tf_prefix)"/>
<arg name="controllers" value="$(arg controllers)"/>
<arg name="stopped_controllers" value="$(arg stopped_controllers)"/>
<arg name="headless_mode" value="$(arg headless_mode)"/>
<arg name="tool_voltage" value="$(arg tool_voltage)"/>
<arg name="tool_parity" value="$(arg tool_parity)"/>
<arg name="tool_baud_rate" value="$(arg tool_baud_rate)"/>
<arg name="tool_stop_bits" value="$(arg tool_stop_bits)"/>
<arg name="tool_rx_idle_chars" value="$(arg tool_rx_idle_chars)"/>
<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)"/>
</include>
</launch>

28
ur_robot_driver/launch/ur5_bringup.launch Normal file
View File

@@ -0,0 +1,28 @@
<?xml version="1.0"?>
<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="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."/>
<arg name="controller_config_file" default="$(find ur_robot_driver)/config/ur5_controllers.yaml" doc="Config file used for defining the ROS-Control controllers."/>
<arg name="robot_description_file" default="$(find ur_description)/launch/ur5_upload.launch" doc="Robot description launch file."/>
<arg name="kinematics_config" default="$(find ur_description)/config/ur5_default.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="limited" default="false" doc="Use the description in limited mode (Every axis rotates from -PI to PI)"/>
<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."/>
<include file="$(find ur_robot_driver)/launch/ur_common.launch">
<arg name="debug" value="$(arg debug)"/>
<arg name="use_tool_communication" value="false"/>
<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="kinematics_config" value="$(arg kinematics_config)"/>
<arg name="limited" value="$(arg limited)"/>
<arg name="tf_prefix" value="$(arg tf_prefix)"/>
<arg name="controllers" value="$(arg controllers)"/>
<arg name="stopped_controllers" value="$(arg stopped_controllers)"/>
<arg name="headless_mode" value="$(arg headless_mode)"/>
</include>
</launch>

45
ur_robot_driver/launch/ur5e_bringup.launch Normal file
View File

@@ -0,0 +1,45 @@
<?xml version="1.0"?>
<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="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."/>
<arg name="controller_config_file" default="$(find ur_robot_driver)/config/ur5e_controllers.yaml" doc="Config file used for defining the ROS-Control controllers."/>
<arg name="robot_description_file" default="$(find ur_e_description)/launch/ur5e_upload.launch" doc="Robot description launch file."/>
<arg name="kinematics_config" default="$(find ur_e_description)/config/ur5e_default.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="use_tool_communication" default="false" doc="On e-Series robots tool communication can be enabled with this argument"/>
<arg name="tool_voltage" default="0" doc="Tool voltage set at the beginning of the UR program. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_parity" default="0" doc="Parity configuration used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_baud_rate" default="115200" doc="Baud rate used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_stop_bits" default="1" doc="Number of stop bits used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_rx_idle_chars" default="1.5" doc="Number of idle chars in RX channel used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_tx_idle_chars" default="3.5" doc="Number of idle chars in TX channel used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<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="limited" default="false" doc="Use the description in limited mode (Every axis rotates from -PI to PI)"/>
<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."/>
<include file="$(find ur_robot_driver)/launch/ur_common.launch">
<arg name="debug" value="$(arg debug)"/>
<arg name="use_tool_communication" value="$(arg use_tool_communication)"/>
<arg name="controller_config_file" value="$(arg controller_config_file)"/>
<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="limited" value="$(arg limited)"/>
<arg name="tf_prefix" value="$(arg tf_prefix)"/>
<arg name="controllers" value="$(arg controllers)"/>
<arg name="stopped_controllers" value="$(arg stopped_controllers)"/>
<arg name="headless_mode" value="$(arg headless_mode)"/>
<arg name="tool_voltage" value="$(arg tool_voltage)"/>
<arg name="tool_parity" value="$(arg tool_parity)"/>
<arg name="tool_baud_rate" value="$(arg tool_baud_rate)"/>
<arg name="tool_stop_bits" value="$(arg tool_stop_bits)"/>
<arg name="tool_rx_idle_chars" value="$(arg tool_rx_idle_chars)"/>
<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)"/>
</include>
</launch>

51
ur_robot_driver/launch/ur_common.launch Normal file
View File

@@ -0,0 +1,51 @@
<?xml version="1.0"?>
<launch>
<arg name="debug" default="false" doc="Debug flag that will get passed on to ur_control.launch"/>
<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="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."/>
<arg name="stopped_controllers" default="pos_traj_controller" doc="Controllers that are initally loaded, but not started."/>
<arg name="tool_voltage" default="0" doc="Tool voltage set at the beginning of the UR program. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_parity" default="0" doc="Parity configuration used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_baud_rate" default="115200" doc="Baud rate used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_stop_bits" default="1" doc="Number of stop bits used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_rx_idle_chars" default="1.5" doc="Number of idle chars in RX channel used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_tx_idle_chars" default="3.5" doc="Number of idle chars in TX channel used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<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="robot_description_file" doc="Robot description launch file."/>
<arg name="limited" default="false" doc="Use the description in limited mode (Every axis rotates from -PI to PI)"/>
<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."/>
<!-- robot model -->
<include file="$(arg robot_description_file)">
<arg name="limited" value="$(arg limited)"/>
<arg name="kinematics_config" value="$(arg kinematics_config)"/>
</include>
<!-- Convert joint states to /tf tranforms -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
<include file="$(find ur_robot_driver)/launch/ur_control.launch">
<arg name="debug" value="$(arg debug)"/>
<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="kinematics_config" value="$(arg kinematics_config)"/>
<arg name="tf_prefix" value="$(arg tf_prefix)"/>
<arg name="controllers" value="$(arg controllers)"/>
<arg name="stopped_controllers" value="$(arg stopped_controllers)"/>
<arg name="headless_mode" value="$(arg headless_mode)"/>
<arg name="tool_voltage" value="$(arg tool_voltage)"/>
<arg name="tool_parity" value="$(arg tool_parity)"/>
<arg name="tool_baud_rate" value="$(arg tool_baud_rate)"/>
<arg name="tool_stop_bits" value="$(arg tool_stop_bits)"/>
<arg name="tool_rx_idle_chars" value="$(arg tool_rx_idle_chars)"/>
<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)"/>
</include>
</launch>

76
ur_robot_driver/launch/ur_control.launch Normal file
View File

@@ -0,0 +1,76 @@
<?xml version="1.0"?>
<launch>
<!-- GDB functionality -->
<arg name="debug" default="false" doc="If set to true, will start the driver inside gdb" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
<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="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."/>
<arg name="stopped_controllers" default="pos_traj_controller" doc="Controllers that are initally loaded, but not started."/>
<arg name="urscript_file" default="$(find ur_robot_driver)/resources/servoj.urscript" doc="Path to URScript that will be sent to the robot and that forms the main control program."/>
<arg name="rtde_output_recipe_file" default="$(find ur_robot_driver)/resources/rtde_output_recipe.txt" doc="Recipe file used for the RTDE-outputs. Only change this if you know what you're doing."/>
<arg name="rtde_input_recipe_file" default="$(find ur_robot_driver)/resources/rtde_input_recipe.txt" doc="Recipe file used for the RTDE-inputs. Only change this if you know what you're doing."/>
<arg name="tool_voltage" default="0" doc="Tool voltage set at the beginning of the UR program. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_parity" default="0" doc="Parity configuration used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_baud_rate" default="115200" doc="Baud rate used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_stop_bits" default="1" doc="Number of stop bits used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_rx_idle_chars" default="1.5" doc="Number of idle chars in RX channel used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<arg name="tool_tx_idle_chars" default="3.5" doc="Number of idle chars in TX channel used for tool communication. Only used, when `use_tool_communication` is set to true."/>
<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."/>
<!-- 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)"/>
<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)"/>
<param name="input_recipe_file" value="$(arg rtde_input_recipe_file)"/>
<param name="headless_mode" value="$(arg headless_mode)"/>
<param name="tf_prefix" value="$(arg tf_prefix)"/>
<param name="use_tool_communication" value="$(arg use_tool_communication)"/>
<!--<param name="reverse_port" type="int" value="$(arg reverse_port)" />-->
<param name="tool_voltage" value="$(arg tool_voltage)"/>
<param name="tool_parity" value="$(arg tool_parity)"/>
<param name="tool_baud_rate" value="$(arg tool_baud_rate)"/>
<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)"/>
</node>
<!-- 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="device_name" value="$(arg tool_device_name)"/>
<param name="tcp_port" value="$(arg tool_tcp_port)"/>
</node>
<!-- Load controller settings -->
<rosparam file="$(arg controller_config_file)" command="load"/>
<!-- spawn controller manager -->
<node name="ros_control_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" args="$(arg controllers)" />
<!-- load other controller -->
<node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
output="screen" args="load $(arg stopped_controllers)" />
<node name="controller_stopper" pkg="controller_stopper" type="node" respawn="false" output="screen">
<remap from="robot_running" to="ur_hardware_interface/robot_program_running"/>
<rosparam param="consistent_controllers">
- "joint_state_controller"
- "speed_scaling_state_controller"
- "force_torque_sensor_controller"
</rosparam>
</node>
</launch>

53
ur_robot_driver/package.xml Normal file
View File

@@ -0,0 +1,53 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>ur_robot_driver</name>
<version>0.0.3</version>
<description>The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series.</description>
<author>Thomas Timm Andersen</author>
<author>Simon Rasmussen</author>
<author>Felix Mauch</author>
<author>Lea Steffen</author>
<author>Tristan Schnell</author>
<maintainer email="mauch@fzi.de">Felix Mauch</maintainer>
<license>Apache 2.0</license>
<license>BSD 2-clause</license>
<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>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>boost</build_depend>
<depend>actionlib</depend>
<depend>control_msgs</depend>
<depend>controller_manager</depend>
<depend>geometry_msgs</depend>
<depend>hardware_interface</depend>
<depend>industrial_msgs</depend>
<depend>roscpp</depend>
<depend>sensor_msgs</depend>
<depend>tf</depend>
<depend>tf2_msgs</depend>
<depend>tf2_geometry_msgs</depend>
<depend>trajectory_msgs</depend>
<depend>ur_controllers</depend>
<depend>ur_msgs</depend>
<depend>std_srvs</depend>
<exec_depend>force_torque_sensor_controller</exec_depend>
<exec_depend>joint_state_controller</exec_depend>
<exec_depend>joint_trajectory_controller</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>socat</exec_depend>
<exec_depend>ur_description</exec_depend>
<exec_depend>velocity_controllers</exec_depend>
<export>
<rosdoc config="doc/rosdoc.yaml" />
</export>
</package>

BIN
ur_robot_driver/resources/externalcontrol-1.0.1.urcap Normal file
View File

Binary file not shown.

BIN
ur_robot_driver/resources/rs485-1.0.urcap Normal file
View File

Binary file not shown.

12
ur_robot_driver/resources/rtde_input_recipe.txt Normal file
View File

@@ -0,0 +1,12 @@
speed_slider_mask
speed_slider_fraction
standard_digital_output_mask
standard_digital_output
configurable_digital_output_mask
configurable_digital_output
tool_digital_output_mask
tool_digital_output
standard_analog_output_mask
standard_analog_output_type
standard_analog_output_0
standard_analog_output_1

22
ur_robot_driver/resources/rtde_output_recipe.txt Normal file
View File

@@ -0,0 +1,22 @@
timestamp
actual_q
actual_qd
speed_scaling
target_speed_fraction
runtime_state
actual_TCP_force
actual_TCP_pose
actual_digital_input_bits
actual_digital_output_bits
standard_analog_input0
standard_analog_input1
standard_analog_output0
standard_analog_output1
analog_io_types
tool_mode
tool_analog_input_types
tool_analog_input0
tool_analog_input1
tool_output_voltage
tool_output_current
tool_temperature

97
ur_robot_driver/resources/servoj.urscript Normal file
View File

@@ -0,0 +1,97 @@
{{BEGIN_REPLACE}}
steptime = get_steptime()
textmsg("steptime=", steptime)
MULT_jointstate = {{JOINT_STATE_REPLACE}}
#Constants
SERVO_STOPPED = -2
SERVO_UNINITIALIZED = -1
SERVO_IDLE = 0
SERVO_RUNNING = 1
#Global variables are also showed in the Teach pendants variable list
global cmd_servo_state = SERVO_UNINITIALIZED
global cmd_servo_q = get_actual_joint_positions()
global cmd_servo_q_last = get_actual_joint_positions()
global extrapolate_count = 0
global extrapolate_max_count = 0
def set_servo_setpoint(q):
cmd_servo_state = SERVO_RUNNING
cmd_servo_q_last = cmd_servo_q
cmd_servo_q = q
end
def extrapolate():
diff = [cmd_servo_q[0] - cmd_servo_q_last[0], cmd_servo_q[1] - cmd_servo_q_last[1], cmd_servo_q[2] - cmd_servo_q_last[2], cmd_servo_q[3] - cmd_servo_q_last[3], cmd_servo_q[4] - cmd_servo_q_last[4], cmd_servo_q[5] - cmd_servo_q_last[5]]
cmd_servo_q_last = cmd_servo_q
cmd_servo_q = [cmd_servo_q[0] + diff[0], cmd_servo_q[1] + diff[1], cmd_servo_q[2] + diff[2], cmd_servo_q[3] + diff[3], cmd_servo_q[4] + diff[4], cmd_servo_q[5] + diff[5]]
return cmd_servo_q
end
thread servoThread():
state = SERVO_IDLE
while state > SERVO_STOPPED:
enter_critical
q = cmd_servo_q
do_extrapolate = False
if (cmd_servo_state == SERVO_IDLE):
do_extrapolate = True
end
state = cmd_servo_state
if cmd_servo_state > SERVO_UNINITIALIZED:
cmd_servo_state = SERVO_IDLE
end
if do_extrapolate:
extrapolate_count = extrapolate_count + 1
if extrapolate_count > extrapolate_max_count:
extrapolate_max_count = extrapolate_count
end
q = extrapolate()
servoj(q, t=steptime, {{SERVO_J_REPLACE}})
elif state == SERVO_RUNNING:
extrapolate_count = 0
servoj(q, t=steptime, {{SERVO_J_REPLACE}})
else:
extrapolate_count = 0
sync()
end
exit_critical
end
textmsg("servo thread ended")
stopj(0.1)
end
socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
thread_servo = run servoThread()
keepalive = -2
textmsg("External control active")
params_mult = socket_read_binary_integer(1+6, "reverse_socket")
keepalive = params_mult[1]
while keepalive > 0:
enter_critical
socket_send_line(1, "reverse_socket")
params_mult = socket_read_binary_integer(1+6, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
if params_mult[0] > 0:
keepalive = params_mult[1]
if params_mult[1] > 1:
q = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate]
set_servo_setpoint(q)
end
else:
keepalive = keepalive - 1
end
exit_critical
end
textmsg("Stopping communication and servoing")
cmd_servo_state = SERVO_STOPPED
sleep(.1)
socket_close("reverse_socket")
kill thread_servo

38
ur_robot_driver/scripts/tool_communication Executable file
View File

@@ -0,0 +1,38 @@
#!/usr/bin/env python
"""Small helper script to start the tool communication interface"""
import subprocess
import rospy
def main():
"""Starts socat"""
rospy.init_node("ur_tool_communication")
# IP address of the robot
robot_ip = rospy.get_param("~robot_ip")
# Port on which the remote pc (robot) publishes the interface
tcp_port = rospy.get_param("~tcp_port", "54321")
# By default, socat will create a pty in /dev/pts/N with n being an increasing number.
# Additionally, a symlink at the given location will be created. Use an absolute path here.
local_device = rospy.get_param("~device_name")
rospy.loginfo("Remote device is available at '" + local_device + "'")
cfg_params = ["pty"]
cfg_params.append("link=" + local_device)
cfg_params.append("raw")
cfg_params.append("ignoreeof")
cfg_params.append("waitslave")
cmd = ["socat"]
cmd.append(",".join(cfg_params))
cmd.append(":".join(["tcp", robot_ip, tcp_port]))
rospy.loginfo("Starting socat with following command:\n" + " ".join(cmd))
subprocess.call(cmd)
if __name__ == '__main__':
main()

159
ur_robot_driver/src/comm/server.cpp Normal file
View File

@@ -0,0 +1,159 @@
/*
* Copyright 2019, FZI Forschungszentrum Informatik (refactor)
*
* Copyright 2017, 2018 Jarek Potiuk (low bandwidth trajectory follower)
*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "ur_robot_driver/comm/server.h"
#include <arpa/inet.h>
#include <netinet/tcp.h>
#include <unistd.h>
#include <cstring>
#include "ur_robot_driver/log.h"
namespace ur_driver
{
namespace comm
{
URServer::URServer(int port) : port_(port)
{
}
URServer::~URServer()
{
TCPSocket::close();
}
std::string URServer::getIP()
{
sockaddr_in name;
socklen_t len = sizeof(name);
int res = ::getsockname(getSocketFD(), (sockaddr*)&name, &len);
if (res < 0)
{
LOG_ERROR("Could not get local IP");
return std::string();
}
char buf[128];
inet_ntop(AF_INET, &name.sin_addr, buf, sizeof(buf));
return std::string(buf);
}
bool URServer::open(int socket_fd, struct sockaddr* address, size_t address_len)
{
int flag = 1;
setsockopt(socket_fd, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int));
return ::bind(socket_fd, address, address_len) == 0;
}
bool URServer::bind()
{
std::string empty;
bool res = TCPSocket::setup(empty, port_);
if (!res)
return false;
if (::listen(getSocketFD(), 1) < 0)
return false;
return true;
}
bool URServer::accept()
{
if (TCPSocket::getState() != comm::SocketState::Connected || client_.getSocketFD() > 0)
return false;
struct sockaddr addr;
socklen_t addr_len;
int client_fd = -1;
int retry = 0;
while ((client_fd = ::accept(getSocketFD(), &addr, &addr_len)) == -1)
{
LOG_ERROR("Accepting socket connection failed. (errno: %d)", errno);
if (retry++ >= 5)
return false;
}
TCPSocket::setOptions(client_fd);
return client_.setSocketFD(client_fd);
}
void URServer::disconnectClient()
{
if (client_.getState() != comm::SocketState::Connected)
return;
client_.close();
}
bool URServer::write(const uint8_t* buf, size_t buf_len, size_t& written)
{
return client_.write(buf, buf_len, written);
}
bool URServer::readLine(char* buffer, size_t buf_len)
{
char* current_pointer = buffer;
char ch;
size_t total_read;
if (buf_len <= 0 || buffer == NULL)
{
return false;
}
total_read = 0;
for (;;)
{
if (client_.read(&ch))
{
if (total_read < buf_len - 1) // just in case ...
{
total_read++;
*current_pointer++ = ch;
}
if (ch == '\n')
{
break;
}
}
else
{
if (total_read == 0)
{
return false;
}
else
{
break;
}
}
}
*current_pointer = '\0';
return true;
}
} // namespace comm
} // namespace ur_driver

210
ur_robot_driver/src/comm/tcp_socket.cpp Normal file
View File

@@ -0,0 +1,210 @@
/*
* Copyright 2019, FZI Forschungszentrum Informatik (refactor)
*
* Copyright 2017, 2018 Jarek Potiuk (low bandwidth trajectory follower)
*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include <arpa/inet.h>
#include <endian.h>
#include <netinet/tcp.h>
#include <unistd.h>
#include <cstring>
#include "ur_robot_driver/log.h"
#include "ur_robot_driver/comm/tcp_socket.h"
namespace ur_driver
{
namespace comm
{
TCPSocket::TCPSocket() : socket_fd_(-1), state_(SocketState::Invalid)
{
}
TCPSocket::~TCPSocket()
{
close();
}
void TCPSocket::setOptions(int socket_fd)
{
int flag = 1;
setsockopt(socket_fd, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(int));
setsockopt(socket_fd, IPPROTO_TCP, TCP_QUICKACK, &flag, sizeof(int));
if (recv_timeout_ != nullptr)
{
setsockopt(socket_fd, SOL_SOCKET, SO_RCVTIMEO, recv_timeout_.get(), sizeof(timeval));
}
}
bool TCPSocket::setup(std::string& host, int port)
{
if (state_ == SocketState::Connected)
return false;
LOG_DEBUG("Setting up connection: %s:%d", host.c_str(), port);
// gethostbyname() is deprecated so use getadderinfo() as described in:
// http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo
const char* host_name = host.empty() ? nullptr : host.c_str();
std::string service = std::to_string(port);
struct addrinfo hints, *result;
std::memset(&hints, 0, sizeof(hints));
hints.ai_family = AF_UNSPEC;
hints.ai_socktype = SOCK_STREAM;
hints.ai_flags = AI_PASSIVE;
if (getaddrinfo(host_name, service.c_str(), &hints, &result) != 0)
{
LOG_ERROR("Failed to get address for %s:%d", host.c_str(), port);
return false;
}
bool connected = false;
// loop through the list of addresses untill we find one that's connectable
for (struct addrinfo* p = result; p != nullptr; p = p->ai_next)
{
socket_fd_ = ::socket(p->ai_family, p->ai_socktype, p->ai_protocol);
if (socket_fd_ != -1 && open(socket_fd_, p->ai_addr, p->ai_addrlen))
{
connected = true;
break;
}
}
freeaddrinfo(result);
if (!connected)
{
state_ = SocketState::Invalid;
LOG_ERROR("Connection setup failed for %s:%d", host.c_str(), port);
}
else
{
setOptions(socket_fd_);
state_ = SocketState::Connected;
LOG_DEBUG("Connection established for %s:%d", host.c_str(), port);
}
return connected;
}
bool TCPSocket::setSocketFD(int socket_fd)
{
if (state_ == SocketState::Connected)
return false;
socket_fd_ = socket_fd;
state_ = SocketState::Connected;
return true;
}
void TCPSocket::close()
{
if (state_ != SocketState::Connected)
return;
state_ = SocketState::Closed;
::close(socket_fd_);
socket_fd_ = -1;
}
std::string TCPSocket::getIP() const
{
sockaddr_in name;
socklen_t len = sizeof(name);
int res = ::getsockname(socket_fd_, (sockaddr*)&name, &len);
if (res < 0)
{
LOG_ERROR("Could not get local IP");
return std::string();
}
char buf[128];
inet_ntop(AF_INET, &name.sin_addr, buf, sizeof(buf));
return std::string(buf);
}
bool TCPSocket::read(char* character)
{
size_t read_chars;
// It's inefficient, but in our case we read very small messages
// and the overhead connected with reading character by character is
// negligible - adding buffering would complicate the code needlessly.
return read((uint8_t*)character, 1, read_chars);
}
bool TCPSocket::read(uint8_t* buf, const size_t buf_len, size_t& read)
{
read = 0;
if (state_ != SocketState::Connected)
return false;
ssize_t res = ::recv(socket_fd_, buf, buf_len, 0);
if (res == 0)
{
state_ = SocketState::Disconnected;
return false;
}
else if (res < 0)
return false;
read = static_cast<size_t>(res);
return true;
}
bool TCPSocket::write(const uint8_t* buf, const size_t buf_len, size_t& written)
{
written = 0;
if (state_ != SocketState::Connected)
return false;
size_t remaining = buf_len;
// handle partial sends
while (written < buf_len)
{
ssize_t sent = ::send(socket_fd_, buf + written, remaining, 0);
if (sent <= 0)
return false;
written += sent;
remaining -= sent;
}
return true;
}
void TCPSocket::setReceiveTimeout(const timeval& timeout)
{
recv_timeout_.reset(new timeval(timeout));
if (state_ == SocketState::Connected)
{
setOptions(socket_fd_);
}
}
} // namespace comm
} // namespace ur_driver

54
ur_robot_driver/src/primary/primary_package.cpp Normal file
View File

@@ -0,0 +1,54 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#include "ur_robot_driver/primary/primary_package.h"
namespace ur_driver
{
namespace primary_interface
{
bool PrimaryPackage::parseWith(comm::BinParser& bp)
{
bp.rawData(buffer_, buffer_length_);
return true;
}
std::string PrimaryPackage::toString() const
{
std::stringstream ss;
ss << "Raw byte stream: ";
for (size_t i = 0; i < buffer_length_; ++i)
{
uint8_t* buf = buffer_.get();
ss << std::hex << static_cast<int>(buf[i]) << " ";
}
ss << std::endl;
return ss.str();
}
} // namespace primary_interface
} // namespace ur_driver

49
ur_robot_driver/src/primary/robot_message.cpp Normal file
View File

@@ -0,0 +1,49 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik (ur_robot_driver)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#include "ur_robot_driver/primary/robot_message.h"
namespace ur_driver
{
namespace primary_interface
{
bool RobotMessage::parseWith(comm::BinParser& bp)
{
return true;
}
std::string RobotMessage::toString() const
{
std::stringstream ss;
ss << "timestamp: " << timestamp_ << std::endl;
ss << "source: " << static_cast<int>(source_) << std::endl;
ss << "message_type: " << static_cast<int>(message_type_) << std::endl;
return ss.str();
}
} // namespace primary_interface
} // namespace ur_driver

61
ur_robot_driver/src/primary/robot_message/version_message.cpp Normal file
View File

@@ -0,0 +1,61 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik (ur_robot_driver)
// Copyright 2017, 2018 Simon Rasmussen (refactor)
//
// Copyright 2015, 2016 Thomas Timm Andersen (original version)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-08
*
*/
//----------------------------------------------------------------------
#include "ur_robot_driver/log.h"
#include "ur_robot_driver/primary/robot_message/version_message.h"
namespace ur_driver
{
namespace primary_interface
{
bool VersionMessage::parseWith(comm::BinParser& bp)
{
bp.parse(project_name_length_);
bp.parse(project_name_, project_name_length_);
bp.parse(major_version_);
bp.parse(minor_version_);
bp.parse(svn_version_);
bp.parse(build_number_);
bp.parseRemainder(build_date_);
return true; // not really possible to check dynamic size packets
}
std::string VersionMessage::toString() const
{
std::stringstream ss;
ss << "project name: " << project_name_ << std::endl;
ss << "version: " << unsigned(major_version_) << "." << unsigned(minor_version_) << "." << svn_version_ << std::endl;
ss << "build date: " << build_date_;
return ss.str();
}
} // namespace primary_interface
} // namespace ur_driver

105
ur_robot_driver/src/primary/robot_state/kinematics_info.cpp Normal file
View File

@@ -0,0 +1,105 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-08
*
*/
//----------------------------------------------------------------------
#include "ur_robot_driver/log.h"
#include "ur_robot_driver/primary/robot_state/kinematics_info.h"
#include <iomanip>
namespace ur_driver
{
namespace primary_interface
{
bool KinematicsInfo::parseWith(comm::BinParser& bp)
{
bp.parse(checksum_);
bp.parse(dh_theta_);
bp.parse(dh_a_);
bp.parse(dh_d_);
bp.parse(dh_alpha_);
bp.parse(calibration_status_);
return true;
}
std::string KinematicsInfo::toString() const
{
std::stringstream os;
os << "checksum: [";
for (size_t i = 0; i < checksum_.size(); ++i)
{
os << checksum_[i] << " ";
}
os << "]" << std::endl;
os << "dh_theta: [";
for (size_t i = 0; i < dh_theta_.size(); ++i)
{
os << std::setprecision(15) << dh_theta_[i] << " ";
}
os << "]" << std::endl;
os << "dh_a: [";
for (size_t i = 0; i < dh_a_.size(); ++i)
{
os << std::setprecision(15) << dh_a_[i] << " ";
}
os << "]" << std::endl;
os << "dh_d: [";
for (size_t i = 0; i < dh_d_.size(); ++i)
{
os << std::setprecision(15) << dh_d_[i] << " ";
}
os << "]" << std::endl;
os << "dh_alpha: [";
for (size_t i = 0; i < dh_alpha_.size(); ++i)
{
os << std::setprecision(15) << dh_alpha_[i] << " ";
}
os << "]" << std::endl;
os << "calibration_status: " << calibration_status_ << std::endl;
return os.str();
}
std::string KinematicsInfo ::toHash() const
{
std::stringstream ss;
for (size_t i = 0; i < 6; ++i)
{
ss << dh_theta_[i];
ss << dh_d_[i];
ss << dh_a_[i];
ss << dh_alpha_[i];
}
std::hash<std::string> hash_fn;
return "calib_" + std::to_string(hash_fn(ss.str()));
}
} // namespace primary_interface
} // namespace ur_driver

707
ur_robot_driver/src/ros/hardware_interface.cpp Normal file
View File

@@ -0,0 +1,707 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-11
*
*/
//----------------------------------------------------------------------
#include "ur_robot_driver/ros/hardware_interface.h"
#include "ur_robot_driver/ur/tool_communication.h"
#include <ur_robot_driver/exceptions.h>
#include <Eigen/Geometry>
namespace ur_driver
{
HardwareInterface::HardwareInterface()
: joint_position_command_({ 0, 0, 0, 0, 0, 0 })
, joint_positions_{ { 0, 0, 0, 0, 0, 0 } }
, joint_velocities_{ { 0, 0, 0, 0, 0, 0 } }
, joint_efforts_{ { 0, 0, 0, 0, 0, 0 } }
, standard_analog_input_{ { 0, 0 } }
, standard_analog_output_{ { 0, 0 } }
, joint_names_(6)
, runtime_state_(static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::STOPPED))
, position_controller_running_(false)
, pausing_state_(PausingState::RUNNING)
, pausing_ramp_up_increment_(0.01)
, controllers_initialized_(false)
{
}
bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh)
{
joint_velocities_ = { { 0, 0, 0, 0, 0, 0 } };
joint_efforts_ = { { 0, 0, 0, 0, 0, 0 } };
std::string script_filename;
std::string output_recipe_filename;
std::string input_recipe_filename;
// The robot's IP address.
if (!robot_hw_nh.getParam("robot_ip", robot_ip_))
{
ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("robot_ip_") << " not given.");
return false;
}
robot_hw_nh.param<std::string>("tf_prefix", tf_prefix_, "");
// Path to the urscript code that will be sent to the robot.
if (!robot_hw_nh.getParam("script_file", script_filename))
{
ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("script_file") << " not given.");
return false;
}
// Path to the file containing the recipe used for requesting RTDE outputs.
if (!robot_hw_nh.getParam("output_recipe_file", output_recipe_filename))
{
ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("output_recipe_file") << " not given.");
return false;
}
// Path to the file containing the recipe used for requesting RTDE inputs.
if (!robot_hw_nh.getParam("input_recipe_file", input_recipe_filename))
{
ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("input_recipe_file") << " not given.");
return false;
}
bool headless_mode;
// Start robot in headless mode. This does not require the 'External Control' URCap to be running
// on the robot, but this will send the URScript to the robot directly. On e-Series robots this
// requires the robot to run in 'remote-control' mode.
if (!robot_hw_nh.getParam("headless_mode", headless_mode))
{
ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("headless_mode") << " not given.");
return false;
}
// Whenever the runtime state of the "External Control" program node in the UR-program changes, a
// message gets published here. So this is equivalent to the information whether the robot accepts
// commands from ROS side.
program_state_pub_ = robot_hw_nh.advertise<std_msgs::Bool>("robot_program_running", 10, true);
tcp_transform_.header.frame_id = tf_prefix_ + "base";
tcp_transform_.child_frame_id = tf_prefix_ + "tool0_controller";
// Should the tool's RS485 interface be forwarded to the ROS machine? This is only available on
// e-Series models. Setting this parameter to TRUE requires multiple other parameters to be set,as
// well.
bool use_tool_communication = robot_hw_nh.param<bool>("use_tool_communication", "false");
std::unique_ptr<ToolCommSetup> tool_comm_setup;
if (use_tool_communication)
{
tool_comm_setup.reset(new ToolCommSetup());
using ToolVoltageT = std::underlying_type<ToolVoltage>::type;
ToolVoltageT tool_voltage;
// Tool voltage that will be set as soon as the UR-Program on the robot is started. Note: This
// parameter is only evaluated, when the parameter "use_tool_communication" is set to TRUE.
// Then, this parameter is required.
if (!robot_hw_nh.getParam("tool_voltage", tool_voltage))
{
ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_voltage") << " not given.");
return false;
}
tool_comm_setup->setToolVoltage(static_cast<ToolVoltage>(tool_voltage));
using ParityT = std::underlying_type<Parity>::type;
ParityT parity;
// Parity used for tool communication. Will be set as soon as the UR-Program on the robot is
// started. Can be 0 (None), 1 (odd) and 2 (even).
//
// Note: This parameter is only evaluated, when the parameter "use_tool_communication"
// is set to TRUE. Then, this parameter is required.
if (!robot_hw_nh.getParam("tool_parity", parity))
{
ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_parity") << " not given.");
return false;
}
int baud_rate;
// Baud rate used for tool communication. Will be set as soon as the UR-Program on the robot is
// started. See UR documentation for valid baud rates.
//
// Note: This parameter is only evaluated, when the parameter "use_tool_communication"
// is set to TRUE. Then, this parameter is required.
if (!robot_hw_nh.getParam("tool_baud_rate", baud_rate))
{
ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_baud_rate") << " not given.");
return false;
}
tool_comm_setup->setBaudRate(baud_rate);
int stop_bits;
// Number of stop bits used for tool communication. Will be set as soon as the UR-Program on the robot is
// started. Can be 1 or 2.
//
// Note: This parameter is only evaluated, when the parameter "use_tool_communication"
// is set to TRUE. Then, this parameter is required.
if (!robot_hw_nh.getParam("tool_stop_bits", stop_bits))
{
ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_stop_bits") << " not given.");
return false;
}
tool_comm_setup->setStopBits(stop_bits);
int rx_idle_chars;
// Number of idle chars for the RX unit used for tool communication. Will be set as soon as the UR-Program on the
// robot is started. Valid values: min=1.0, max=40.0
//
// Note: This parameter is only evaluated, when the parameter "use_tool_communication"
// is set to TRUE. Then, this parameter is required.
if (!robot_hw_nh.getParam("tool_rx_idle_chars", rx_idle_chars))
{
ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_rx_idle_chars") << " not given.");
return false;
}
tool_comm_setup->setRxIdleChars(rx_idle_chars);
tool_comm_setup->setParity(static_cast<Parity>(parity));
int tx_idle_chars;
// Number of idle chars for the TX unit used for tool communication. Will be set as soon as the UR-Program on the
// robot is started. Valid values: min=0.0, max=40.0
//
// Note: This parameter is only evaluated, when the parameter "use_tool_communication"
// is set to TRUE. Then, this parameter is required.
if (!robot_hw_nh.getParam("tool_tx_idle_chars", tx_idle_chars))
{
ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_tx_idle_chars") << " not given.");
return false;
}
tool_comm_setup->setTxIdleChars(tx_idle_chars);
}
// Hash of the calibration reported by the robot. This is used for validating the robot
// description is using the correct calibration. If the robot's calibration doesn't match this
// hash, an error will be printed. You can use the robot as usual, however Cartesian poses of the
// 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", "");
ROS_INFO_STREAM("Initializing urdriver");
try
{
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));
}
catch (ur_driver::ToolCommNotAvailable& e)
{
ROS_FATAL_STREAM(e.what() << " See parameter '" << robot_hw_nh.resolveName("use_tool_communication") << "'.");
return false;
}
catch (ur_driver::UrException& e)
{
ROS_FATAL_STREAM(e.what());
return false;
}
// Send arbitrary script commands to this topic. Note: On e-Series the robot has to be in
// remote-control mode.
//
// Sending scripts to this will stop program execution unless wrapped in a secondary program:
//
// sec myProgram():
// set_digital_out(0, True)
// end
command_sub_ = robot_hw_nh.subscribe("script_command", 1, &HardwareInterface::commandCallback, this);
// Names of the joints. Usually, this is given in the controller config file.
if (!root_nh.getParam("hardware_interface/joints", joint_names_))
{
ROS_ERROR_STREAM("Cannot find required parameter " << root_nh.resolveName("hardware_interface/joints")
<< " on the parameter server.");
throw std::runtime_error("Cannot find required parameter "
"'controller_joint_names' on the parameter server.");
}
// Create ros_control interfaces
for (std::size_t i = 0; i < joint_positions_.size(); ++i)
{
ROS_DEBUG_STREAM("Registing handles for joint " << joint_names_[i]);
// Create joint state interface for all joints
js_interface_.registerHandle(hardware_interface::JointStateHandle(joint_names_[i], &joint_positions_[i],
&joint_velocities_[i], &joint_efforts_[i]));
// Create joint position control interface
pj_interface_.registerHandle(
hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]));
spj_interface_.registerHandle(ur_controllers::ScaledJointHandle(
js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i], &speed_scaling_combined_));
}
speedsc_interface_.registerHandle(
ur_controllers::SpeedScalingHandle("speed_scaling_factor", &speed_scaling_combined_));
fts_interface_.registerHandle(hardware_interface::ForceTorqueSensorHandle(
"wrench", tf_prefix_ + "tool0_controller", fts_measurements_.begin(), fts_measurements_.begin() + 3));
// Register interfaces
registerInterface(&js_interface_);
registerInterface(&spj_interface_);
registerInterface(&pj_interface_);
registerInterface(&speedsc_interface_);
registerInterface(&fts_interface_);
tcp_pose_pub_.reset(new realtime_tools::RealtimePublisher<tf2_msgs::TFMessage>(root_nh, "/tf", 100));
io_pub_.reset(new realtime_tools::RealtimePublisher<ur_msgs::IOStates>(robot_hw_nh, "io_states", 1));
io_pub_->msg_.digital_in_states.resize(actual_dig_in_bits_.size());
io_pub_->msg_.digital_out_states.resize(actual_dig_out_bits_.size());
io_pub_->msg_.analog_in_states.resize(standard_analog_input_.size());
io_pub_->msg_.analog_out_states.resize(standard_analog_output_.size());
for (size_t i = 0; i < actual_dig_in_bits_.size(); ++i)
{
io_pub_->msg_.digital_in_states[i].pin = i;
}
for (size_t i = 0; i < actual_dig_out_bits_.size(); ++i)
{
io_pub_->msg_.digital_out_states[i].pin = i;
}
for (size_t i = 0; i < standard_analog_input_.size(); ++i)
{
io_pub_->msg_.analog_in_states[i].pin = i;
}
for (size_t i = 0; i < standard_analog_output_.size(); ++i)
{
io_pub_->msg_.analog_out_states[i].pin = i;
}
tool_data_pub_.reset(new realtime_tools::RealtimePublisher<ur_msgs::ToolDataMsg>(robot_hw_nh, "tool_data", 1));
// 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.
set_speed_slider_srv_ = robot_hw_nh.advertiseService("set_speed_slider", &HardwareInterface::setSpeedSlider, this);
// Service to set any of the robot's IOs
set_io_srv_ = robot_hw_nh.advertiseService("set_io", &HardwareInterface::setIO, this);
if (headless_mode)
{
// When in headless mode, this sends the URScript program to the robot for execution. Use this
// after the program has been interrupted, e.g. by a protective- or EM-stop.
resend_robot_program_srv_ =
robot_hw_nh.advertiseService("resend_robot_program", &HardwareInterface::resendRobotProgram, this);
}
// 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);
ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_robot_driver hardware_interface");
return true;
}
template <typename T>
void HardwareInterface::readData(const std::unique_ptr<rtde_interface::DataPackage>& data_pkg,
const std::string& var_name, T& data)
{
if (!data_pkg->getData(var_name, data))
{
// This throwing should never happen unless misconfigured
std::string error_msg = "Did not find '" + var_name + "' in data sent from robot. This should not happen!";
throw std::runtime_error(error_msg);
}
}
template <typename T, size_t N>
void HardwareInterface::readBitsetData(const std::unique_ptr<rtde_interface::DataPackage>& data_pkg,
const std::string& var_name, std::bitset<N>& data)
{
if (!data_pkg->getData<T, N>(var_name, data))
{
// This throwing should never happen unless misconfigured
std::string error_msg = "Did not find '" + var_name + "' in data sent from robot. This should not happen!";
throw std::runtime_error(error_msg);
}
}
void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period)
{
std::unique_ptr<rtde_interface::DataPackage> data_pkg = ur_driver_->getDataPackage();
if (data_pkg)
{
readData(data_pkg, "actual_q", joint_positions_);
readData(data_pkg, "actual_qd", joint_velocities_);
readData(data_pkg, "target_speed_fraction", target_speed_fraction_);
readData(data_pkg, "speed_scaling", speed_scaling_);
readData(data_pkg, "runtime_state", runtime_state_);
readData(data_pkg, "actual_TCP_force", fts_measurements_);
readData(data_pkg, "actual_TCP_pose", tcp_pose_);
readData(data_pkg, "standard_analog_input0", standard_analog_input_[0]);
readData(data_pkg, "standard_analog_input1", standard_analog_input_[1]);
readData(data_pkg, "standard_analog_output0", standard_analog_output_[0]);
readData(data_pkg, "standard_analog_output1", standard_analog_output_[1]);
readData(data_pkg, "tool_mode", tool_mode_);
readData(data_pkg, "tool_analog_input0", tool_analog_input_[0]);
readData(data_pkg, "tool_analog_input1", tool_analog_input_[1]);
readData(data_pkg, "tool_output_voltage", tool_output_voltage_);
readData(data_pkg, "tool_output_current", tool_output_current_);
readData(data_pkg, "tool_temperature", tool_temperature_);
readBitsetData<uint64_t>(data_pkg, "actual_digital_input_bits", actual_dig_in_bits_);
readBitsetData<uint64_t>(data_pkg, "actual_digital_output_bits", actual_dig_out_bits_);
readBitsetData<uint32_t>(data_pkg, "analog_io_types", analog_io_types_);
readBitsetData<uint32_t>(data_pkg, "tool_analog_input_types", tool_analog_input_types_);
publishIOData();
publishToolData();
// Transform fts measurements to tool frame
extractToolPose(time);
transformForceTorque();
publishPose();
// pausing state follows runtime state when pausing
if (runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PAUSED))
{
pausing_state_ = PausingState::PAUSED;
}
// When the robot resumed program execution and pausing state was PAUSED, we enter RAMPUP
else if (runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PLAYING) &&
pausing_state_ == PausingState::PAUSED)
{
speed_scaling_combined_ = 0.0;
pausing_state_ = PausingState::RAMPUP;
}
if (pausing_state_ == PausingState::RAMPUP)
{
double speed_scaling_ramp = speed_scaling_combined_ + pausing_ramp_up_increment_;
speed_scaling_combined_ = std::min(speed_scaling_ramp, speed_scaling_ * target_speed_fraction_);
if (speed_scaling_ramp > speed_scaling_ * target_speed_fraction_)
{
pausing_state_ = PausingState::RUNNING;
}
}
else if (runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::RESUMING))
{
// We have to keep speed scaling on ROS side at 0 during RESUMING to prevent controllers from
// continuing to interpolate
speed_scaling_combined_ = 0.0;
}
else
{
// Normal case
speed_scaling_combined_ = speed_scaling_ * target_speed_fraction_;
}
}
else
{
ROS_ERROR("Could not get fresh data package from robot");
}
}
void HardwareInterface ::write(const ros::Time& time, const ros::Duration& period)
{
if ((runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PLAYING) ||
runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PAUSING)) &&
robot_program_running_)
{
if (position_controller_running_)
{
ur_driver_->writeJointCommand(joint_position_command_);
}
else if (robot_program_running_)
{
ur_driver_->writeKeepalive();
}
else
{
ur_driver_->stopControl();
}
}
}
bool HardwareInterface ::prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list)
{
bool ret_val = true;
if (controllers_initialized_ && !isRobotProgramRunning() && !start_list.empty())
{
for (auto& controller : start_list)
{
if (!controller.claimed_resources.empty())
{
ROS_ERROR_STREAM("Robot control is currently inactive. Starting controllers that claim resources is currently "
"not possible. Not starting controller '"
<< controller.name << "'");
ret_val = false;
}
}
}
controllers_initialized_ = true;
return ret_val;
}
void HardwareInterface ::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list)
{
position_controller_running_ = false;
for (auto& controller_it : start_list)
{
for (auto& resource_it : controller_it.claimed_resources)
{
if (resource_it.hardware_interface == "ur_controllers::ScaledPositionJointInterface")
{
position_controller_running_ = true;
}
if (resource_it.hardware_interface == "hardware_interface::PositionJointInterface")
{
position_controller_running_ = true;
}
}
}
}
uint32_t HardwareInterface ::getControlFrequency() const
{
if (ur_driver_ != nullptr)
{
return ur_driver_->getControlFrequency();
}
throw std::runtime_error("ur_driver is not yet initialized");
}
void HardwareInterface ::transformForceTorque()
{
tcp_force_.setValue(fts_measurements_[0], fts_measurements_[1], fts_measurements_[2]);
tcp_torque_.setValue(fts_measurements_[3], fts_measurements_[4], fts_measurements_[5]);
tf2::Quaternion rotation_quat;
tf2::fromMsg(tcp_transform_.transform.rotation, rotation_quat);
tcp_force_ = tf2::quatRotate(rotation_quat.inverse(), tcp_force_);
tcp_torque_ = tf2::quatRotate(rotation_quat.inverse(), tcp_torque_);
fts_measurements_ = { tcp_force_.x(), tcp_force_.y(), tcp_force_.z(),
tcp_torque_.x(), tcp_torque_.y(), tcp_torque_.z() };
}
bool HardwareInterface ::isRobotProgramRunning() const
{
return robot_program_running_;
}
void HardwareInterface ::handleRobotProgramState(bool program_running)
{
if (robot_program_running_ == false && program_running)
{
controller_reset_necessary_ = true;
}
robot_program_running_ = program_running;
std_msgs::Bool msg;
msg.data = robot_program_running_;
program_state_pub_.publish(msg);
}
bool HardwareInterface ::shouldResetControllers()
{
if (controller_reset_necessary_)
{
controller_reset_necessary_ = false;
return true;
}
else
{
return false;
}
}
void HardwareInterface ::extractToolPose(const ros::Time& timestamp)
{
double tcp_angle = std::sqrt(std::pow(tcp_pose_[3], 2) + std::pow(tcp_pose_[4], 2) + std::pow(tcp_pose_[5], 2));
tf2::Vector3 rotation_vec(tcp_pose_[3], tcp_pose_[4], tcp_pose_[5]);
tf2::Quaternion rotation;
if (tcp_angle > 1e-16)
{
rotation.setRotation(rotation_vec.normalized(), tcp_angle);
}
tcp_transform_.header.stamp = timestamp;
tcp_transform_.transform.translation.x = tcp_pose_[0];
tcp_transform_.transform.translation.y = tcp_pose_[1];
tcp_transform_.transform.translation.z = tcp_pose_[2];
tcp_transform_.transform.rotation = tf2::toMsg(rotation);
}
void HardwareInterface ::publishPose()
{
if (tcp_pose_pub_)
{
if (tcp_pose_pub_->trylock())
{
tcp_pose_pub_->msg_.transforms.clear();
tcp_pose_pub_->msg_.transforms.push_back(tcp_transform_);
tcp_pose_pub_->unlockAndPublish();
}
}
}
void HardwareInterface::publishIOData()
{
if (io_pub_)
{
if (io_pub_->trylock())
{
for (size_t i = 0; i < actual_dig_in_bits_.size(); ++i)
{
io_pub_->msg_.digital_in_states[i].state = actual_dig_in_bits_[i];
}
for (size_t i = 0; i < actual_dig_out_bits_.size(); ++i)
{
io_pub_->msg_.digital_out_states[i].state = actual_dig_out_bits_[i];
}
for (size_t i = 0; i < standard_analog_input_.size(); ++i)
{
io_pub_->msg_.analog_in_states[i].state = standard_analog_input_[i];
io_pub_->msg_.analog_in_states[i].domain = analog_io_types_[i];
}
for (size_t i = 0; i < standard_analog_output_.size(); ++i)
{
io_pub_->msg_.analog_out_states[i].state = standard_analog_output_[i];
io_pub_->msg_.analog_out_states[i].domain = analog_io_types_[i + 2];
}
io_pub_->unlockAndPublish();
}
}
}
void HardwareInterface::publishToolData()
{
if (tool_data_pub_)
{
if (tool_data_pub_->trylock())
{
tool_data_pub_->msg_.tool_mode = tool_mode_;
tool_data_pub_->msg_.analog_input_range2 = tool_analog_input_types_[0];
tool_data_pub_->msg_.analog_input_range3 = tool_analog_input_types_[1];
tool_data_pub_->msg_.analog_input2 = tool_analog_input_[0];
tool_data_pub_->msg_.analog_input2 = tool_analog_input_[1];
tool_data_pub_->msg_.tool_output_voltage = tool_output_voltage_;
tool_data_pub_->msg_.tool_current = tool_output_current_;
tool_data_pub_->msg_.tool_temperature = tool_temperature_;
tool_data_pub_->unlockAndPublish();
}
}
}
bool HardwareInterface::stopControl(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res)
{
if (isRobotProgramRunning())
{
robot_program_running_ = false;
res.success = true;
res.message = "Deactivated control";
}
else
{
res.success = true;
res.message = "No control active. Nothing to do.";
}
return true;
}
bool HardwareInterface::setSpeedSlider(ur_msgs::SetSpeedSliderFractionRequest& req,
ur_msgs::SetSpeedSliderFractionResponse& res)
{
if (req.speed_slider_fraction >= 0.01 && req.speed_slider_fraction <= 1.0 && ur_driver_ != nullptr)
{
res.success = ur_driver_->getRTDEWriter().sendSpeedSlider(req.speed_slider_fraction);
}
else
{
res.success = false;
}
return true;
}
bool HardwareInterface::setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& res)
{
if (req.fun == req.FUN_SET_DIGITAL_OUT && ur_driver_ != nullptr)
{
if (req.pin <= 7)
{
res.success = ur_driver_->getRTDEWriter().sendStandardDigitalOutput(req.pin, req.state);
}
else if (req.pin <= 15)
{
res.success = ur_driver_->getRTDEWriter().sendConfigurableDigitalOutput(req.pin - 8, req.state);
}
else
{
res.success = ur_driver_->getRTDEWriter().sendToolDigitalOutput(req.pin - 16, req.state);
}
}
else if (req.fun == req.FUN_SET_ANALOG_OUT && ur_driver_ != nullptr)
{
res.success = ur_driver_->getRTDEWriter().sendStandardAnalogOutput(req.pin, req.state);
}
else
{
LOG_ERROR("Cannot execute function %u. This is not (yet) supported.", req.fun);
res.success = false;
}
return true;
}
bool HardwareInterface::resendRobotProgram(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res)
{
res.success = ur_driver_->sendRobotProgram();
if (res.success)
{
res.message = "Successfully resent robot program";
}
else
{
res.message = "Could not resend robot program";
}
return true;
}
void HardwareInterface::commandCallback(const std_msgs::StringConstPtr& msg)
{
std::string str = msg->data;
if (str.back() != '\n')
{
str.append("\n");
}
if (ur_driver_ == nullptr)
{
throw std::runtime_error("Trying to use the ur_driver_ member before it is initialized. This should not happen, "
"please contact the package maintainer.");
}
if (ur_driver_->sendScript(str))
{
ROS_DEBUG_STREAM("Sent script to robot");
}
else
{
ROS_ERROR_STREAM("Error sending script to robot");
}
}
} // namespace ur_driver

158
ur_robot_driver/src/ros/hardware_interface_node.cpp Normal file
View File

@@ -0,0 +1,158 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-11
*
*/
//----------------------------------------------------------------------
#include <ros/ros.h>
#include <controller_manager/controller_manager.h>
#include <csignal>
#include <ur_robot_driver/ros/hardware_interface.h>
std::unique_ptr<ur_driver::HardwareInterface> g_hw_interface;
void signalHandler(int signum)
{
std::cout << "Interrupt signal (" << signum << ") received.\n";
g_hw_interface.reset();
// cleanup and close up stuff here
// terminate program
exit(signum);
}
int main(int argc, char** argv)
{
// Set up ROS.
ros::init(argc, argv, "ur_hardware_interface");
ros::AsyncSpinner spinner(2);
spinner.start();
ros::NodeHandle nh;
ros::NodeHandle nh_priv("~");
// register signal SIGINT and signal handler
signal(SIGINT, signalHandler);
// Set up timers
ros::Time timestamp;
ros::Duration period;
auto stopwatch_last = std::chrono::steady_clock::now();
auto stopwatch_now = stopwatch_last;
g_hw_interface.reset(new ur_driver::HardwareInterface);
if (!g_hw_interface->init(nh, nh_priv))
{
ROS_ERROR_STREAM("Could not correctly initialize robot. Exiting");
exit(1);
}
ROS_DEBUG_STREAM("initialized hw interface");
controller_manager::ControllerManager cm(g_hw_interface.get(), nh);
// Get current time and elapsed time since last read
timestamp = ros::Time::now();
stopwatch_now = std::chrono::steady_clock::now();
period.fromSec(std::chrono::duration_cast<std::chrono::duration<double>>(stopwatch_now - stopwatch_last).count());
stopwatch_last = stopwatch_now;
std::ifstream realtime_file("/sys/kernel/realtime", std::ios::in);
bool has_realtime;
realtime_file >> has_realtime;
if (has_realtime)
{
const int max_thread_priority = sched_get_priority_max(SCHED_FIFO);
if (max_thread_priority != -1)
{
// We'll operate on the currently running thread.
pthread_t this_thread = pthread_self();
// struct sched_param is used to store the scheduling priority
struct sched_param params;
// We'll set the priority to the maximum.
params.sched_priority = max_thread_priority;
int ret = pthread_setschedparam(this_thread, SCHED_FIFO, &params);
if (ret != 0)
{
ROS_ERROR_STREAM("Unsuccessful in setting main thread realtime priority. Error code: " << ret);
}
// Now verify the change in thread priority
int policy = 0;
ret = pthread_getschedparam(this_thread, &policy, &params);
if (ret != 0)
{
std::cout << "Couldn't retrieve real-time scheduling paramers" << std::endl;
}
// Check the correct policy was applied
if (policy != SCHED_FIFO)
{
ROS_ERROR("Main thread: Scheduling is NOT SCHED_FIFO!");
}
else
{
ROS_INFO("Main thread: SCHED_FIFO OK");
}
// Print thread scheduling priority
ROS_INFO_STREAM("Main thread priority is " << params.sched_priority);
}
else
{
ROS_ERROR("Could not get maximum thread priority for main thread");
}
}
double expected_cycle_time = 1.0 / (static_cast<double>(g_hw_interface->getControlFrequency()));
// Run as fast as possible
while (ros::ok())
{
// Receive current state from robot
g_hw_interface->read(timestamp, period);
// Get current time and elapsed time since last read
timestamp = ros::Time::now();
stopwatch_now = std::chrono::steady_clock::now();
period.fromSec(std::chrono::duration_cast<std::chrono::duration<double>>(stopwatch_now - stopwatch_last).count());
stopwatch_last = stopwatch_now;
cm.update(timestamp, period, g_hw_interface->shouldResetControllers());
g_hw_interface->write(timestamp, period);
// if (!control_rate.sleep())
if (period.toSec() > expected_cycle_time)
{
// ROS_WARN_STREAM("Could not keep cycle rate of " << expected_cycle_time * 1000 << "ms");
// ROS_WARN_STREAM("Actual cycle time:" << period.toNSec() / 1000000.0 << "ms");
}
}
spinner.stop();
ROS_INFO_STREAM_NAMED("hardware_interface", "Shutting down.");
return 0;
}

48
ur_robot_driver/src/rtde/control_package_pause.cpp Normal file
View File

@@ -0,0 +1,48 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#include "ur_robot_driver/rtde/control_package_pause.h"
namespace ur_driver
{
namespace rtde_interface
{
bool ControlPackagePause::parseWith(comm::BinParser& bp)
{
bp.parse(accepted_);
return true;
}
std::string ControlPackagePause::toString() const
{
std::stringstream ss;
ss << "accepted: " << static_cast<int>(accepted_);
return ss.str();
}
} // namespace rtde_interface
} // namespace ur_driver

70
ur_robot_driver/src/rtde/control_package_setup_inputs.cpp Normal file
View File

@@ -0,0 +1,70 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#include "ur_robot_driver/rtde/control_package_setup_inputs.h"
namespace ur_driver
{
namespace rtde_interface
{
bool ControlPackageSetupInputs::parseWith(comm::BinParser& bp)
{
bp.parse(input_recipe_id_);
bp.parseRemainder(variable_types_);
return true;
}
std::string ControlPackageSetupInputs::toString() const
{
std::stringstream ss;
ss << "input recipe id: " << static_cast<int>(input_recipe_id_) << std::endl;
ss << "variable types: " << variable_types_;
return ss.str();
}
size_t ControlPackageSetupInputsRequest::generateSerializedRequest(uint8_t* buffer,
std::vector<std::string> variable_names)
{
if (variable_names.size() == 0)
{
return 0;
}
std::string variables;
for (const auto& piece : variable_names)
variables += (piece + ",");
variables.pop_back();
uint16_t payload_size = variables.size();
size_t size = 0;
size += PackageHeader::serializeHeader(buffer, PACKAGE_TYPE, payload_size);
size += comm::PackageSerializer::serialize(buffer + size, variables);
return size;
}
} // namespace rtde_interface
} // namespace ur_driver

92
ur_robot_driver/src/rtde/control_package_setup_outputs.cpp Normal file
View File

@@ -0,0 +1,92 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#include "ur_robot_driver/rtde/control_package_setup_outputs.h"
namespace ur_driver
{
namespace rtde_interface
{
bool ControlPackageSetupOutputs::parseWith(comm::BinParser& bp)
{
bp.parse(output_recipe_id_);
bp.parseRemainder(variable_types_);
return true;
}
std::string ControlPackageSetupOutputs::toString() const
{
std::stringstream ss;
ss << "output recipe id: " << static_cast<int>(output_recipe_id_) << std::endl;
ss << "variable types: " << variable_types_;
return ss.str();
}
size_t ControlPackageSetupOutputsRequest::generateSerializedRequest(uint8_t* buffer, double output_frequency,
std::vector<std::string> variable_names)
{
if (variable_names.size() == 0)
{
return 0;
}
std::string variables;
for (const auto& piece : variable_names)
variables += (piece + ",");
variables.pop_back();
uint16_t payload_size = sizeof(double) + variables.size();
size_t size = 0;
size += PackageHeader::serializeHeader(buffer, PACKAGE_TYPE, payload_size);
size += comm::PackageSerializer::serialize(buffer + size, output_frequency);
size += comm::PackageSerializer::serialize(buffer + size, variables);
return size;
}
size_t ControlPackageSetupOutputsRequest::generateSerializedRequest(uint8_t* buffer,
std::vector<std::string> variable_names)
{
if (variable_names.size() == 0)
{
return 0;
}
std::string variables;
for (const auto& piece : variable_names)
variables += (piece + ",");
variables.pop_back();
uint16_t payload_size = variables.size();
size_t size = 0;
size += PackageHeader::serializeHeader(buffer, PACKAGE_TYPE, payload_size);
size += comm::PackageSerializer::serialize(buffer + size, variables);
return size;
}
} // namespace rtde_interface
} // namespace ur_driver

Some files were not shown because too many files have changed in this diff Show More