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

moved driver to subfolder

Browse Source
This commit is contained in:
Felix Mauch
2019-04-17 17:03:34 +02:00
parent 7a31b25e38
commit be5553648d
104 changed files with 0 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

140
ur_rtde_driver/CMakeLists.txt Normal file
View File

@@ -0,0 +1,140 @@
cmake_minimum_required(VERSION 2.8.12)
project(ur_rtde_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
trajectory_msgs
ur_controllers
ur_msgs
)
find_package(Boost REQUIRED)
catkin_package(
INCLUDE_DIRS
include
LIBRARIES
ur_rtde_driver
CATKIN_DEPENDS
actionlib
control_msgs
controller_manager
geometry_msgs
hardware_interface
industrial_msgs
roscpp
sensor_msgs
trajectory_msgs
ur_controllers
ur_msgs
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_rtde_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/ur/rt_state.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
)
target_link_libraries(ur_rtde_driver ${catkin_LIBRARIES})
add_dependencies(ur_rtde_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_executable(ur_rtde_driver_node
src/ros/hardware_interface.cpp
src/ros/hardware_interface_node.cpp
)
target_link_libraries(ur_rtde_driver_node ${catkin_LIBRARIES} ur_rtde_driver)
add_dependencies(ur_rtde_driver_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_executable(plain_driver
src/main_plain_driver.cpp
)
target_link_libraries(plain_driver ${catkin_LIBRARIES} ur_rtde_driver)
add_dependencies(plain_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
install(TARGETS ur_rtde_driver ur_rtde_driver_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME 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"
)
#if (CATKIN_ENABLE_TESTING)
#set(${PROJECT_NAME}_TEST_SOURCES
#tests/ur/master_board.cpp
#tests/ur/robot_mode.cpp
#tests/ur/rt_state.cpp)
#catkin_add_gtest(ur_rtde_driver_test ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_TEST_SOURCES} tests/main.cpp)
#target_link_libraries(ur_rtde_driver_test ur_rtde_driver ${catkin_LIBRARIES})
#endif()

180
ur_rtde_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

3
ur_rtde_driver/README.md Normal file
View File

@@ -0,0 +1,3 @@
# ur_rtde_driver
This driver is forked from the [ur_modern_driver](https://github.com/ros-industrial/ur_modern_driver).

112
ur_rtde_driver/config/ur10_controllers.yaml Normal file
View File

@@ -0,0 +1,112 @@
# Settings for ros_control control loop
hardware_control_loop:
loop_hz: 125
# Settings for ros_control hardware interface
hardware_interface:
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
pos_based_pos_traj_controller:
type: position_controllers/JointTrajectoryController
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
elbow_joint: {trajectory: 0.1, goal: 0.1}
wrist_1_joint: {trajectory: 0.1, goal: 0.1}
wrist_2_joint: {trajectory: 0.1, goal: 0.1}
wrist_3_joint: {trajectory: 0.1, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 125
action_monitor_rate: 10
# state_publish_rate: 50 # Defaults to 50
# action_monitor_rate: 20 # Defaults to 20
#stop_trajectory_duration: 0 # Defaults to 0.0
# Joint Trajectory Controller -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
vel_based_pos_traj_controller:
type: velocity_controllers/JointTrajectoryController
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
elbow_joint: {trajectory: 0.1, goal: 0.1}
wrist_1_joint: {trajectory: 0.1, goal: 0.1}
wrist_2_joint: {trajectory: 0.1, goal: 0.1}
wrist_3_joint: {trajectory: 0.1, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 125
action_monitor_rate: 10
gains:
#!!These values have not been optimized!!
shoulder_pan_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
shoulder_lift_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
elbow_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
wrist_1_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
wrist_2_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
wrist_3_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
# Use a feedforward term to reduce the size of PID gains
velocity_ff:
shoulder_pan_joint: 1.0
shoulder_lift_joint: 1.0
elbow_joint: 1.0
wrist_1_joint: 1.0
wrist_2_joint: 1.0
wrist_3_joint: 1.0
# state_publish_rate: 50 # Defaults to 50
# action_monitor_rate: 20 # Defaults to 20
#stop_trajectory_duration: 0 # Defaults to 0.0
# Pass an array of joint velocities directly to the joints
joint_group_vel_controller:
type: velocity_controllers/JointGroupVelocityController
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint

107
ur_rtde_driver/config/ur3_controllers.yaml Normal file
View File

@@ -0,0 +1,107 @@
# Settings for ros_control control loop
hardware_control_loop:
loop_hz: 125
# Settings for ros_control hardware interface
hardware_interface:
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
# Joint Trajectory Controller - position based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
pos_based_pos_traj_controller:
type: position_controllers/JointTrajectoryController
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
elbow_joint: {trajectory: 0.1, goal: 0.1}
wrist_1_joint: {trajectory: 0.1, goal: 0.1}
wrist_2_joint: {trajectory: 0.1, goal: 0.1}
wrist_3_joint: {trajectory: 0.1, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 125
action_monitor_rate: 10
# state_publish_rate: 50 # Defaults to 50
# action_monitor_rate: 20 # Defaults to 20
#stop_trajectory_duration: 0 # Defaults to 0.0
# Joint Trajectory Controller - velocity based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
vel_based_pos_traj_controller:
type: velocity_controllers/JointTrajectoryController
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
elbow_joint: {trajectory: 0.1, goal: 0.1}
wrist_1_joint: {trajectory: 0.1, goal: 0.1}
wrist_2_joint: {trajectory: 0.1, goal: 0.1}
wrist_3_joint: {trajectory: 0.1, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 125
action_monitor_rate: 10
gains:
#!!These values have not been optimized!!
shoulder_pan_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
shoulder_lift_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
elbow_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
wrist_1_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
wrist_2_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
wrist_3_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
# Use a feedforward term to reduce the size of PID gains
velocity_ff:
shoulder_pan_joint: 1.0
shoulder_lift_joint: 1.0
elbow_joint: 1.0
wrist_1_joint: 1.0
wrist_2_joint: 1.0
wrist_3_joint: 1.0
# state_publish_rate: 50 # Defaults to 50
# action_monitor_rate: 20 # Defaults to 20
#stop_trajectory_duration: 0 # Defaults to 0.0
# Pass an array of joint velocities directly to the joints
joint_group_vel_controller:
type: velocity_controllers/JointGroupVelocityController
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint

107
ur_rtde_driver/config/ur5_controllers.yaml Normal file
View File

@@ -0,0 +1,107 @@
# Settings for ros_control control loop
hardware_control_loop:
loop_hz: 125
# Settings for ros_control hardware interface
hardware_interface:
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
# Joint Trajectory Controller - position based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
pos_based_pos_traj_controller:
type: position_controllers/JointTrajectoryController
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
elbow_joint: {trajectory: 0.1, goal: 0.1}
wrist_1_joint: {trajectory: 0.1, goal: 0.1}
wrist_2_joint: {trajectory: 0.1, goal: 0.1}
wrist_3_joint: {trajectory: 0.1, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 125
action_monitor_rate: 10
# state_publish_rate: 50 # Defaults to 50
# action_monitor_rate: 20 # Defaults to 20
#stop_trajectory_duration: 0 # Defaults to 0.0
# Joint Trajectory Controller - velocity based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
vel_based_pos_traj_controller:
type: velocity_controllers/JointTrajectoryController
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
elbow_joint: {trajectory: 0.1, goal: 0.1}
wrist_1_joint: {trajectory: 0.1, goal: 0.1}
wrist_2_joint: {trajectory: 0.1, goal: 0.1}
wrist_3_joint: {trajectory: 0.1, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 125
action_monitor_rate: 10
gains:
#!!These values have not been optimized!!
shoulder_pan_joint: {p: 1.2, i: 0.01, d: 0.1, i_clamp: 1}
shoulder_lift_joint: {p: 1.2, i: 0.01, d: 0.1, i_clamp: 1}
elbow_joint: {p: 1.2, i: 0.01, d: 0.1, i_clamp: 1}
wrist_1_joint: {p: 1.2, i: 0.01, d: 0.1, i_clamp: 1}
wrist_2_joint: {p: 1.2, i: 0.01, d: 0.1, i_clamp: 1}
wrist_3_joint: {p: 1.2, i: 0.01, d: 0.1, i_clamp: 1}
# Use a feedforward term to reduce the size of PID gains
velocity_ff:
shoulder_pan_joint: 1.0
shoulder_lift_joint: 1.0
elbow_joint: 1.0
wrist_1_joint: 1.0
wrist_2_joint: 1.0
wrist_3_joint: 1.0
# state_publish_rate: 50 # Defaults to 50
# action_monitor_rate: 20 # Defaults to 20
#stop_trajectory_duration: 0 # Defaults to 0.0
# Pass an array of joint velocities directly to the joints
joint_group_vel_controller:
type: velocity_controllers/JointGroupVelocityController
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint

250
ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h Normal file
View File

@@ -0,0 +1,250 @@
/*
* 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 "ur_rtde_driver/log.h"
#include "ur_rtde_driver/types.h"
namespace ur_driver
{
namespace comm
{
class BinParser
{
private:
uint8_t *buf_pos_, *buf_end_;
BinParser& parent_;
public:
BinParser(uint8_t* buffer, size_t buf_len) : buf_pos_(buffer), buf_end_(buffer + buf_len), parent_(*this)
{
assert(buf_pos_ <= buf_end_);
}
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_);
}
~BinParser()
{
parent_.buf_pos_ = buf_pos_;
}
// 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);
}
template <typename T>
T peek()
{
assert(buf_pos_ + sizeof(T) <= buf_end_);
T val;
std::memcpy(&val, buf_pos_, sizeof(T));
return decode(val);
}
template <typename T>
void parse(T& val)
{
val = peek<T>();
buf_pos_ += sizeof(T);
}
void parse(double& val)
{
uint64_t inner;
parse<uint64_t>(inner);
std::memcpy(&val, &inner, sizeof(double));
}
void parse(float& val)
{
uint32_t inner;
parse<uint32_t>(inner);
std::memcpy(&val, &inner, sizeof(float));
}
// 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
void parse(bool& val)
{
uint8_t inner;
parse<uint8_t>(inner);
val = inner != 0;
}
// Explicit parsing order of fields to avoid issues with struct layout
void parse(double3_t& val)
{
parse(val.x);
parse(val.y);
parse(val.z);
}
void parse(vector3d_t& val)
{
for (size_t i = 0; i < val.size(); ++i)
{
parse(val[i]);
}
}
void parse(vector6d_t& val)
{
for (size_t i = 0; i < val.size(); ++i)
{
parse(val[i]);
}
}
void parse(vector6int32_t& val)
{
for (size_t i = 0; i < val.size(); ++i)
{
parse(val[i]);
}
}
void parse(vector6uint32_t& val)
{
for (size_t i = 0; i < val.size(); ++i)
{
parse(val[i]);
}
}
// Explicit parsing order of fields to avoid issues with struct layout
void parse(cartesian_coord_t& val)
{
parse(val.position);
parse(val.rotation);
}
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();
}
void parseRemainder(std::string& val)
{
parse(val, size_t(buf_end_ - buf_pos_));
}
void parse(std::string& val, size_t len)
{
val.assign(reinterpret_cast<char*>(buf_pos_), len);
buf_pos_ += len;
}
// Special string parse function that assumes uint8_t len followed by chars
void parse(std::string& val)
{
uint8_t len;
parse(len);
parse(val, size_t(len));
}
template <typename T, size_t N>
void parse(std::array<T, N>& array)
{
for (size_t i = 0; i < N; i++)
{
parse(array[i]);
}
}
template <typename T, size_t N>
void parse(std::bitset<N>& set)
{
T val;
parse(val);
set = std::bitset<N>(val);
}
void consume()
{
buf_pos_ = buf_end_;
}
void consume(size_t bytes)
{
buf_pos_ += bytes;
}
bool checkSize(size_t bytes)
{
return bytes <= size_t(buf_end_ - buf_pos_);
}
template <typename T>
bool checkSize(void)
{
return checkSize(T::SIZE);
}
bool empty()
{
return buf_pos_ == buf_end_;
}
void debug()
{
LOG_DEBUG("BinParser: %p - %p (%zu bytes)", buf_pos_, buf_end_, buf_end_ - buf_pos_);
}
};
} // namespace comm
} // namespace ur_driver

63
ur_rtde_driver/include/ur_rtde_driver/comm/package.h Normal file
View File

@@ -0,0 +1,63 @@
// 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_rtde_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;
using _header_type = HeaderT;
virtual bool parseWith(BinParser& bp) = 0;
virtual std::string toString() const = 0;
private:
HeaderT header_;
};
} // namespace comm
} // namespace ur_driver
#endif // ifndef UR_RTDE_DRIVER_PACKAGE_H_INCLUDED

95
ur_rtde_driver/include/ur_rtde_driver/comm/package_serializer.h Normal file
View File

@@ -0,0 +1,95 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- htobeGIN 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
{
class PackageSerializer
{
public:
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;
}
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

56
ur_rtde_driver/include/ur_rtde_driver/comm/parser.h Normal file
View File

@@ -0,0 +1,56 @@
/*
* 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_rtde_driver/comm/bin_parser.h"
#include "ur_rtde_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 instant of class binaryParser
* \param results 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

254
ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h Normal file
View File

@@ -0,0 +1,254 @@
/*
* 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_rtde_driver/comm/package.h"
#include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/queue/readerwriterqueue.h"
#include <atomic>
#include <chrono>
#include <thread>
#include <vector>
namespace ur_driver
{
namespace comm
{
// TODO: Remove these!!!
using namespace moodycamel;
template <typename T>
class IConsumer
{
public:
virtual void setupConsumer()
{
}
virtual void teardownConsumer()
{
}
virtual void stopConsumer()
{
}
virtual void onTimeout()
{
}
virtual bool consume(std::shared_ptr<T> product) = 0;
};
template <typename T>
class MultiConsumer : public IConsumer<T>
{
private:
std::vector<IConsumer<T>*> consumers_;
public:
MultiConsumer(std::vector<IConsumer<T>*> consumers) : consumers_(consumers)
{
}
virtual void setupConsumer()
{
for (auto& con : consumers_)
{
con->setupConsumer();
}
}
virtual void teardownConsumer()
{
for (auto& con : consumers_)
{
con->teardownConsumer();
}
}
virtual void stopConsumer()
{
for (auto& con : consumers_)
{
con->stopConsumer();
}
}
virtual void onTimeout()
{
for (auto& con : consumers_)
{
con->onTimeout();
}
}
bool consume(std::shared_ptr<T> product)
{
bool res = true;
for (auto& con : consumers_)
{
if (!con->consume(product))
res = false;
}
return res;
}
};
template <typename HeaderT>
class IProducer
{
public:
virtual void setupProducer()
{
}
virtual void teardownProducer()
{
}
virtual void stopProducer()
{
}
virtual bool tryGet(std::vector<std::unique_ptr<URPackage<HeaderT>>>& products) = 0;
};
class INotifier
{
public:
virtual void started(std::string name)
{
}
virtual void stopped(std::string name)
{
}
};
template <typename HeaderT>
class Pipeline
{
public:
typedef std::chrono::high_resolution_clock Clock;
typedef Clock::time_point Time;
using _package_type = URPackage<HeaderT>;
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 }
{
}
Pipeline(IProducer<HeaderT>& producer, std::string name, INotifier& notifier)
: producer_(producer), consumer_(nullptr), name_(name), notifier_(notifier), queue_{ 32 }, running_{ false }
{
}
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_);
}
void stop()
{
if (!running_)
return;
LOG_DEBUG("Stopping pipeline! <%s>", name_.c_str());
if (consumer_ != nullptr)
consumer_->stopConsumer();
producer_.stopProducer();
running_ = false;
pThread_.join();
cThread_.join();
notifier_.stopped(name_);
}
bool getLatestProduct(std::unique_ptr<URPackage<HeaderT>>& product, std::chrono::milliseconds timeout)
{
return queue_.wait_dequeue_timed(product, timeout);
}
private:
IProducer<HeaderT>& producer_;
IConsumer<_package_type>* consumer_;
std::string name_;
INotifier& notifier_;
BlockingReaderWriterQueue<std::unique_ptr<_package_type>> queue_;
std::atomic<bool> running_;
std::thread pThread_, cThread_;
void runProducer()
{
std::vector<std::unique_ptr<_package_type>> products;
while (running_)
{
if (!producer_.tryGet(products))
{
break;
}
for (auto& p : products)
{
if (!queue_.try_enqueue(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());
if (consumer_ != nullptr)
consumer_->stopConsumer();
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_.wait_dequeue_timed(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());
producer_.stopProducer();
running_ = false;
notifier_.stopped(name_);
}
};
} // namespace comm
} // namespace ur_driver

92
ur_rtde_driver/include/ur_rtde_driver/comm/producer.h Normal file
View File

@@ -0,0 +1,92 @@
/*
* 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_rtde_driver/comm/pipeline.h"
#include "ur_rtde_driver/comm/parser.h"
#include "ur_rtde_driver/comm/stream.h"
#include "ur_rtde_driver/comm/package.h"
namespace ur_driver
{
namespace comm
{
template <typename HeaderT>
class URProducer : public IProducer<HeaderT>
{
private:
URStream<HeaderT>& stream_;
Parser<HeaderT>& parser_;
std::chrono::seconds timeout_;
public:
URProducer(URStream<HeaderT>& stream, Parser<HeaderT>& parser) : stream_(stream), parser_(parser), timeout_(1)
{
}
void setupProducer()
{
stream_.connect();
}
void teardownProducer()
{
stream_.disconnect();
}
void stopProducer()
{
stream_.disconnect();
}
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

92
ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h 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-11
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_REVERSE_INTERFACE_H_INCLUDED
#define UR_RTDE_DRIVER_REVERSE_INTERFACE_H_INCLUDED
#include "ur_rtde_driver/comm/server.h"
#include "ur_rtde_driver/types.h"
#include <cstring>
#include <endian.h>
namespace ur_driver
{
namespace comm
{
class ReverseInterface
{
public:
ReverseInterface() = delete;
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");
}
}
~ReverseInterface() = default;
bool write(const vector6d_t& positions)
{
uint8_t buffer[sizeof(uint32_t) * 7];
uint8_t* b_pos = buffer;
for (auto const& pos : positions)
{
int32_t val = static_cast<int32_t>(pos * MULT_JOINTSTATE);
val = htobe32(val);
b_pos += append(b_pos, val);
}
int32_t val = htobe32(1);
append(b_pos, val);
size_t written;
return server_.write(buffer, sizeof(buffer), written);
}
template <typename T>
size_t append(uint8_t* buffer, T& val)
{
size_t s = sizeof(T);
std::memcpy(buffer, &val, s);
return s;
}
private:
URServer server_;
static const int32_t MULT_JOINTSTATE = 1000000;
};
} // namespace comm
} // namespace ur_driver
#endif // UR_RTDE_DRIVER_REVERSE_INTERFACE_H_INCLUDED

55
ur_rtde_driver/include/ur_rtde_driver/comm/server.h Normal file
View File

@@ -0,0 +1,55 @@
/*
* 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_rtde_driver/comm/tcp_socket.h"
namespace ur_driver
{
namespace comm
{
#define MAX_SERVER_BUF_LEN 50
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:
URServer(int port);
~URServer();
std::string getIP();
bool bind();
bool accept();
void disconnectClient();
bool readLine(char* buffer, size_t buf_len);
bool write(const uint8_t* buf, size_t buf_len, size_t& written);
};
} // namespace comm
} // namespace ur_driver

57
ur_rtde_driver/include/ur_rtde_driver/comm/shell_consumer.h Normal file
View File

@@ -0,0 +1,57 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// -- 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_rtde_driver/log.h"
#include "ur_rtde_driver/comm/pipeline.h"
#include "ur_rtde_driver/comm/package.h"
namespace ur_driver
{
namespace comm
{
template <typename HeaderT>
class ShellConsumer : public IConsumer<URPackage<HeaderT>>
{
public:
ShellConsumer() = default;
virtual ~ShellConsumer() = default;
virtual void setupConsumer()
{
}
virtual void teardownConsumer()
{
}
virtual void stopConsumer()
{
}
virtual void onTimeout()
{
}
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_rtde_driver/include/ur_rtde_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_rtde_driver/log.h"
#include "ur_rtde_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_INFO("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

116
ur_rtde_driver/include/ur_rtde_driver/comm/tcp_socket.h Normal file
View File

@@ -0,0 +1,116 @@
/*
* 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>
namespace ur_driver
{
namespace comm
{
enum class SocketState
{
Invalid,
Connected,
Disconnected,
Closed
};
/*!
* \brief Class for TCP socket abstraction
*/
class TCPSocket
{
private:
std::atomic<int> socket_fd_;
std::atomic<SocketState> state_;
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:
TCPSocket();
virtual ~TCPSocket();
SocketState getState()
{
return state_;
}
int getSocketFD()
{
return socket_fd_;
}
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();
/*!
* \brief Reads one byte from the socket
*
* \param character[out] 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();
};
} // namespace comm
} // namespace ur_driver

105
ur_rtde_driver/include/ur_rtde_driver/event_counter.h Normal file
View File

@@ -0,0 +1,105 @@
/*
* 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 <cstdlib>
#include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/ur/consumer.h"
namespace ur_driver
{
class EventCounter : public URRTPacketConsumer
{
private:
typedef std::chrono::high_resolution_clock Clock;
Clock::time_point events_[250];
size_t idx_ = 0;
Clock::time_point last_;
public:
void trigger()
{
// auto now = Clock::now();
// LOG_INFO("Time diff: %d ms", std::chrono::duration_cast<std::chrono::microseconds>(now - last_));
// last_ = now;
// return;
events_[idx_] = Clock::now();
idx_ += 1;
if (idx_ > 250)
{
std::chrono::time_point<std::chrono::high_resolution_clock> t_min =
std::chrono::time_point<std::chrono::high_resolution_clock>::max();
std::chrono::time_point<std::chrono::high_resolution_clock> t_max =
std::chrono::time_point<std::chrono::high_resolution_clock>::min();
for (auto const& e : events_)
{
if (e < t_min)
t_min = e;
if (e > t_max)
t_max = e;
}
auto diff = t_max - t_min;
auto secs = std::chrono::duration_cast<std::chrono::seconds>(diff).count();
auto ms = std::chrono::duration_cast<std::chrono::microseconds>(diff).count();
std::chrono::duration<double> test(t_max - t_min);
LOG_INFO("Recieved 250 messages at %f Hz", (250.0 / test.count()));
idx_ = 0;
}
}
public:
bool consume(RTState_V1_6__7& state)
{
trigger();
return true;
}
bool consume(RTState_V1_8& state)
{
trigger();
return true;
}
bool consume(RTState_V3_0__1& state)
{
trigger();
return true;
}
bool consume(RTState_V3_2__3& state)
{
trigger();
return true;
}
void setupConsumer()
{
last_ = Clock::now();
}
void teardownConsumer()
{
}
void stopConsumer()
{
}
};
} // namespace ur_driver

39
ur_rtde_driver/include/ur_rtde_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

72
ur_rtde_driver/include/ur_rtde_driver/primary/package_header.h Normal file
View File

@@ -0,0 +1,72 @@
// 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_rtde_driver/types.h"
namespace ur_driver
{
namespace primary_interface
{
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
};
class PackageHeader
{
public:
PackageHeader() = default;
virtual ~PackageHeader() = default;
using _package_size_type = int32_t;
static size_t getPackageLength(uint8_t* buf)
{
return be32toh(*(reinterpret_cast<_package_size_type*>(buf)));
}
private:
_package_size_type package_size_;
RobotPackageType package_type_;
};
} // namespace primary_interface
} // namespace ur_driver
#endif // ifndef UR_RTDE_DRIVER_PRIMARY_INTERFACE_H_INCLUDED

65
ur_rtde_driver/include/ur_rtde_driver/primary/primary_package.h Normal file
View File

@@ -0,0 +1,65 @@
// 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_rtde_driver/primary/package_header.h"
#include "ur_rtde_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;
virtual bool parseWith(comm::BinParser& bp);
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 */

171
ur_rtde_driver/include/ur_rtde_driver/primary/primary_parser.h Normal file
View File

@@ -0,0 +1,171 @@
/*
* 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_rtde_driver/comm/bin_parser.h"
#include "ur_rtde_driver/comm/pipeline.h"
#include "ur_rtde_driver/comm/parser.h"
#include "ur_rtde_driver/primary/package_header.h"
#include "ur_rtde_driver/primary/robot_state.h"
#include "ur_rtde_driver/primary/robot_message.h"
//#include "ur_rtde_driver/primary/robot_state/robot_mode_data.h"
#include "ur_rtde_driver/primary/robot_state/kinematics_info.h"
//#include "ur_rtde_driver/primary/robot_state/master_board.h"
#include "ur_rtde_driver/primary/robot_message/version_message.h"
namespace ur_driver
{
namespace primary_interface
{
class PrimaryParser : public comm::Parser<PackageHeader>
{
public:
PrimaryParser() = default;
virtual ~PrimaryParser() = default;
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(state_from_type(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(message_from_type(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_WARN("Invalid robot package type recieved: %u", static_cast<uint8_t>(type));
bp.consume();
return true;
}
}
return true;
}
private:
RobotState* state_from_type(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* message_from_type(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

69
ur_rtde_driver/include/ur_rtde_driver/primary/robot_message.h Normal file
View File

@@ -0,0 +1,69 @@
// 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_rtde_driver/primary/primary_package.h"
namespace ur_driver
{
namespace primary_interface
{
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
};
class RobotMessage : public PrimaryPackage
{
public:
RobotMessage(const uint64_t timestamp, const uint8_t source) : timestamp_(timestamp), source_(source)
{
}
virtual ~RobotMessage() = default;
virtual bool parseWith(comm::BinParser& bp);
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 */

61
ur_rtde_driver/include/ur_rtde_driver/primary/robot_message/version_message.h 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
//
// 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_rtde_driver/primary/robot_message.h"
namespace ur_driver
{
namespace primary_interface
{
class VersionMessage : public RobotMessage
{
public:
VersionMessage() = delete;
VersionMessage(uint64_t timestamp, uint8_t source) : RobotMessage(timestamp, source)
{
}
virtual ~VersionMessage() = default;
virtual bool parseWith(comm::BinParser& bp);
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_; // TODO Exists in version 3.3 above only
std::string build_date_;
};
} // namespace primary_interface
} // namespace ur_driver
#endif // ifndef UR_RTDE_DRIVER_VERSION_MESSAGE_H_INCLUDED

86
ur_rtde_driver/include/ur_rtde_driver/primary/robot_state.h Normal file
View File

@@ -0,0 +1,86 @@
// 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 <mutex>
#include <condition_variable>
#include "ur_rtde_driver/primary/primary_package.h"
#include "ur_rtde_driver/primary/package_header.h"
namespace ur_driver
{
namespace primary_interface
{
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;
RobotState(const RobotStateType type) : state_type_(type)
{
}
virtual ~RobotState() = default;
virtual bool parseWith(comm::BinParser& bp)
{
return PrimaryPackage::parseWith(bp);
}
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 */

53
ur_rtde_driver/include/ur_rtde_driver/primary/robot_state/kinematics_info.h Normal file
View File

@@ -0,0 +1,53 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// -- 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_rtde_driver/types.h"
#include "ur_rtde_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;
KinematicsInfo(const RobotStateType type) : RobotState(type)
{
}
virtual ~KinematicsInfo() = default;
virtual bool parseWith(comm::BinParser& bp);
virtual std::string toString() 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.
};
// TODO: Handle pre-3.6 as they don't have kinematics info
} // namespace primary_interface
} // namespace ur_driver
#endif // ifndef UR_RTDE_DRIVER_KINEMATICS_INFO_H_INCLUDED

115
ur_rtde_driver/include/ur_rtde_driver/primary/robot_state/master_board.h Normal file
View File

@@ -0,0 +1,115 @@
/*
* 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_rtde_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_rtde_driver/include/ur_rtde_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_rtde_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_rtde_driver/include/ur_rtde_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_rtde_driver/include/ur_rtde_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 compiler_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();
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 compiler_fence(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 weak_atomic
{
public:
weak_atomic()
{
}
#ifdef AE_VCPP
#pragma warning(disable : 4100) // Get rid of (erroneous) 'unreferenced formal parameter' warning
#endif
template <typename U>
weak_atomic(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
weak_atomic(nullptr_t) : value(nullptr)
{
}
#endif
weak_atomic(weak_atomic const& other) : value(other.value)
{
}
weak_atomic(weak_atomic&& 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 weak_atomic const& operator=(U&& x)
{
value = std::forward<U>(x);
return *this;
}
AE_FORCEINLINE weak_atomic const& operator=(weak_atomic const& other)
{
value = other.value;
return *this;
}
AE_FORCEINLINE T load() const
{
return value;
}
AE_FORCEINLINE T fetch_add_acquire(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 fetch_add_release(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 weak_atomic const& operator=(U&& x)
{
value.store(std::forward<U>(x), std::memory_order_relaxed);
return *this;
}
AE_FORCEINLINE weak_atomic const& operator=(weak_atomic 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 fetch_add_acquire(T increment)
{
return value.fetch_add(increment, std::memory_order_acquire);
}
AE_FORCEINLINE T fetch_add_release(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 try_wait()
{
const unsigned long RC_WAIT_TIMEOUT = 0x00000102;
return WaitForSingleObject(m_hSema, 0) != RC_WAIT_TIMEOUT;
}
bool timed_wait(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 m_sema;
Semaphore(const Semaphore& other);
Semaphore& operator=(const Semaphore& other);
public:
Semaphore(int initialCount = 0)
{
assert(initialCount >= 0);
semaphore_create(mach_task_self(), &m_sema, SYNC_POLICY_FIFO, initialCount);
}
~Semaphore()
{
semaphore_destroy(mach_task_self(), m_sema);
}
void wait()
{
semaphore_wait(m_sema);
}
bool try_wait()
{
return timed_wait(0);
}
bool timed_wait(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(m_sema, ts);
return rc != KERN_OPERATION_TIMED_OUT;
}
void signal()
{
semaphore_signal(m_sema);
}
void signal(int count)
{
while (count-- > 0)
{
semaphore_signal(m_sema);
}
}
};
#elif defined(__unix__)
//---------------------------------------------------------
// Semaphore (POSIX, Linux)
//---------------------------------------------------------
class Semaphore
{
private:
sem_t m_sema;
Semaphore(const Semaphore& other);
Semaphore& operator=(const Semaphore& other);
public:
Semaphore(int initialCount = 0)
{
assert(initialCount >= 0);
sem_init(&m_sema, 0, initialCount);
}
~Semaphore()
{
sem_destroy(&m_sema);
}
void wait()
{
// http://stackoverflow.com/questions/2013181/gdb-causes-sem-wait-to-fail-with-eintr-error
int rc;
do
{
rc = sem_wait(&m_sema);
} while (rc == -1 && errno == EINTR);
}
bool try_wait()
{
int rc;
do
{
rc = sem_trywait(&m_sema);
} while (rc == -1 && errno == EINTR);
return !(rc == -1 && errno == EAGAIN);
}
bool timed_wait(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(&m_sema, &ts);
} while (rc == -1 && errno == EINTR);
return !(rc == -1 && errno == ETIMEDOUT);
}
void signal()
{
sem_post(&m_sema);
}
void signal(int count)
{
while (count-- > 0)
{
sem_post(&m_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:
weak_atomic<ssize_t> m_count;
Semaphore m_sema;
bool waitWithPartialSpinning(std::int64_t timeout_usecs = -1)
{
ssize_t oldCount;
// 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 (m_count.load() > 0)
{
m_count.fetch_add_acquire(-1);
return true;
}
compiler_fence(memory_order_acquire); // Prevent the compiler from collapsing the loop.
}
oldCount = m_count.fetch_add_acquire(-1);
if (oldCount > 0)
return true;
if (timeout_usecs < 0)
{
m_sema.wait();
return true;
}
if (m_sema.timed_wait(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)
{
oldCount = m_count.fetch_add_release(1);
if (oldCount < 0)
return false; // successfully restored things to the way they were
// Oh, the producer thread just signaled the semaphore after all. Try again:
oldCount = m_count.fetch_add_acquire(-1);
if (oldCount > 0 && m_sema.try_wait())
return true;
}
}
public:
LightweightSemaphore(ssize_t initialCount = 0) : m_count(initialCount)
{
assert(initialCount >= 0);
}
bool tryWait()
{
if (m_count.load() > 0)
{
m_count.fetch_add_acquire(-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 oldCount = m_count.fetch_add_release(count);
assert(oldCount >= -1);
if (oldCount < 0)
{
m_sema.signal(1);
}
}
ssize_t availableApprox() const
{
ssize_t count = m_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

864
ur_rtde_driver/include/ur_rtde_driver/queue/readerwriterqueue.h Normal file
View File

@@ -0,0 +1,864 @@
// ©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 maxSize = 15)
#ifndef NDEBUG
: enqueuing(false), dequeuing(false)
#endif
{
assert(maxSize > 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* firstBlock = nullptr;
largestBlockSize = ceilToPow2(maxSize + 1); // We need a spare slot to fit maxSize elements in the block
if (largestBlockSize > 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 initialBlockCount = (maxSize + MAX_BLOCK_SIZE * 2 - 3) / (MAX_BLOCK_SIZE - 1);
largestBlockSize = MAX_BLOCK_SIZE;
Block* lastBlock = nullptr;
for (size_t i = 0; i != initialBlockCount; ++i)
{
auto block = make_block(largestBlockSize);
if (block == nullptr)
{
#ifdef MOODYCAMEL_EXCEPTIONS_ENABLED
throw std::bad_alloc();
#else
abort();
#endif
}
if (firstBlock == nullptr)
{
firstBlock = block;
}
else
{
lastBlock->next = block;
}
lastBlock = block;
block->next = firstBlock;
}
}
else
{
firstBlock = make_block(largestBlockSize);
if (firstBlock == nullptr)
{
#ifdef MOODYCAMEL_EXCEPTIONS_ENABLED
throw std::bad_alloc();
#else
abort();
#endif
}
firstBlock->next = firstBlock;
}
frontBlock = firstBlock;
tailBlock = firstBlock;
// 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* frontBlock_ = frontBlock;
Block* block = frontBlock_;
do
{
Block* nextBlock = block->next;
size_t blockFront = block->front;
size_t blockTail = block->tail;
for (size_t i = blockFront; i != blockTail; i = (i + 1) & block->sizeMask)
{
auto element = reinterpret_cast<T*>(block->data + i * sizeof(T));
element->~T();
(void)element;
}
auto rawBlock = block->rawThis;
block->~Block();
std::free(rawBlock);
block = nextBlock;
} while (block != frontBlock_);
}
// 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 try_enqueue(T const& element)
{
return inner_enqueue<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 try_enqueue(T&& element)
{
return inner_enqueue<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 inner_enqueue<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 inner_enqueue<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 try_dequeue(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* frontBlock_ = frontBlock.load();
size_t blockTail = frontBlock_->localTail;
size_t blockFront = frontBlock_->front.load();
if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load()))
{
fence(memory_order_acquire);
non_empty_front_block:
// Front block not empty, dequeue from here
auto element = reinterpret_cast<T*>(frontBlock_->data + blockFront * sizeof(T));
result = std::move(*element);
element->~T();
blockFront = (blockFront + 1) & frontBlock_->sizeMask;
fence(memory_order_release);
frontBlock_->front = blockFront;
}
else if (frontBlock_ != tailBlock.load())
{
fence(memory_order_acquire);
frontBlock_ = frontBlock.load();
blockTail = frontBlock_->localTail = frontBlock_->tail.load();
blockFront = frontBlock_->front.load();
fence(memory_order_acquire);
if (blockFront != blockTail)
{
// 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* nextBlock = frontBlock_->next;
// Don't need an acquire fence here since next can only ever be set on the tailBlock,
// and we're not the tailBlock, and we did an acquire earlier after reading tailBlock which
// ensures next is up-to-date on this CPU in case we recently were at tailBlock.
size_t nextBlockFront = nextBlock->front.load();
size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load();
fence(memory_order_acquire);
// Since the tailBlock is only ever advanced after being written to,
// we know there's for sure an element to dequeue on it
assert(nextBlockFront != nextBlockTail);
AE_UNUSED(nextBlockTail);
// We're done with this block, let the producer use it if it needs
fence(memory_order_release); // Expose possibly pending changes to frontBlock->front from last dequeue
frontBlock = frontBlock_ = nextBlock;
compiler_fence(memory_order_release); // Not strictly needed
auto element = reinterpret_cast<T*>(frontBlock_->data + nextBlockFront * sizeof(T));
result = std::move(*element);
element->~T();
nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask;
fence(memory_order_release);
frontBlock_->front = nextBlockFront;
}
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 `try_dequeue` 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 try_dequeue() for reasoning
Block* frontBlock_ = frontBlock.load();
size_t blockTail = frontBlock_->localTail;
size_t blockFront = frontBlock_->front.load();
if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load()))
{
fence(memory_order_acquire);
non_empty_front_block:
return reinterpret_cast<T*>(frontBlock_->data + blockFront * sizeof(T));
}
else if (frontBlock_ != tailBlock.load())
{
fence(memory_order_acquire);
frontBlock_ = frontBlock.load();
blockTail = frontBlock_->localTail = frontBlock_->tail.load();
blockFront = frontBlock_->front.load();
fence(memory_order_acquire);
if (blockFront != blockTail)
{
goto non_empty_front_block;
}
Block* nextBlock = frontBlock_->next;
size_t nextBlockFront = nextBlock->front.load();
fence(memory_order_acquire);
assert(nextBlockFront != nextBlock->tail.load());
return reinterpret_cast<T*>(nextBlock->data + nextBlockFront * 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 try_dequeue() for reasoning
Block* frontBlock_ = frontBlock.load();
size_t blockTail = frontBlock_->localTail;
size_t blockFront = frontBlock_->front.load();
if (blockFront != blockTail || blockFront != (frontBlock_->localTail = frontBlock_->tail.load()))
{
fence(memory_order_acquire);
non_empty_front_block:
auto element = reinterpret_cast<T*>(frontBlock_->data + blockFront * sizeof(T));
element->~T();
blockFront = (blockFront + 1) & frontBlock_->sizeMask;
fence(memory_order_release);
frontBlock_->front = blockFront;
}
else if (frontBlock_ != tailBlock.load())
{
fence(memory_order_acquire);
frontBlock_ = frontBlock.load();
blockTail = frontBlock_->localTail = frontBlock_->tail.load();
blockFront = frontBlock_->front.load();
fence(memory_order_acquire);
if (blockFront != blockTail)
{
goto non_empty_front_block;
}
// Front block is empty but there's another block ahead, advance to it
Block* nextBlock = frontBlock_->next;
size_t nextBlockFront = nextBlock->front.load();
size_t nextBlockTail = nextBlock->localTail = nextBlock->tail.load();
fence(memory_order_acquire);
assert(nextBlockFront != nextBlockTail);
AE_UNUSED(nextBlockTail);
fence(memory_order_release);
frontBlock = frontBlock_ = nextBlock;
compiler_fence(memory_order_release);
auto element = reinterpret_cast<T*>(frontBlock_->data + nextBlockFront * sizeof(T));
element->~T();
nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask;
fence(memory_order_release);
frontBlock_->front = nextBlockFront;
}
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 size_approx() const
{
size_t result = 0;
Block* frontBlock_ = frontBlock.load();
Block* block = frontBlock_;
do
{
fence(memory_order_acquire);
size_t blockFront = block->front.load();
size_t blockTail = block->tail.load();
result += (blockTail - blockFront) & block->sizeMask;
block = block->next.load();
} while (block != frontBlock_);
return result;
}
private:
enum AllocationMode
{
CanAlloc,
CannotAlloc
};
template <AllocationMode canAlloc, typename U>
bool inner_enqueue(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* tailBlock_ = tailBlock.load();
size_t blockFront = tailBlock_->localFront;
size_t blockTail = tailBlock_->tail.load();
size_t nextBlockTail = (blockTail + 1) & tailBlock_->sizeMask;
if (nextBlockTail != blockFront || nextBlockTail != (tailBlock_->localFront = tailBlock_->front.load()))
{
fence(memory_order_acquire);
// This block has room for at least one more element
char* location = tailBlock_->data + blockTail * sizeof(T);
new (location) T(std::forward<U>(element));
fence(memory_order_release);
tailBlock_->tail = nextBlockTail;
}
else
{
fence(memory_order_acquire);
if (tailBlock_->next.load() != frontBlock)
{
// Note that the reason we can't advance to the frontBlock 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 frontBlock
// tailBlock is full, but there's a free block ahead, use it
Block* tailBlockNext = tailBlock_->next.load();
size_t nextBlockFront = tailBlockNext->localFront = tailBlockNext->front.load();
nextBlockTail = tailBlockNext->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(nextBlockFront == nextBlockTail);
tailBlockNext->localFront = nextBlockFront;
char* location = tailBlockNext->data + nextBlockTail * sizeof(T);
new (location) T(std::forward<U>(element));
tailBlockNext->tail = (nextBlockTail + 1) & tailBlockNext->sizeMask;
fence(memory_order_release);
tailBlock = tailBlockNext;
}
else if (canAlloc == CanAlloc)
{
// tailBlock is full and there's no free block ahead; create a new block
auto newBlockSize = largestBlockSize >= MAX_BLOCK_SIZE ? largestBlockSize : largestBlockSize * 2;
auto newBlock = make_block(newBlockSize);
if (newBlock == nullptr)
{
// Could not allocate a block!
return false;
}
largestBlockSize = newBlockSize;
new (newBlock->data) T(std::forward<U>(element));
assert(newBlock->front == 0);
newBlock->tail = newBlock->localTail = 1;
newBlock->next = tailBlock_->next.load();
tailBlock_->next = newBlock;
// Might be possible for the dequeue thread to see the new tailBlock->next
// *without* seeing the new tailBlock value, but this is OK since it can't
// advance to the next block until tailBlock is set anyway (because the only
// case where it could try to read the next is if it's already at the tailBlock,
// and it won't advance past tailBlock in any circumstance).
fence(memory_order_release);
tailBlock = newBlock;
}
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* align_for(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& _inSection) : inSection(_inSection)
{
assert(!inSection && "ReaderWriterQueue does not support enqueuing or dequeuing elements from other elements' "
"ctors and dtors");
inSection = true;
}
~ReentrantGuard()
{
inSection = false;
}
private:
ReentrantGuard& operator=(ReentrantGuard const&);
private:
bool& inSection;
};
#endif
struct Block
{
// Avoid false-sharing by putting highly contended variables on their own cache lines
weak_atomic<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(weak_atomic<size_t>) - sizeof(size_t)];
weak_atomic<size_t> tail; // (Atomic) Elements are enqueued here
size_t localFront;
char cachelineFiller1[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic<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)
weak_atomic<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* make_block(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 newBlockRaw = static_cast<char*>(std::malloc(size));
if (newBlockRaw == nullptr)
{
return nullptr;
}
auto newBlockAligned = align_for<Block>(newBlockRaw);
auto newBlockData = align_for<T>(newBlockAligned + sizeof(Block));
return new (newBlockAligned) Block(capacity, newBlockRaw, newBlockData);
}
private:
weak_atomic<Block*> frontBlock; // (Atomic) Elements are enqueued to this block
char cachelineFiller[MOODYCAMEL_CACHE_LINE_SIZE - sizeof(weak_atomic<Block*>)];
weak_atomic<Block*> tailBlock; // (Atomic) Elements are dequeued from this block
size_t largestBlockSize;
#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 try_enqueue(T const& element)
{
if (inner.try_enqueue(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 try_enqueue(T&& element)
{
if (inner.try_enqueue(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 try_dequeue(U& result)
{
if (sema.tryWait())
{
bool success = inner.try_dequeue(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 wait_dequeue(U& result)
{
sema.wait();
bool success = inner.try_dequeue(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 wait_dequeue.
template <typename U>
bool wait_dequeue_timed(U& result, std::int64_t timeout_usecs)
{
if (!sema.wait(timeout_usecs))
{
return false;
}
bool success = inner.try_dequeue(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 wait_dequeue.
template <typename U, typename Rep, typename Period>
inline bool wait_dequeue_timed(U& result, std::chrono::duration<Rep, Period> const& timeout)
{
return wait_dequeue_timed(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 `try_dequeue` 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 size_approx() 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

78
ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h Normal file
View File

@@ -0,0 +1,78 @@
// 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 <ur_controllers/speed_scaling_interface.h>
#include "ur_rtde_driver/ur/ur_driver.h"
namespace ur_driver
{
class HardwareInterface : public hardware_interface::RobotHW
{
public:
HardwareInterface();
virtual ~HardwareInterface() = default;
virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) override;
virtual void read(const ros::Time& time, const ros::Duration& period) override;
virtual void write(const ros::Time& time, const ros::Duration& period) override;
virtual bool prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list) override;
virtual void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list) override;
uint32_t getControlFrequency() const;
protected:
std::unique_ptr<UrDriver> ur_driver_;
hardware_interface::JointStateInterface js_interface_;
hardware_interface::PositionJointInterface pj_interface_;
ur_controllers::SpeedScalingInterface speedsc_interface_;
// hardware_interface::VelocityJointInterface vj_interface_;
vector6d_t joint_position_command_;
// std::vector<double> joint_velocity_command_;
vector6d_t joint_positions_;
vector6d_t joint_velocities_;
vector6d_t joint_efforts_;
double speed_scaling_value_;
std::vector<std::string> joint_names_;
bool position_controller_running_;
};
} // namespace ur_driver
#endif // ifndef UR_DRIVER_HARDWARE_INTERFACE_H_INCLUDED

82
ur_rtde_driver/include/ur_rtde_driver/ros/io_service.h Normal file
View File

@@ -0,0 +1,82 @@
/*
* 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 <ros/ros.h>
#include <ur_msgs/SetIO.h>
#include <ur_msgs/SetIORequest.h>
#include <ur_msgs/SetIOResponse.h>
#include <ur_msgs/SetPayload.h>
#include <ur_msgs/SetPayloadRequest.h>
#include <ur_msgs/SetPayloadResponse.h>
#include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/ur/commander.h"
namespace ur_driver
{
class IOService
{
private:
ros::NodeHandle nh_;
URCommander& commander_;
ros::ServiceServer io_service_;
ros::ServiceServer payload_service_;
bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp)
{
LOG_INFO("setIO called with [%d, %d]", req.fun, req.pin);
bool res = false;
bool flag = req.state > 0.0 ? true : false;
switch (req.fun)
{
case ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT:
res = commander_.setDigitalOut(req.pin, flag);
break;
case ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT:
res = commander_.setAnalogOut(req.pin, req.state);
break;
case ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE:
res = commander_.setToolVoltage(static_cast<uint8_t>(req.state));
break;
case ur_msgs::SetIO::Request::FUN_SET_FLAG:
res = commander_.setFlag(req.pin, flag);
break;
default:
LOG_WARN("Invalid setIO function called (%d)", req.fun);
}
return (resp.success = res);
}
bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& resp)
{
LOG_INFO("setPayload called");
// TODO check min and max payload?
return (resp.success = commander_.setPayload(req.payload));
}
public:
IOService(URCommander& commander)
: commander_(commander)
, io_service_(nh_.advertiseService("ur_driver/set_io", &IOService::setIO, this))
, payload_service_(nh_.advertiseService("ur_driver/set_payload", &IOService::setPayload, this))
{
}
};
} // namespace ur_driver

91
ur_rtde_driver/include/ur_rtde_driver/ros/service_stopper.h Normal file
View File

@@ -0,0 +1,91 @@
/*
* 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 <ros/ros.h>
#include <std_srvs/Empty.h>
#include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/ur/consumer.h"
namespace ur_driver
{
enum class RobotState
{
Running,
Error,
EmergencyStopped,
ProtectiveStopped
};
enum class ActivationMode
{
Never,
Always,
OnStartup
};
class Service
{
public:
virtual void onRobotStateChange(RobotState state) = 0;
};
class ServiceStopper : public URStatePacketConsumer
{
private:
ros::NodeHandle nh_;
ros::ServiceServer enable_service_;
std::vector<Service*> services_;
RobotState last_state_;
ActivationMode activation_mode_;
void notify_all(RobotState state);
bool handle(SharedRobotModeData& data, bool error);
bool enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp);
public:
ServiceStopper(std::vector<Service*> services);
virtual bool consume(RobotModeData_V1_X& data)
{
return handle(data, data.robot_mode != robot_mode_V1_X::ROBOT_RUNNING_MODE);
}
virtual bool consume(RobotModeData_V3_0__1& data)
{
return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING);
}
virtual bool consume(RobotModeData_V3_2& data)
{
return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING);
}
// unused
virtual bool consume(MasterBoardData_V1_X& data)
{
return true;
}
virtual bool consume(MasterBoardData_V3_0__1& data)
{
return true;
}
virtual bool consume(MasterBoardData_V3_2& data)
{
return true;
}
};
} // namespace ur_driver

59
ur_rtde_driver/include/ur_rtde_driver/rtde/control_package_pause.h Normal file
View File

@@ -0,0 +1,59 @@
// 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_rtde_driver/rtde/rtde_package.h"
namespace ur_driver
{
namespace rtde_interface
{
class ControlPackagePause : public RTDEPackage
{
public:
ControlPackagePause() = default;
virtual ~ControlPackagePause() = default;
virtual bool parseWith(comm::BinParser& bp);
virtual std::string toString() const;
uint8_t accepted_;
};
class ControlPackagePauseRequest : public RTDEPackage
{
public:
ControlPackagePauseRequest() = default;
virtual ~ControlPackagePauseRequest() = default;
};
} // namespace rtde_interface
} // namespace ur_driver
#endif // UR_RTDE_DRIVER_CONTROL_PACKAGE_PAUSE_H_INCLUDED

62
ur_rtde_driver/include/ur_rtde_driver/rtde/control_package_setup_inputs.h Normal file
View File

@@ -0,0 +1,62 @@
// 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_rtde_driver/rtde/rtde_package.h"
namespace ur_driver
{
namespace rtde_interface
{
class ControlPackageSetupInputs : public RTDEPackage
{
public:
ControlPackageSetupInputs() = default;
virtual ~ControlPackageSetupInputs() = default;
virtual bool parseWith(comm::BinParser& bp);
virtual std::string toString() const;
uint8_t input_recipe_id_;
std::string variable_types_;
};
class ControlPackageSetupInputsRequest : public RTDEPackage
{
public:
ControlPackageSetupInputsRequest() = default;
virtual ~ControlPackageSetupInputsRequest() = default;
std::string variable_names_;
};
} // namespace rtde_interface
} // namespace ur_driver
#endif // UR_RTDE_DRIVER_CONTROL_PACKAGE_SETUP_INPUTS_H_INCLUDED

72
ur_rtde_driver/include/ur_rtde_driver/rtde/control_package_setup_outputs.h Normal file
View File

@@ -0,0 +1,72 @@
// 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_rtde_driver/rtde/rtde_package.h"
#include "ur_rtde_driver/rtde/package_header.h"
namespace ur_driver
{
namespace rtde_interface
{
class ControlPackageSetupOutputs : public RTDEPackage
{
public:
ControlPackageSetupOutputs() = default;
virtual ~ControlPackageSetupOutputs() = default;
virtual bool parseWith(comm::BinParser& bp);
virtual std::string toString() const;
uint8_t output_recipe_id_;
std::string variable_types_;
};
class ControlPackageSetupOutputsRequest : public RTDEPackage
{
public:
ControlPackageSetupOutputsRequest() = default;
virtual ~ControlPackageSetupOutputsRequest() = default;
static size_t generateSerializedRequest(uint8_t* buffer, double output_frequency,
std::vector<std::string> variable_names);
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

65
ur_rtde_driver/include/ur_rtde_driver/rtde/control_package_start.h Normal file
View File

@@ -0,0 +1,65 @@
// 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_rtde_driver/rtde/rtde_package.h"
namespace ur_driver
{
namespace rtde_interface
{
class ControlPackageStart : public RTDEPackage
{
public:
ControlPackageStart() = default;
virtual ~ControlPackageStart() = default;
virtual bool parseWith(comm::BinParser& bp);
virtual std::string toString() const;
uint8_t accepted_;
};
class ControlPackageStartRequest : public RTDEPackage
{
public:
ControlPackageStartRequest() = default;
virtual ~ControlPackageStartRequest() = default;
static size_t generateSerializedRequest(uint8_t* buffer);
private:
static const uint16_t PAYLOAD_SIZE = 0;
static const PackageType PACKAGE_TYPE = PackageType::RTDE_CONTROL_PACKAGE_START;
};
} // namespace rtde_interface
} // namespace ur_driver
#endif // UR_RTDE_DRIVER_CONTROL_PACKAGE_START_H_INCLUDED

111
ur_rtde_driver/include/ur_rtde_driver/rtde/data_package.h Normal file
View File

@@ -0,0 +1,111 @@
// 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_rtde_driver/types.h"
#include "ur_rtde_driver/rtde/rtde_package.h"
#include <boost/variant.hpp>
namespace ur_driver
{
namespace rtde_interface
{
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();
}
};
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;
DataPackage(const std::vector<std::string>& recipe) : recipe_(recipe)
{
}
virtual ~DataPackage() = default;
virtual bool parseWith(comm::BinParser& bp);
virtual std::string toString() const;
template <typename T>
/*!
* \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.
*
* \returns True on success, false if the field cannot be found inside the package.
*/
bool getData(const std::string& name, T& val)
{
if (data_.find(name) != data_.end())
{
// TODO: Can we check this somehow?
val = boost::strict_get<T>(data_[name]);
}
else
{
return false;
}
return true;
}
private:
// Const would be better here
static std::unordered_map<std::string, _rtde_type_variant> type_list_;
uint8_t recipe_id_;
std::unordered_map<std::string, _rtde_type_variant> data_;
std::vector<std::string> recipe_;
};
} // namespace rtde_interface
} // namespace ur_driver
#endif // ifndef UR_RTDE_DRIVER_DATA_PACKAGE_H_INCLUDED

68
ur_rtde_driver/include/ur_rtde_driver/rtde/get_urcontrol_version.h Normal file
View File

@@ -0,0 +1,68 @@
// 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_rtde_driver/rtde/rtde_package.h"
namespace ur_driver
{
namespace rtde_interface
{
class GetUrcontrolVersion : public RTDEPackage
{
public:
GetUrcontrolVersion() = default;
virtual ~GetUrcontrolVersion() = default;
virtual bool parseWith(comm::BinParser& bp);
virtual std::string toString() const;
uint32_t major_;
uint32_t minor_;
uint32_t bugfix_;
uint32_t build_;
};
class GetUrcontrolVersionRequest : public RTDEPackage
{
public:
GetUrcontrolVersionRequest() = default;
virtual ~GetUrcontrolVersionRequest() = default;
static size_t generateSerializedRequest(uint8_t* buffer);
private:
static const uint16_t PAYLOAD_SIZE = 0;
static const PackageType PACKAGE_TYPE = PackageType::RTDE_GET_URCONTROL_VERSION;
};
} // namespace rtde_interface
} // namespace ur_driver
#endif // UR_RTDE_DRIVER_GET_URCONTROL_VERSION_H_INCLUDED

72
ur_rtde_driver/include/ur_rtde_driver/rtde/package_header.h Normal file
View File

@@ -0,0 +1,72 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// -- 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_rtde_driver/types.h"
#include "ur_rtde_driver/comm/package_serializer.h"
namespace ur_driver
{
namespace rtde_interface
{
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
};
class PackageHeader
{
public:
PackageHeader() = default;
virtual ~PackageHeader() = default;
using _package_size_type = uint16_t;
PackageHeader(PackageType& type) : package_type_(type){};
PackageHeader(_package_size_type& size, PackageType& type) : package_size_(size), package_type_(type){};
static size_t getPackageLength(uint8_t* buf)
{
return be16toh(*(reinterpret_cast<_package_size_type*>(buf)));
}
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;
}
private:
_package_size_type package_size_;
PackageType package_type_;
};
} // namespace rtde_interface
} // namespace ur_driver
#endif // #ifndef UR_RTDE_DRIVER_RTDE__HEADER_H_INCLUDED

68
ur_rtde_driver/include/ur_rtde_driver/rtde/request_protocol_version.h Normal file
View File

@@ -0,0 +1,68 @@
// 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_rtde_driver/rtde/rtde_package.h"
#include "ur_rtde_driver/rtde/package_header.h"
namespace ur_driver
{
namespace rtde_interface
{
class RequestProtocolVersion : public RTDEPackage
{
public:
RequestProtocolVersion() = default;
virtual ~RequestProtocolVersion() = default;
virtual bool parseWith(comm::BinParser& bp);
virtual std::string toString() const;
uint8_t accepted_;
};
class RequestProtocolVersionRequest : public RTDEPackage
{
public:
RequestProtocolVersionRequest() = default;
virtual ~RequestProtocolVersionRequest() = default;
static size_t generateSerializedRequest(uint8_t* buffer, uint16_t version);
uint16_t protocol_version_;
private:
static const uint16_t PAYLOAD_SIZE = sizeof(uint16_t);
static const PackageType PACKAGE_TYPE = PackageType::RTDE_REQUEST_PROTOCOL_VERSION;
};
} // namespace rtde_interface
} // namespace ur_driver
#endif // UR_RTDE_DRIVER_REQUEST_PROTOCOL_VERSION_H_INCLUDED

75
ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h Normal file
View File

@@ -0,0 +1,75 @@
// 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_rtde_driver/comm/pipeline.h"
#include "ur_rtde_driver/rtde/package_header.h"
#include "ur_rtde_driver/rtde/rtde_package.h"
#include "ur_rtde_driver/comm/stream.h"
#include "ur_rtde_driver/rtde/rtde_parser.h"
#include "ur_rtde_driver/comm/producer.h"
#include "ur_rtde_driver/rtde/data_package.h"
#include "ur_rtde_driver/rtde/request_protocol_version.h"
#include "ur_rtde_driver/rtde/control_package_setup_outputs.h"
#include "ur_rtde_driver/rtde/control_package_start.h"
#include "ur_rtde_driver/log.h"
static const int UR_RTDE_PORT = 30004;
static const std::string PIPELINE_NAME = "RTDE Data Pipeline";
namespace ur_driver
{
namespace rtde_interface
{
class RTDEClient
{
public:
RTDEClient() = delete;
RTDEClient(std::string ROBOT_IP, comm::INotifier& notifier);
~RTDEClient() = default;
bool init();
bool start();
bool getDataPackage(std::unique_ptr<comm::URPackage<PackageHeader>>& data_package, std::chrono::milliseconds timeout);
private:
comm::URStream<PackageHeader> stream_;
RTDEParser parser_;
comm::URProducer<PackageHeader> prod_;
comm::Pipeline<PackageHeader> pipeline_;
constexpr static const double CB3_MAX_FREQUENCY = 125.0;
constexpr static const double URE_MAX_FREQUENCY = 500.0;
std::vector<std::string> readRecipe();
};
} // namespace rtde_interface
} // namespace ur_driver
#endif // UR_RTDE_DRIVER_RTDE_CLIENT_H_INCLUDED

66
ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_package.h Normal file
View File

@@ -0,0 +1,66 @@
// 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_rtde_driver/rtde/package_header.h"
#include "ur_rtde_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() = default;
RTDEPackage(const PackageType type) : type_(type)
{
}
virtual ~RTDEPackage() = default;
virtual bool parseWith(comm::BinParser& bp);
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

133
ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_parser.h Normal file
View File

@@ -0,0 +1,133 @@
/*
* 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_rtde_driver/comm/parser.h"
#include "ur_rtde_driver/comm/bin_parser.h"
#include "ur_rtde_driver/comm/pipeline.h"
#include "ur_rtde_driver/rtde/control_package_pause.h"
#include "ur_rtde_driver/rtde/control_package_setup_inputs.h"
#include "ur_rtde_driver/rtde/control_package_setup_outputs.h"
#include "ur_rtde_driver/rtde/control_package_start.h"
#include "ur_rtde_driver/rtde/data_package.h"
#include "ur_rtde_driver/rtde/get_urcontrol_version.h"
#include "ur_rtde_driver/rtde/package_header.h"
#include "ur_rtde_driver/rtde/request_protocol_version.h"
#include "ur_rtde_driver/rtde/text_message.h"
namespace ur_driver
{
namespace rtde_interface
{
class RTDEParser : public comm::Parser<PackageHeader>
{
public:
RTDEParser() = delete;
RTDEParser(const std::vector<std::string>& recipe) : recipe_(recipe)
{
}
virtual ~RTDEParser() = default;
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(package_from_type(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;
}
private:
std::vector<std::string> recipe_;
RTDEPackage* package_from_type(PackageType type)
{
switch (type)
{
case PackageType::RTDE_TEXT_MESSAGE:
return new TextMessage;
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);
}
}
};
} // namespace rtde_interface
} // namespace ur_driver

56
ur_rtde_driver/include/ur_rtde_driver/rtde/text_message.h Normal file
View File

@@ -0,0 +1,56 @@
// 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_rtde_driver/rtde/rtde_package.h"
namespace ur_driver
{
namespace rtde_interface
{
class TextMessage : public RTDEPackage
{
public:
TextMessage() = default;
virtual ~TextMessage() = default;
virtual bool parseWith(comm::BinParser& bp);
virtual std::string toString() const;
uint8_t message_length_;
std::string message_;
uint8_t source_length_;
std::string source_;
uint8_t warning_level_;
};
} // namespace rtde_interface
} // namespace ur_driver
#endif // UR_RTDE_DRIVER_TEXT_MESSAGE_H_INCLUDED

83
ur_rtde_driver/include/ur_rtde_driver/test/random_data.h Normal file
View File

@@ -0,0 +1,83 @@
/*
* 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 <algorithm>
#include <bitset>
#include <climits>
#include <cstddef>
#include <functional>
#include <random>
#include "ur_rtde_driver/bin_parser.h"
namespace ur_driver
{
class RandomDataTest
{
private:
using random_bytes_engine = std::independent_bits_engine<std::default_random_engine, CHAR_BIT, uint8_t>;
uint8_t* buf_;
BinParser bp_;
size_t n_;
public:
RandomDataTest(size_t n) : buf_(new uint8_t[n]), bp_(buf_, n), n_(n)
{
random_bytes_engine rbe;
std::generate(buf_, buf_ + n, std::ref(rbe));
}
~RandomDataTest()
{
delete buf_;
}
BinParser getParser(bool skip = false)
{
return BinParser(buf_, n_ - (skip ? sizeof(int32_t) : 0));
}
template <typename T>
T getNext()
{
T actual;
bp_.parse(actual);
return actual;
}
template <typename T, size_t N>
std::bitset<N> getNext()
{
T actual;
bp_.parse(actual);
return std::bitset<N>(actual);
}
template <typename T>
void set(T data, size_t pos)
{
std::memcpy(&data, buf_ + pos, sizeof(T));
}
void skip(size_t n)
{
bp_.consume(n);
}
};
} // namespace ur_driver

25
ur_rtde_driver/include/ur_rtde_driver/test/utils.h Normal file
View File

@@ -0,0 +1,25 @@
/*
* 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
#define ASSERT_DOUBLE_ARRAY_EQ(fn, name) \
for (auto const& v : name) \
{ \
ASSERT_EQ(fn, v) << #name " failed parsing"; \
}

69
ur_rtde_driver/include/ur_rtde_driver/types.h Normal file
View File

@@ -0,0 +1,69 @@
/*
* 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
{
// TODO:Replace
struct double3_t
{
double x, y, z;
};
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>;
struct cartesian_coord_t
{
double3_t position;
double3_t rotation;
};
inline bool operator==(const double3_t& lhs, const double3_t& rhs)
{
return lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z;
}
inline bool operator==(const cartesian_coord_t& lhs, const cartesian_coord_t& rhs)
{
return lhs.position == rhs.position && lhs.rotation == rhs.rotation;
}
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

99
ur_rtde_driver/include/ur_rtde_driver/ur/commander.h Normal file
View File

@@ -0,0 +1,99 @@
/*
* 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 <array>
#include <iomanip>
#include <sstream>
#include "ur_rtde_driver/comm/stream.h"
#include "ur_rtde_driver/primary/package_header.h"
namespace ur_driver
{
class URCommander
{
private:
comm::URStream<primary_interface::PackageHeader>& stream_;
protected:
bool write(const std::string& s);
void formatArray(std::ostringstream& out, std::array<double, 6>& values);
public:
URCommander(comm::URStream<primary_interface::PackageHeader>& stream) : stream_(stream)
{
}
virtual bool speedj(std::array<double, 6>& speeds, double acceleration) = 0;
virtual bool setDigitalOut(uint8_t pin, bool value) = 0;
virtual bool setAnalogOut(uint8_t pin, double value) = 0;
// shared
bool uploadProg(const std::string& s);
bool stopj(double a = 10.0);
bool setToolVoltage(uint8_t voltage);
bool setFlag(uint8_t pin, bool value);
bool setPayload(double value);
};
class URCommander_V1_X : public URCommander
{
public:
URCommander_V1_X(comm::URStream<primary_interface::PackageHeader>& stream) : URCommander(stream)
{
}
virtual bool speedj(std::array<double, 6>& speeds, double acceleration);
virtual bool setDigitalOut(uint8_t pin, bool value);
virtual bool setAnalogOut(uint8_t pin, double value);
};
class URCommander_V3_X : public URCommander
{
public:
URCommander_V3_X(comm::URStream<primary_interface::PackageHeader>& stream) : URCommander(stream)
{
}
virtual bool speedj(std::array<double, 6>& speeds, double acceleration) = 0;
virtual bool setDigitalOut(uint8_t pin, bool value);
virtual bool setAnalogOut(uint8_t pin, double value);
};
class URCommander_V3_1__2 : public URCommander_V3_X
{
public:
URCommander_V3_1__2(comm::URStream<primary_interface::PackageHeader>& stream) : URCommander_V3_X(stream)
{
}
virtual bool speedj(std::array<double, 6>& speeds, double acceleration);
};
class URCommander_V3_3 : public URCommander_V3_X
{
public:
URCommander_V3_3(comm::URStream<primary_interface::PackageHeader>& stream) : URCommander_V3_X(stream)
{
}
virtual bool speedj(std::array<double, 6>& speeds, double acceleration);
};
} // namespace ur_driver

71
ur_rtde_driver/include/ur_rtde_driver/ur/consumer.h Normal file
View File

@@ -0,0 +1,71 @@
/*
* 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_rtde_driver/comm/pipeline.h"
#include "ur_rtde_driver/ur/master_board.h"
#include "ur_rtde_driver/ur/messages.h"
#include "ur_rtde_driver/ur/robot_mode.h"
#include "ur_rtde_driver/ur/rt_state.h"
#include "ur_rtde_driver/ur/state.h"
namespace ur_driver
{
class URRTPacketConsumer : public comm::IConsumer<RTPacket>
{
public:
virtual bool consume(std::shared_ptr<RTPacket> packet)
{
return packet->consumeWith(*this);
}
virtual bool consume(RTState_V1_6__7& state) = 0;
virtual bool consume(RTState_V1_8& state) = 0;
virtual bool consume(RTState_V3_0__1& state) = 0;
virtual bool consume(RTState_V3_2__3& state) = 0;
};
class URStatePacketConsumer : public comm::IConsumer<StatePacket>
{
public:
virtual bool consume(std::shared_ptr<StatePacket> packet)
{
return packet->consumeWith(*this);
}
virtual bool consume(MasterBoardData_V1_X& data) = 0;
virtual bool consume(MasterBoardData_V3_0__1& data) = 0;
virtual bool consume(MasterBoardData_V3_2& data) = 0;
virtual bool consume(RobotModeData_V1_X& data) = 0;
virtual bool consume(RobotModeData_V3_0__1& data) = 0;
virtual bool consume(RobotModeData_V3_2& data) = 0;
};
class URMessagePacketConsumer : public comm::IConsumer<MessagePacket>
{
public:
virtual bool consume(std::shared_ptr<MessagePacket> packet)
{
return packet->consumeWith(*this);
}
virtual bool consume(VersionMessage& message) = 0;
};
} // namespace ur_driver

144
ur_rtde_driver/include/ur_rtde_driver/ur/factory.h Normal file
View File

@@ -0,0 +1,144 @@
/*
* 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 <cstdlib>
#include "ur_rtde_driver/comm/parser.h"
#include "ur_rtde_driver/comm/stream.h"
#include "ur_rtde_driver/ur/consumer.h"
#include "ur_rtde_driver/ur/messages_parser.h"
#include "ur_rtde_driver/ur/producer.h"
#include "ur_rtde_driver/ur/rt_parser.h"
#include "ur_rtde_driver/ur/state_parser.h"
namespace ur_driver
{
static const int UR_PRIMARY_PORT = 30001;
class URFactory : private URMessagePacketConsumer
{
private:
comm::URStream stream_;
URMessageParser parser_;
uint8_t major_version_;
uint8_t minor_version_;
bool consume(VersionMessage& vm)
{
LOG_INFO("Got VersionMessage:");
LOG_INFO("project name: %s", vm.project_name.c_str());
LOG_INFO("version: %u.%u.%d", vm.major_version, vm.minor_version, vm.svn_version);
LOG_INFO("build date: %s", vm.build_date.c_str());
major_version_ = vm.major_version;
minor_version_ = vm.minor_version;
return true;
}
void setupConsumer()
{
}
void teardownConsumer()
{
}
void stopConsumer()
{
}
public:
URFactory(std::string& host) : stream_(host, UR_PRIMARY_PORT)
{
URProducer<MessagePacket> prod(stream_, parser_);
std::vector<std::unique_ptr<MessagePacket>> results;
prod.setupProducer();
if (!prod.tryGet(results) || results.size() == 0)
{
LOG_FATAL("No version message received, init failed!");
std::exit(EXIT_FAILURE);
}
for (auto const& p : results)
{
p->consumeWith(*this);
}
if (major_version_ == 0 && minor_version_ == 0)
{
LOG_FATAL("No version message received, init failed!");
std::exit(EXIT_FAILURE);
}
prod.teardownProducer();
}
bool isVersion3()
{
return major_version_ == 3;
}
std::unique_ptr<URCommander> getCommander(comm::URStream& stream)
{
if (major_version_ == 1)
return std::unique_ptr<URCommander>(new URCommander_V1_X(stream));
else if (minor_version_ < 3)
return std::unique_ptr<URCommander>(new URCommander_V3_1__2(stream));
else
return std::unique_ptr<URCommander>(new URCommander_V3_3(stream));
}
std::unique_ptr<comm::URParser<StatePacket>> getStateParser()
{
if (major_version_ == 1)
{
return std::unique_ptr<comm::URParser<StatePacket>>(new URStateParser_V1_X);
}
else
{
if (minor_version_ < 3)
return std::unique_ptr<comm::URParser<StatePacket>>(new URStateParser_V3_0__1);
else if (minor_version_ < 5)
return std::unique_ptr<comm::URParser<StatePacket>>(new URStateParser_V3_2);
else
return std::unique_ptr<comm::URParser<StatePacket>>(new URStateParser_V3_5);
}
}
std::unique_ptr<comm::URParser<RTPacket>> getRTParser()
{
if (major_version_ == 1)
{
if (minor_version_ < 8)
return std::unique_ptr<comm::URParser<RTPacket>>(new URRTStateParser_V1_6__7);
else
return std::unique_ptr<comm::URParser<RTPacket>>(new URRTStateParser_V1_8);
}
else
{
if (minor_version_ < 3)
return std::unique_ptr<comm::URParser<RTPacket>>(new URRTStateParser_V3_0__1);
else
return std::unique_ptr<comm::URParser<RTPacket>>(new URRTStateParser_V3_2__3);
}
}
};
} // namespace ur_driver

72
ur_rtde_driver/include/ur_rtde_driver/ur/messages.h Normal file
View File

@@ -0,0 +1,72 @@
/*
* 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 <cstddef>
#include "ur_rtde_driver/bin_parser.h"
#include "ur_rtde_driver/comm/pipeline.h"
namespace ur_driver
{
enum class robot_message_type : 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
};
class URMessagePacketConsumer;
class MessagePacket
{
public:
MessagePacket(uint64_t timestamp, uint8_t source) : timestamp(timestamp), source(source)
{
}
virtual bool parseWith(BinParser& bp) = 0;
virtual bool consumeWith(URMessagePacketConsumer& consumer) = 0;
uint64_t timestamp;
uint8_t source;
};
class VersionMessage : public MessagePacket
{
public:
VersionMessage(uint64_t timestamp, uint8_t source) : MessagePacket(timestamp, source)
{
}
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URMessagePacketConsumer& consumer);
std::string project_name;
uint8_t major_version;
uint8_t minor_version;
int32_t svn_version;
std::string build_date;
};
} // namespace ur_driver

74
ur_rtde_driver/include/ur_rtde_driver/ur/messages_parser.h Normal file
View File

@@ -0,0 +1,74 @@
/*
* 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_rtde_driver/bin_parser.h"
#include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/comm/parser.h"
#include "ur_rtde_driver/comm/pipeline.h"
#include "ur_rtde_driver/ur/messages.h"
namespace ur_driver
{
class URMessageParser : public comm::URParser<MessagePacket>
{
public:
bool parse(BinParser& bp, std::vector<std::unique_ptr<MessagePacket>>& results)
{
int32_t packet_size;
message_type type;
bp.parse(packet_size);
bp.parse(type);
if (type != message_type::ROBOT_MESSAGE)
{
LOG_WARN("Invalid message type recieved: %u", static_cast<uint8_t>(type));
return false;
}
uint64_t timestamp;
uint8_t source;
robot_message_type message_type;
bp.parse(timestamp);
bp.parse(source);
bp.parse(message_type);
std::unique_ptr<MessagePacket> result;
bool parsed = false;
switch (message_type)
{
case robot_message_type::ROBOT_MESSAGE_VERSION:
{
VersionMessage* vm = new VersionMessage(timestamp, source);
parsed = vm->parseWith(bp);
result.reset(vm);
break;
}
default:
return false;
}
results.push_back(std::move(result));
return parsed;
}
};
} // namespace ur_driver

139
ur_rtde_driver/include/ur_rtde_driver/ur/robot_mode.h Normal file
View File

@@ -0,0 +1,139 @@
/*
* 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 <cstddef>
#include "ur_rtde_driver/comm/bin_parser.h"
#include "ur_rtde_driver/types.h"
#include "ur_rtde_driver/comm/ur/state.h"
namespace ur_driver
{
class SharedRobotModeData
{
public:
virtual bool parseWith(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_V1_X : uint8_t
{
ROBOT_RUNNING_MODE = 0,
ROBOT_FREEDRIVE_MODE = 1,
ROBOT_READY_MODE = 2,
ROBOT_INITIALIZING_MODE = 3,
ROBOT_SECURITY_STOPPED_MODE = 4,
ROBOT_EMERGENCY_STOPPED_MODE = 5,
ROBOT_FATAL_ERROR_MODE = 6,
ROBOT_NO_POWER_MODE = 7,
ROBOT_NOT_CONNECTED_MODE = 8,
ROBOT_SHUTDOWN_MODE = 9,
ROBOT_SAFEGUARD_STOP_MODE = 10
};
class RobotModeData_V1_X : public SharedRobotModeData, public StatePacket
{
public:
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URStatePacketConsumer& consumer);
robot_mode_V1_X robot_mode;
double speed_fraction;
static const size_t SIZE = SharedRobotModeData::SIZE + sizeof(uint8_t) + sizeof(robot_mode_V1_X) + sizeof(double);
static_assert(RobotModeData_V1_X::SIZE == 24, "RobotModeData_V1_X has missmatched size");
};
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 StatePacket
{
public:
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URStatePacketConsumer& consumer);
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(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(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 ur_driver

59
ur_rtde_driver/include/ur_rtde_driver/ur/rt_parser.h Normal file
View File

@@ -0,0 +1,59 @@
/*
* 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_rtde_driver/bin_parser.h"
#include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/comm/pipeline.h"
#include "ur_rtde_driver/comm/parser.h"
#include "ur_rtde_driver/ur/rt_state.h"
namespace ur_driver
{
template <typename T>
class URRTStateParser : public comm::URParser<RTPacket>
{
public:
bool parse(BinParser& bp, std::vector<std::unique_ptr<RTPacket>>& results)
{
int32_t packet_size = bp.peek<int32_t>();
if (!bp.checkSize(packet_size))
{
LOG_ERROR("Buffer len shorter than expected packet length");
return false;
}
bp.parse(packet_size); // consumes the peeked data
std::unique_ptr<RTPacket> packet(new T);
if (!packet->parseWith(bp))
return false;
results.push_back(std::move(packet));
return true;
}
};
typedef URRTStateParser<RTState_V1_6__7> URRTStateParser_V1_6__7;
typedef URRTStateParser<RTState_V1_8> URRTStateParser_V1_8;
typedef URRTStateParser<RTState_V3_0__1> URRTStateParser_V3_0__1;
typedef URRTStateParser<RTState_V3_2__3> URRTStateParser_V3_2__3;
} // namespace ur_driver

140
ur_rtde_driver/include/ur_rtde_driver/ur/rt_state.h Normal file
View File

@@ -0,0 +1,140 @@
/*
* 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 <cstddef>
#include "ur_rtde_driver/comm/bin_parser.h"
#include "ur_rtde_driver/comm/comm/pipeline.h"
#include "ur_rtde_driver/types.h"
namespace ur_driver
{
class URRTPacketConsumer;
class RTPacket
{
public:
virtual bool parseWith(BinParser& bp) = 0;
virtual bool consumeWith(URRTPacketConsumer& consumer) = 0;
};
class RTShared
{
protected:
void parse_shared1(BinParser& bp);
void parse_shared2(BinParser& bp);
public:
double time;
std::array<double, 6> q_target;
std::array<double, 6> qd_target;
std::array<double, 6> qdd_target;
std::array<double, 6> i_target;
std::array<double, 6> m_target;
std::array<double, 6> q_actual;
std::array<double, 6> qd_actual;
std::array<double, 6> i_actual;
// gap here depending on version
std::array<double, 6> tcp_force;
// does not contain "_actual" postfix in V1_X but
// they're the same fields so share anyway
cartesian_coord_t tool_vector_actual;
cartesian_coord_t tcp_speed_actual;
// gap here depending on version
uint64_t digital_inputs;
std::array<double, 6> motor_temperatures;
double controller_time;
double robot_mode;
static const size_t SIZE =
sizeof(double) * 3 + sizeof(double[6]) * 10 + sizeof(cartesian_coord_t) * 2 + sizeof(uint64_t);
};
class RTState_V1_6__7 : public RTShared, public RTPacket
{
public:
bool parseWith(BinParser& bp);
virtual bool consumeWith(URRTPacketConsumer& consumer);
double3_t tool_accelerometer_values;
static const size_t SIZE = RTShared::SIZE + sizeof(double3_t);
static_assert(RTState_V1_6__7::SIZE == 632, "RTState_V1_6__7 has mismatched size!");
};
class RTState_V1_8 : public RTState_V1_6__7
{
public:
bool parseWith(BinParser& bp);
virtual bool consumeWith(URRTPacketConsumer& consumer);
std::array<double, 6> joint_modes;
static const size_t SIZE = RTState_V1_6__7::SIZE + sizeof(double[6]);
static_assert(RTState_V1_8::SIZE == 680, "RTState_V1_8 has mismatched size!");
};
class RTState_V3_0__1 : public RTShared, public RTPacket
{
public:
bool parseWith(BinParser& bp);
virtual bool consumeWith(URRTPacketConsumer& consumer);
std::array<double, 6> i_control;
cartesian_coord_t tool_vector_target;
cartesian_coord_t tcp_speed_target;
std::array<double, 6> joint_modes;
double safety_mode;
double3_t tool_accelerometer_values;
double speed_scaling;
double linear_momentum_norm;
double v_main;
double v_robot;
double i_robot;
std::array<double, 6> v_actual;
static const size_t SIZE =
RTShared::SIZE + sizeof(double[6]) * 3 + sizeof(double3_t) + sizeof(cartesian_coord_t) * 2 + sizeof(double) * 6;
static_assert(RTState_V3_0__1::SIZE == 920, "RTState_V3_0__1 has mismatched size!");
};
class RTState_V3_2__3 : public RTState_V3_0__1
{
public:
bool parseWith(BinParser& bp);
virtual bool consumeWith(URRTPacketConsumer& consumer);
uint64_t digital_outputs;
double program_state;
static const size_t SIZE = RTState_V3_0__1::SIZE + sizeof(uint64_t) + sizeof(double);
static_assert(RTState_V3_2__3::SIZE == 936, "RTState_V3_2__3 has mismatched size!");
};
} // namespace ur_driver

64
ur_rtde_driver/include/ur_rtde_driver/ur/state.h Normal file
View File

@@ -0,0 +1,64 @@
/*
* 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 <cstddef>
#include "ur_rtde_driver/bin_parser.h"
#include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/comm/pipeline.h"
namespace ur_driver
{
enum class package_type : 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
};
enum class message_type : uint8_t
{
ROBOT_STATE = 16,
ROBOT_MESSAGE = 20,
PROGRAM_STATE_MESSAGE = 25
};
class URStatePacketConsumer;
class StatePacket
{
public:
StatePacket()
{
}
virtual ~StatePacket()
{
}
virtual bool parseWith(BinParser& bp) = 0;
virtual bool consumeWith(URStatePacketConsumer& consumer) = 0;
};
} // namespace ur_driver

120
ur_rtde_driver/include/ur_rtde_driver/ur/state_parser.h Normal file
View File

@@ -0,0 +1,120 @@
/*
* 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_rtde_driver/bin_parser.h"
#include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/comm/parser.h"
#include "ur_rtde_driver/comm/pipeline.h"
#include "ur_rtde_driver/ur/master_board.h"
#include "ur_rtde_driver/ur/robot_mode.h"
#include "ur_rtde_driver/ur/state.h"
namespace ur_driver
{
template <typename RMD, typename MBD>
class URStateParser : public comm::URParser<StatePacket>
{
private:
StatePacket* from_type(package_type type)
{
switch (type)
{
case package_type::ROBOT_MODE_DATA:
return new RMD;
case package_type::MASTERBOARD_DATA:
return new MBD;
default:
return nullptr;
}
}
public:
bool parse(BinParser& bp, std::vector<std::unique_ptr<StatePacket>>& results)
{
int32_t packet_size;
message_type type;
bp.parse(packet_size);
bp.parse(type);
if (type != message_type::ROBOT_STATE)
{
// quietly ignore the intial version message
if (type != message_type::ROBOT_MESSAGE)
{
LOG_WARN("Invalid state message type recieved: %u", static_cast<uint8_t>(type));
}
bp.consume();
return true;
}
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
BinParser sbp(bp, sub_size);
sbp.consume(sizeof(sub_size));
package_type type;
sbp.parse(type);
std::unique_ptr<StatePacket> packet(from_type(type));
if (packet == nullptr)
{
sbp.consume();
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;
}
}
return true;
}
};
typedef URStateParser<RobotModeData_V1_X, MasterBoardData_V1_X> URStateParser_V1_X;
typedef URStateParser<RobotModeData_V3_0__1, MasterBoardData_V3_0__1> URStateParser_V3_0__1;
typedef URStateParser<RobotModeData_V3_2, MasterBoardData_V3_2> URStateParser_V3_2;
typedef URStateParser<RobotModeData_V3_5, MasterBoardData_V3_2> URStateParser_V3_5;
} // namespace ur_driver

73
ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h Normal file
View File

@@ -0,0 +1,73 @@
// 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_rtde_driver/rtde/rtde_client.h"
#include "ur_rtde_driver/comm/reverse_interface.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.
*/
UrDriver(const std::string& ROBOT_IP);
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 the
* interface's cycle time. See the private parameter #rtde_frequency_
*/
std::unique_ptr<rtde_interface::DataPackage> getDataPackage();
const uint32_t& getControlFrequency() const
{
return rtde_frequency_;
}
bool writeJointCommand(const vector6d_t& values);
private:
//! This frequency is used for the rtde interface. By default, it will be 125Hz on CB3 and 500Hz on the e-series.
uint32_t rtde_frequency_;
comm::INotifier notifier_;
std::unique_ptr<rtde_interface::RTDEClient> rtde_client_;
std::unique_ptr<comm::ReverseInterface> reverse_interface_;
};
} // namespace ur_driver

54
ur_rtde_driver/launch/ur10_bringup.launch Normal file
View File

@@ -0,0 +1,54 @@
<?xml version="1.0"?>
<!--
Universal robot ur10 launch. Loads ur10 robot description (see ur_common.launch
for more info)
Usage:
ur10_bringup.launch robot_ip:=<value>
-->
<launch>
<!-- robot_ip: IP-address of the robot's socket-messaging server -->
<arg name="robot_ip"/>
<arg name="reverse_port" default="50001"/>
<arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="10.0"/>
<arg name="prefix" default="" />
<arg name="use_lowbandwidth_trajectory_follower" default="false"/>
<arg name="time_interval" default="0.008"/>
<arg name="servoj_time" default="0.008" />
<arg name="servoj_time_waiting" default="0.001" />
<arg name="max_waiting_time" default="2.0" />
<arg name="servoj_gain" default="100." />
<arg name="servoj_lookahead_time" default="1." />
<arg name="max_joint_difference" default="0.01" />
<arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<arg name="shutdown_on_disconnect" default="true" />
<!-- robot model -->
<include file="$(find ur_description)/launch/ur10_upload.launch">
<arg name="limited" value="$(arg limited)"/>
</include>
<!-- ur common -->
<include file="$(find ur_rtde_driver)/launch/ur_common.launch">
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="reverse_port" value="$(arg reverse_port)"/>
<arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/>
<arg name="prefix" default="" />
<arg name="use_lowbandwidth_trajectory_follower" value="$(arg use_lowbandwidth_trajectory_follower)"/>
<arg name="time_interval" value="$(arg time_interval)"/>
<arg name="servoj_time" value="$(arg servoj_time)" />
<arg name="servoj_time_waiting" default="$(arg servoj_time_waiting)" />
<arg name="max_waiting_time" value="$(arg max_waiting_time)" />
<arg name="servoj_gain" value="$(arg servoj_gain)" />
<arg name="servoj_lookahead_time" value="$(arg servoj_lookahead_time)" />
<arg name="max_joint_difference" value="$(arg max_joint_difference)" />
<arg name="base_frame" value="$(arg base_frame)" />
<arg name="tool_frame" value="$(arg tool_frame)" />
<arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
</include>
</launch>

55
ur_rtde_driver/launch/ur10_bringup_compatible.launch Normal file
View File

@@ -0,0 +1,55 @@
<?xml version="1.0"?>
<!--
Universal robot ur10 launch. Loads ur10 robot description (see ur_common.launch
for more info)
Usage:
ur10_bringup.launch robot_ip:=<value>
-->
<launch>
<!-- robot_ip: IP-address of the robot's socket-messaging server -->
<arg name="robot_ip"/>
<arg name="reverse_port" default="50001"/>
<arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="10.0"/>
<arg name="prefix" default="" />
<arg name="use_lowbandwidth_trajectory_follower" default="false"/>
<arg name="time_interval" default="0.08"/>
<arg name="servoj_time" default="0.08" />
<arg name="servoj_time_waiting" default="0.001" />
<arg name="max_waiting_time" default="2.0" />
<arg name="servoj_gain" default="100." />
<arg name="servoj_lookahead_time" default="1." />
<arg name="max_joint_difference" default="0.01" />
<arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<arg name="shutdown_on_disconnect" default="true" />
<!-- robot model -->
<include file="$(find ur_description)/launch/ur10_upload.launch">
<arg name="limited" value="$(arg limited)"/>
</include>
<!-- ur common -->
<include file="$(find ur_rtde_driver)/launch/ur_common.launch">
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="reverse_port" value="$(arg reverse_port)"/>
<arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/>
<arg name="prefix" default="" />
<arg name="use_lowbandwidth_trajectory_follower" value="$(arg use_lowbandwidth_trajectory_follower)"/>
<arg name="time_interval" value="$(arg time_interval)"/>
<arg name="servoj_time" value="$(arg servoj_time)" />
<arg name="servoj_time_waiting" default="$(arg servoj_time_waiting)" />
<arg name="max_waiting_time" value="$(arg max_waiting_time)" />
<arg name="servoj_gain" value="$(arg servoj_gain)" />
<arg name="servoj_lookahead_time" value="$(arg servoj_lookahead_time)" />
<arg name="max_joint_difference" value="$(arg max_joint_difference)" />
<arg name="base_frame" value="$(arg base_frame)" />
<arg name="tool_frame" value="$(arg tool_frame)" />
<arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
</include>
</launch>

48
ur_rtde_driver/launch/ur10_bringup_joint_limited.launch Normal file
View File

@@ -0,0 +1,48 @@
<?xml version="1.0"?>
<!--
Universal robot ur10 launch. Wraps ur10_bringup.launch. Uses the 'limited'
joint range [-PI, PI] on all joints.
Usage:
ur10_bringup_joint_limited.launch robot_ip:=<value>
-->
<launch>
<!-- robot_ip: IP-address of the robot's socket-messaging server -->
<arg name="robot_ip"/>
<arg name="reverse_port" default="50001"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="10.0"/>
<arg name="prefix" default="" />
<arg name="use_lowbandwidth_trajectory_follower" default="false"/>
<arg name="time_interval" default="0.008"/>
<arg name="servoj_time" default="0.008" />
<arg name="servoj_time_waiting" default="0.001" />
<arg name="max_waiting_time" default="2.0" />
<arg name="servoj_gain" default="100." />
<arg name="servoj_lookahead_time" default="1." />
<arg name="max_joint_difference" default="0.01" />
<arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<arg name="shutdown_on_disconnect" default="true" />
<include file="$(find ur_rtde_driver)/launch/ur10_bringup.launch">
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="reverse_port" value="$(arg reverse_port)"/>
<arg name="limited" value="true"/>
<arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/>
<arg name="prefix" value="$(arg prefix)" />
<arg name="use_lowbandwidth_trajectory_follower" value="$(arg use_lowbandwidth_trajectory_follower)"/>
<arg name="time_interval" value="$(arg time_interval)"/>
<arg name="servoj_time" value="$(arg servoj_time)" />
<arg name="servoj_time_waiting" default="$(arg servoj_time_waiting)" />
<arg name="max_waiting_time" value="$(arg max_waiting_time)" />
<arg name="servoj_gain" value="$(arg servoj_gain)" />
<arg name="servoj_lookahead_time" value="$(arg servoj_lookahead_time)" />
<arg name="max_joint_difference" value="$(arg max_joint_difference)" />
<arg name="base_frame" value="$(arg base_frame)" />
<arg name="tool_frame" value="$(arg tool_frame)" />
<arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
</include>
</launch>

56
ur_rtde_driver/launch/ur10_ros_control.launch Normal file
View File

@@ -0,0 +1,56 @@
<?xml version="1.0"?>
<launch>
<!-- GDB functionality -->
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
<arg name="robot_ip"/>
<arg name="reverse_port" default="50001"/>
<arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="3.0"/>
<arg name="prefix" default="" />
<arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
<arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<arg name="shutdown_on_disconnect" default="true" />
<arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller"/>
<arg name="stopped_controllers" default=""/>
<!-- robot model -->
<include file="$(find ur_description)/launch/ur10_upload.launch">
<arg name="limited" value="$(arg limited)"/>
</include>
<!-- Load hardware interface -->
<node name="ur_hardware_interface" pkg="ur_rtde_driver" type="ur_rtde_driver_node" output="screen" launch-prefix="$(arg launch_prefix)">
<param name="robot_ip" type="str" value="$(arg robot_ip)"/>
<!--<param name="reverse_port" type="int" value="$(arg reverse_port)" />-->
<!--<param name="min_payload" type="double" value="$(arg min_payload)"/>-->
<!--<param name="max_payload" type="double" value="$(arg max_payload)"/>-->
<!--<param name="max_velocity" type="double" value="$(arg max_velocity)"/>-->
<!--<param name="use_ros_control" type="bool" value="True"/>-->
<!--<param name="servoj_gain" type="double" value="750" />-->
<!--<param name="prefix" value="$(arg prefix)" />-->
<!--<param name="base_frame" type="str" value="$(arg base_frame)"/>-->
<!--<param name="tool_frame" type="str" value="$(arg tool_frame)"/>-->
<!--<param name="shutdown_on_disconnect" type="bool" value="$(arg shutdown_on_disconnect)"/>-->
</node>
<!-- Load controller settings -->
<rosparam file="$(find ur_rtde_driver)/config/ur10_controllers.yaml" 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)" />-->
<!-- Convert joint states to /tf tranforms -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
</launch>

54
ur_rtde_driver/launch/ur3_bringup.launch Normal file
View File

@@ -0,0 +1,54 @@
<?xml version="1.0"?>
<!--
Universal robot ur3 launch. Loads ur3 robot description (see ur_common.launch
for more info)
Usage:
ur3_bringup.launch robot_ip:=<value>
-->
<launch>
<!-- robot_ip: IP-address of the robot's socket-messaging server -->
<arg name="robot_ip"/>
<arg name="reverse_port" default="50001"/>
<arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="3.0"/>
<arg name="prefix" default="" />
<arg name="use_lowbandwidth_trajectory_follower" default="false"/>
<arg name="time_interval" default="0.008"/>
<arg name="servoj_time" default="0.008" />
<arg name="servoj_time_waiting" default="0.001" />
<arg name="max_waiting_time" default="2.0" />
<arg name="servoj_gain" default="100." />
<arg name="servoj_lookahead_time" default="1." />
<arg name="max_joint_difference" default="0.01" />
<arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<arg name="shutdown_on_disconnect" default="true" />
<!-- robot model -->
<include file="$(find ur_description)/launch/ur3_upload.launch">
<arg name="limited" value="$(arg limited)"/>
</include>
<!-- ur common -->
<include file="$(find ur_rtde_driver)/launch/ur_common.launch">
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="reverse_port" value="$(arg reverse_port)"/>
<arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/>
<arg name="prefix" value="$(arg prefix)" />
<arg name="use_lowbandwidth_trajectory_follower" value="$(arg use_lowbandwidth_trajectory_follower)"/>
<arg name="time_interval" value="$(arg time_interval)"/>
<arg name="servoj_time" value="$(arg servoj_time)" />
<arg name="servoj_time_waiting" default="$(arg servoj_time_waiting)" />
<arg name="max_waiting_time" value="$(arg max_waiting_time)" />
<arg name="servoj_gain" value="$(arg servoj_gain)" />
<arg name="servoj_lookahead_time" value="$(arg servoj_lookahead_time)" />
<arg name="max_joint_difference" value="$(arg max_joint_difference)" />
<arg name="base_frame" value="$(arg base_frame)" />
<arg name="tool_frame" value="$(arg tool_frame)" />
<arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
</include>
</launch>

48
ur_rtde_driver/launch/ur3_bringup_joint_limited.launch Normal file
View File

@@ -0,0 +1,48 @@
<?xml version="1.0"?>
<!--
Universal robot ur3 launch. Wraps ur3_bringup.launch. Uses the 'limited'
joint range [-PI, PI] on all joints.
Usage:
ur3_bringup_joint_limited.launch robot_ip:=<value>
-->
<launch>
<!-- robot_ip: IP-address of the robot's socket-messaging server -->
<arg name="robot_ip"/>
<arg name="reverse_port" default="50001"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="3.0"/>
<arg name="prefix" default="" />
<arg name="use_lowbandwidth_trajectory_follower" default="false"/>
<arg name="time_interval" default="0.008"/>
<arg name="servoj_time" default="0.008" />
<arg name="servoj_time_waiting" default="0.001" />
<arg name="max_waiting_time" default="2.0" />
<arg name="servoj_gain" default="100." />
<arg name="servoj_lookahead_time" default="1." />
<arg name="max_joint_difference" default="0.01" />
<arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<arg name="shutdown_on_disconnect" default="true" />
<include file="$(find ur_rtde_driver)/launch/ur3_bringup.launch">
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="reverse_port" value="$(arg reverse_port)"/>
<arg name="limited" value="true"/>
<arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/>
<arg name="prefix" value="$(arg prefix)" />
<arg name="use_lowbandwidth_trajectory_follower" value="$(arg use_lowbandwidth_trajectory_follower)"/>
<arg name="time_interval" value="$(arg time_interval)"/>
<arg name="servoj_time" value="$(arg servoj_time)" />
<arg name="servoj_time_waiting" default="$(arg servoj_time_waiting)" />
<arg name="max_waiting_time" value="$(arg max_waiting_time)" />
<arg name="servoj_gain" value="$(arg servoj_gain)" />
<arg name="servoj_lookahead_time" value="$(arg servoj_lookahead_time)" />
<arg name="max_joint_difference" value="$(arg max_joint_difference)" />
<arg name="base_frame" value="$(arg base_frame)" />
<arg name="tool_frame" value="$(arg tool_frame)" />
<arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
</include>
</launch>

56
ur_rtde_driver/launch/ur3_ros_control.launch Normal file
View File

@@ -0,0 +1,56 @@
<?xml version="1.0"?>
<launch>
<!-- GDB functionality -->
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
<arg name="robot_ip"/>
<arg name="reverse_port" default="50001"/>
<arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="3.0"/>
<arg name="prefix" default="" />
<arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
<arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<arg name="shutdown_on_disconnect" default="true" />
<arg name="controllers" default="joint_state_controller force_torque_sensor_controller vel_based_pos_traj_controller"/>
<arg name="stopped_controllers" default="pos_based_pos_traj_controller joint_group_vel_controller"/>
<!-- robot model -->
<include file="$(find ur_description)/launch/ur3_upload.launch">
<arg name="limited" value="$(arg limited)"/>
</include>
<!-- Load hardware interface -->
<node name="ur_hardware_interface" pkg="ur_rtde_driver" type="ur_driver" output="log" launch-prefix="$(arg launch_prefix)">
<param name="robot_ip_address" type="str" value="$(arg robot_ip)"/>
<param name="reverse_port" type="int" value="$(arg reverse_port)" />
<param name="min_payload" type="double" value="$(arg min_payload)"/>
<param name="max_payload" type="double" value="$(arg max_payload)"/>
<param name="max_velocity" type="double" value="$(arg max_velocity)"/>
<param name="use_ros_control" type="bool" value="True"/>
<param name="servoj_gain" type="double" value="750" />
<param name="prefix" value="$(arg prefix)" />
<param name="base_frame" type="str" value="$(arg base_frame)"/>
<param name="tool_frame" type="str" value="$(arg tool_frame)"/>
<param name="shutdown_on_disconnect" type="bool" value="$(arg shutdown_on_disconnect)"/>
</node>
<!-- Load controller settings -->
<rosparam file="$(find ur_rtde_driver)/config/ur3_controllers.yaml" 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)" />
<!-- Convert joint states to /tf tranforms -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
</launch>

54
ur_rtde_driver/launch/ur5_bringup.launch Normal file
View File

@@ -0,0 +1,54 @@
<?xml version="1.0"?>
<!--
Universal robot ur5 launch. Loads ur5 robot description (see ur_common.launch
for more info)
Usage:
ur5_bringup.launch robot_ip:=<value>
-->
<launch>
<!-- robot_ip: IP-address of the robot's socket-messaging server -->
<arg name="robot_ip"/>
<arg name="reverse_port" default="50001"/>
<arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="5.0"/>
<arg name="prefix" default="" />
<arg name="use_lowbandwidth_trajectory_follower" default="false"/>
<arg name="time_interval" default="0.008"/>
<arg name="servoj_time" default="0.008" />
<arg name="servoj_time_waiting" default="0.001" />
<arg name="max_waiting_time" default="2.0" />
<arg name="servoj_gain" default="100." />
<arg name="servoj_lookahead_time" default="1." />
<arg name="max_joint_difference" default="0.01" />
<arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<arg name="shutdown_on_disconnect" default="true" />
<!-- robot model -->
<include file="$(find ur_description)/launch/ur5_upload.launch">
<arg name="limited" value="$(arg limited)"/>
</include>
<!-- ur common -->
<include file="$(find ur_rtde_driver)/launch/ur_common.launch">
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="reverse_port" value="$(arg reverse_port)"/>
<arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/>
<arg name="prefix" value="$(arg prefix)" />
<arg name="use_lowbandwidth_trajectory_follower" value="$(arg use_lowbandwidth_trajectory_follower)"/>
<arg name="time_interval" value="$(arg time_interval)"/>
<arg name="servoj_time" value="$(arg servoj_time)" />
<arg name="servoj_time_waiting" default="$(arg servoj_time_waiting)" />
<arg name="max_waiting_time" value="$(arg max_waiting_time)" />
<arg name="servoj_gain" value="$(arg servoj_gain)" />
<arg name="servoj_lookahead_time" value="$(arg servoj_lookahead_time)" />
<arg name="max_joint_difference" value="$(arg max_joint_difference)" />
<arg name="base_frame" value="$(arg base_frame)" />
<arg name="tool_frame" value="$(arg tool_frame)" />
<arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
</include>
</launch>

55
ur_rtde_driver/launch/ur5_bringup_compatible.launch Normal file
View File

@@ -0,0 +1,55 @@
<?xml version="1.0"?>
<!--
Universal robot ur5 launch. Loads ur5 robot description (see ur_common.launch
for more info)
Usage:
ur5_bringup.launch robot_ip:=<value>
-->
<launch>
<!-- robot_ip: IP-address of the robot's socket-messaging server -->
<arg name="robot_ip"/>
<arg name="reverse_port" default="50001"/>
<arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="5.0"/>
<arg name="prefix" default="" />
<arg name="use_lowbandwidth_trajectory_follower" default="false"/>
<arg name="time_interval" default="0.08"/>
<arg name="servoj_time" default="0.08" />
<arg name="servoj_time_waiting" default="0.001" />
<arg name="max_waiting_time" default="2.0" />
<arg name="servoj_gain" default="100." />
<arg name="servoj_lookahead_time" default="1." />
<arg name="max_joint_difference" default="0.01" />
<arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<arg name="shutdown_on_disconnect" default="true" />
<!-- robot model -->
<include file="$(find ur_description)/launch/ur5_upload.launch">
<arg name="limited" value="$(arg limited)"/>
</include>
<!-- ur common -->
<include file="$(find ur_rtde_driver)/launch/ur_common.launch">
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="reverse_port" value="$(arg reverse_port)"/>
<arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/>
<arg name="prefix" value="$(arg prefix)" />
<arg name="use_lowbandwidth_trajectory_follower" value="$(arg use_lowbandwidth_trajectory_follower)"/>
<arg name="time_interval" value="$(arg time_interval)"/>
<arg name="servoj_time" value="$(arg servoj_time)" />
<arg name="servoj_time_waiting" default="$(arg servoj_time_waiting)" />
<arg name="max_waiting_time" value="$(arg max_waiting_time)" />
<arg name="servoj_gain" value="$(arg servoj_gain)" />
<arg name="servoj_lookahead_time" value="$(arg servoj_lookahead_time)" />
<arg name="max_joint_difference" value="$(arg max_joint_difference)" />
<arg name="base_frame" value="$(arg base_frame)" />
<arg name="tool_frame" value="$(arg tool_frame)" />
<arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
</include>
</launch>

48
ur_rtde_driver/launch/ur5_bringup_joint_limited.launch Normal file
View File

@@ -0,0 +1,48 @@
<?xml version="1.0"?>
<!--
Universal robot ur5 launch. Wraps ur5_bringup.launch. Uses the 'limited'
joint range [-PI, PI] on all joints.
Usage:
ur5_bringup_joint_limited.launch robot_ip:=<value>
-->
<launch>
<!-- robot_ip: IP-address of the robot's socket-messaging server -->
<arg name="robot_ip"/>
<arg name="reverse_port" default="50001"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="5.0"/>
<arg name="prefix" default="" />
<arg name="use_lowbandwidth_trajectory_follower" default="false"/>
<arg name="time_interval" default="0.008"/>
<arg name="servoj_time" default="0.008" />
<arg name="servoj_time_waiting" default="0.001" />
<arg name="max_waiting_time" default="2.0" />
<arg name="servoj_gain" default="100." />
<arg name="servoj_lookahead_time" default="1." />
<arg name="max_joint_difference" default="0.01" />
<arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<arg name="shutdown_on_disconnect" default="true" />
<include file="$(find ur_rtde_driver)/launch/ur5_bringup.launch">
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="reverse_port" value="$(arg reverse_port)"/>
<arg name="limited" value="true"/>
<arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/>
<arg name="prefix" value="$(arg prefix)" />
<arg name="use_lowbandwidth_trajectory_follower" value="$(arg use_lowbandwidth_trajectory_follower)"/>
<arg name="time_interval" value="$(arg time_interval)"/>
<arg name="servoj_time" value="$(arg servoj_time)" />
<arg name="servoj_time_waiting" default="$(arg servoj_time_waiting)" />
<arg name="max_waiting_time" value="$(arg max_waiting_time)" />
<arg name="servoj_gain" value="$(arg servoj_gain)" />
<arg name="servoj_lookahead_time" value="$(arg servoj_lookahead_time)" />
<arg name="max_joint_difference" value="$(arg max_joint_difference)" />
<arg name="base_frame" value="$(arg base_frame)" />
<arg name="tool_frame" value="$(arg tool_frame)" />
<arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
</include>
</launch>

56
ur_rtde_driver/launch/ur5_ros_control.launch Normal file
View File

@@ -0,0 +1,56 @@
<?xml version="1.0"?>
<launch>
<!-- GDB functionality -->
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
<arg name="robot_ip"/>
<arg name="reverse_port" default="50001"/>
<arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="3.0"/>
<arg name="prefix" default="" />
<arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
<arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<arg name="shutdown_on_disconnect" default="true" />
<arg name="controllers" default="joint_state_controller force_torque_sensor_controller vel_based_pos_traj_controller"/>
<arg name="stopped_controllers" default="pos_based_pos_traj_controller joint_group_vel_controller"/>
<!-- robot model -->
<include file="$(find ur_description)/launch/ur5_upload.launch">
<arg name="limited" value="$(arg limited)"/>
</include>
<!-- Load hardware interface -->
<node name="ur_hardware_interface" pkg="ur_rtde_driver" type="ur_driver" output="log" launch-prefix="$(arg launch_prefix)">
<param name="robot_ip_address" type="str" value="$(arg robot_ip)"/>
<param name="reverse_port" type="int" value="$(arg reverse_port)" />
<param name="min_payload" type="double" value="$(arg min_payload)"/>
<param name="max_payload" type="double" value="$(arg max_payload)"/>
<param name="max_velocity" type="double" value="$(arg max_velocity)"/>
<param name="use_ros_control" type="bool" value="True"/>
<param name="servoj_gain" type="double" value="750" />
<param name="prefix" value="$(arg prefix)" />
<param name="base_frame" type="str" value="$(arg base_frame)"/>
<param name="tool_frame" type="str" value="$(arg tool_frame)"/>
<param name="shutdown_on_disconnect" type="bool" value="$(arg shutdown_on_disconnect)"/>
</node>
<!-- Load controller settings -->
<rosparam file="$(find ur_rtde_driver)/config/ur5_controllers.yaml" 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)" />
<!-- Convert joint states to /tf tranforms -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
</launch>

60
ur_rtde_driver/launch/ur_common.launch Normal file
View File

@@ -0,0 +1,60 @@
<?xml version="1.0"?>
<!--
Universal robot common bringup. Starts ur driver node and robot state
publisher (translates joint positions to propper tfs).
Usage:
ur_common.launch robot_ip:=<value>
-->
<launch>
<!-- robot_ip: IP-address of the robot's socket-messaging server -->
<arg name="robot_ip" />
<arg name="reverse_port" default="50001"/>
<arg name="min_payload" />
<arg name="max_payload" />
<arg name="prefix" default="" />
<arg name="use_ros_control" default="false"/>
<arg name="use_lowbandwidth_trajectory_follower" default="false"/>
<arg name="time_interval" default="0.008"/>
<arg name="servoj_time" default="0.008" />
<arg name="servoj_time_waiting" default="0.001" />
<arg name="max_waiting_time" default="2.0" />
<arg name="servoj_gain" default="100." />
<arg name="servoj_lookahead_time" default="1." />
<arg name="max_joint_difference" default="0.01" />
<arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<arg name="shutdown_on_disconnect" default="true" />
<!-- require_activation defines when the service /ur_driver/robot_enable needs to be called. -->
<arg name="require_activation" default="Never" /> <!-- Never, Always, OnStartup -->
<!-- The max_velocity parameter is only used for debugging in the ur_driver. It's not related to actual velocity limits -->
<arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<!-- driver -->
<node name="ur_driver" pkg="ur_rtde_driver" type="ur_driver" output="screen">
<!-- copy the specified IP address to be consistant with ROS-Industrial spec. -->
<param name="prefix" type="str" value="$(arg prefix)" />
<param name="robot_ip_address" type="str" value="$(arg robot_ip)" />
<param name="reverse_port" type="int" value="$(arg reverse_port)" />
<param name="use_ros_control" type="bool" value="$(arg use_ros_control)"/>
<param name="use_lowbandwidth_trajectory_follower" type="bool" value="$(arg use_lowbandwidth_trajectory_follower)"/>
<param name="min_payload" type="double" value="$(arg min_payload)" />
<param name="max_payload" type="double" value="$(arg max_payload)" />
<param name="max_velocity" type="double" value="$(arg max_velocity)" />
<param name="time_interval" type="double" value="$(arg time_interval)" />
<param name="servoj_time" type="double" value="$(arg servoj_time)" />
<param name="servoj_time_waiting" type="double" value="$(arg servoj_time_waiting)" />
<param name="max_waiting_time" type="double" value="$(arg max_waiting_time)" />
<param name="servoj_gain" type="double" value="$(arg servoj_gain)" />
<param name="servoj_lookahead_time" type="double" value="$(arg servoj_lookahead_time)" />
<param name="max_joint_difference" type="double" value="$(arg max_joint_difference)" />
<param name="base_frame" type="str" value="$(arg base_frame)"/>
<param name="tool_frame" type="str" value="$(arg tool_frame)"/>
<param name="require_activation" type="str" value="$(arg require_activation)" />
<param name="shutdown_on_disconnect" type="bool" value="$(arg shutdown_on_disconnect)"/>
</node>
</launch>

45
ur_rtde_driver/package.xml Normal file
View File

@@ -0,0 +1,45 @@
<?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_rtde_driver</name>
<version>0.1.0</version>
<description>The new driver for Universal Robots UR3, UR5 and UR10 robots with CB2 and CB3 controllers.</description>
<author>Thomas Timm Andersen</author>
<author>Simon Rasmussen</author>
<maintainer email="g.a.vanderhoorn@tudelft.nl">G.A. vd. Hoorn (TU Delft Robotics Institute)</maintainer>
<maintainer email="nhg@ipa.fhg.de">Nadia Hammoudeh Garcia</maintainer>
<license>Apache 2.0</license>
<license>BSD 2-clause</license>
<license>Zlib</license>
<url type="website">http://wiki.ros.org/ur_rtde_driver</url>
<url type="bugtracker">https://github.com/ros-industrial/ur_rtde_driver/issues</url>
<url type="repository">https://github.com/ros-industrial/ur_rtde_driver</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>boost</build_depend>
<depend>hardware_interface</depend>
<depend>controller_manager</depend>
<depend>actionlib</depend>
<depend>control_msgs</depend>
<depend>geometry_msgs</depend>
<depend>industrial_msgs</depend>
<depend>roscpp</depend>
<depend>sensor_msgs</depend>
<depend>trajectory_msgs</depend>
<depend>ur_controllers</depend>
<depend>ur_msgs</depend>
<depend>tf</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>ur_description</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>velocity_controllers</exec_depend>
<test_depend>rosunit</test_depend>
</package>

157
ur_rtde_driver/src/comm/server.cpp Normal file
View File

@@ -0,0 +1,157 @@
/*
* 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_rtde_driver/comm/server.h"
#include <arpa/inet.h>
#include <netinet/tcp.h>
#include <unistd.h>
#include <cstring>
#include "ur_rtde_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

192
ur_rtde_driver/src/comm/tcp_socket.cpp Normal file
View File

@@ -0,0 +1,192 @@
/*
* 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_rtde_driver/log.h"
#include "ur_rtde_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));
}
bool TCPSocket::setup(std::string& host, int port)
{
if (state_ == SocketState::Connected)
return false;
LOG_INFO("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_INFO("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()
{
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;
}
} // namespace comm
} // namespace ur_driver

40
ur_rtde_driver/src/main_plain_driver.cpp Normal file
View File

@@ -0,0 +1,40 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-11
*
*/
//----------------------------------------------------------------------
#include <ur_rtde_driver/ur/ur_driver.h>
using namespace ur_driver;
int main(int argc, char* argv[])
{
std::string ROBOT_IP = "192.168.56.101";
if (argc > 1)
{
ROBOT_IP = argv[1];
}
UrDriver driver(ROBOT_IP);
while (true)
{
sleep(1);
std::unique_ptr<rtde_interface::DataPackage> data_pkg = driver.getDataPackage();
if (data_pkg)
{
data_pkg->toString();
}
}
return 0;
}

54
ur_rtde_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_rtde_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_rtde_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_rtde_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_rtde_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_rtde_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_rtde_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_rtde_driver/log.h"
#include "ur_rtde_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

76
ur_rtde_driver/src/primary/robot_state/kinematics_info.cpp Normal file
View File

@@ -0,0 +1,76 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-08
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/primary/robot_state/kinematics_info.h"
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 << dh_theta_[i] << " ";
}
os << "]" << std::endl;
os << "dh_a: [";
for (size_t i = 0; i < dh_a_.size(); ++i)
{
os << dh_a_[i] << " ";
}
os << "]" << std::endl;
os << "dh_d: [";
for (size_t i = 0; i < dh_d_.size(); ++i)
{
os << dh_d_[i] << " ";
}
os << "]" << std::endl;
os << "dh_alpha: [";
for (size_t i = 0; i < dh_alpha_.size(); ++i)
{
os << dh_alpha_[i] << " ";
}
os << "]" << std::endl;
os << "calibration_status: " << calibration_status_ << std::endl;
return os.str();
}
} // namespace primary_interface
} // namespace ur_driver

57
ur_rtde_driver/src/producer.cpp Normal file
View File

@@ -0,0 +1,57 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2018-12-11
*
*/
//----------------------------------------------------------------------
#include <ur_rtde_driver/comm/producer.h>
#include <ur_rtde_driver/comm/shell_consumer.h>
#include <ur_rtde_driver/comm/stream.h>
#include <ur_rtde_driver/comm/parser.h>
#include <ur_rtde_driver/comm/pipeline.h>
#include <ur_rtde_driver/primary/package_header.h>
#include <ur_rtde_driver/primary/primary_parser.h>
static const int UR_PRIMARY_PORT = 30001;
static const int UR_SECONDARY_PORT = 30002;
static const int UR_RT_PORT = 30003;
using namespace ur_driver;
using namespace primary_interface;
int main(int argc, char* argv[])
{
std::string ROBOT_IP = "192.168.56.101";
// std::string ROBOT_IP = "192.168.0.104";
comm::URStream<PackageHeader> stream(ROBOT_IP, UR_PRIMARY_PORT);
if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug))
{
ros::console::notifyLoggerLevelsChanged();
}
primary_interface::PrimaryParser parser;
comm::URProducer<PackageHeader> prod(stream, parser);
comm::ShellConsumer<PackageHeader> consumer;
comm::INotifier notifier;
comm::Pipeline<PackageHeader> pipeline(prod, consumer, "Pipeline", notifier);
LOG_INFO("Running now");
pipeline.run();
while (true)
{
sleep(1);
// LOG_INFO("Still running");
}
return 0;
}

152
ur_rtde_driver/src/ros/hardware_interface.cpp Normal file
View File

@@ -0,0 +1,152 @@
// 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_rtde_driver/ros/hardware_interface.h"
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 }
, joint_names_(6)
, position_controller_running_(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 ROBOT_IP = robot_hw_nh.param<std::string>("robot_ip", "192.168.56.101");
ROS_INFO_STREAM("Initializing urdriver");
ur_driver_.reset(new UrDriver(ROBOT_IP));
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)
{
// 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]));
}
speedsc_interface_.registerHandle(
ur_controllers::SpeedScalingHandle("speed_scaling_factor", &speed_scaling_combined_));
// Register interfaces
registerInterface(&js_interface_);
registerInterface(&pj_interface_);
registerInterface(&speedsc_interface_);
ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_rtde_driver hardware_interface");
return true;
}
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)
{
if (!data_pkg->getData("actual_q", joint_positions_))
{
// This throwing should never happen unless misconfigured
throw std::runtime_error("Did not find joint position in data sent from robot. This should not happen!");
}
if (!data_pkg->getData("actual_qd", joint_velocities_))
{
// This throwing should never happen unless misconfigured
throw std::runtime_error("Did not find joint velocity in data sent from robot. This should not happen!");
}
if (!data_pkg->getData("target_speed_fraction", speed_scaling_value_))
{
// This throwing should never happen unless misconfigured
throw std::runtime_error("Did not find speed_scaling in data sent from robot. This should not happen!");
}
}
else
{
ROS_ERROR("Could not get fresh data package from robot");
}
}
void HardwareInterface ::write(const ros::Time& time, const ros::Duration& period)
{
if (position_controller_running_)
{
ur_driver_->writeJointCommand(joint_position_command_);
}
}
bool HardwareInterface ::prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list)
{
// TODO: Implement
return true;
}
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 == "hardware_interface::PositionJointInterface")
{
position_controller_running_ = true;
}
}
}
}
uint32_t HardwareInterface ::getControlFrequency() const
{
if (ur_driver_ != nullptr)
{
return ur_driver_->getControlFrequency();
}
// TODO: Do this nicer than throwing an exception
throw std::runtime_error("ur_driver is not yet initialized");
}
} // namespace ur_driver

86
ur_rtde_driver/src/ros/hardware_interface_node.cpp Normal file
View File

@@ -0,0 +1,86 @@
// 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 <ur_rtde_driver/ros/hardware_interface.h>
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("~");
ur_driver::HardwareInterface hw_interface;
// Set up timers
ros::Time timestamp;
ros::Duration period;
auto stopwatch_last = std::chrono::steady_clock::now();
auto stopwatch_now = stopwatch_last;
hw_interface.init(nh, nh_priv);
controller_manager::ControllerManager cm(&hw_interface, 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;
ros::Rate control_rate(static_cast<double>(hw_interface.getControlFrequency()));
// Run as fast as possible
while (ros::ok())
{
// Receive current state from robot
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);
hw_interface.write(timestamp, period);
if (!control_rate.sleep())
{
ROS_WARN_STREAM("Could not keep cycle rate of " << control_rate.expectedCycleTime().toNSec() / 1000000.0 << "ms");
}
}
spinner.stop();
ROS_INFO_STREAM_NAMED("hardware_interface", "Shutting down.");
return 0;
}

97
ur_rtde_driver/src/ros/service_stopper.cpp Normal file
View File

@@ -0,0 +1,97 @@
/*
* 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_rtde_driver/ros/service_stopper.h"
namespace ur_driver
{
ServiceStopper::ServiceStopper(std::vector<Service*> services)
: enable_service_(nh_.advertiseService("ur_driver/robot_enable", &ServiceStopper::enableCallback, this))
, services_(services)
, last_state_(RobotState::Error)
, activation_mode_(ActivationMode::Never)
{
std::string mode;
ros::param::param("~require_activation", mode, std::string("Never"));
if (mode == "Always")
{
activation_mode_ = ActivationMode::Always;
}
else if (mode == "OnStartup")
{
activation_mode_ = ActivationMode::OnStartup;
}
else
{
if (mode != "Never")
{
LOG_WARN("Found invalid value for param require_activation: '%s'\nShould be one of Never, OnStartup, Always",
mode.c_str());
mode = "Never";
}
notify_all(RobotState::Running);
}
LOG_INFO("Service 'ur_driver/robot_enable' activation mode: %s", mode.c_str());
}
bool ServiceStopper::enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp)
{
// After the startup call OnStartup and Never behave the same
if (activation_mode_ == ActivationMode::OnStartup)
activation_mode_ = ActivationMode::Never;
notify_all(RobotState::Running);
return true;
}
void ServiceStopper::notify_all(RobotState state)
{
if (last_state_ == state)
return;
for (auto const service : services_)
{
service->onRobotStateChange(state);
}
last_state_ = state;
}
bool ServiceStopper::handle(SharedRobotModeData& data, bool error)
{
if (data.emergency_stopped)
{
notify_all(RobotState::EmergencyStopped);
}
else if (data.protective_stopped)
{
notify_all(RobotState::ProtectiveStopped);
}
else if (error)
{
notify_all(RobotState::Error);
}
else if (activation_mode_ == ActivationMode::Never)
{
// No error encountered, the user requested automatic reactivation
notify_all(RobotState::Running);
}
return true;
}
} // namespace ur_driver

48
ur_rtde_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_rtde_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

50
ur_rtde_driver/src/rtde/control_package_setup_inputs.cpp Normal file
View File

@@ -0,0 +1,50 @@
// 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_rtde_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();
}
} // namespace rtde_interface
} // namespace ur_driver

92
ur_rtde_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_rtde_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 = sizeof(double) + 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

53
ur_rtde_driver/src/rtde/control_package_start.cpp Normal file
View File

@@ -0,0 +1,53 @@
// 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_rtde_driver/rtde/control_package_start.h"
namespace ur_driver
{
namespace rtde_interface
{
bool ControlPackageStart::parseWith(comm::BinParser& bp)
{
bp.parse(accepted_);
return true;
}
std::string ControlPackageStart::toString() const
{
std::stringstream ss;
ss << "accepted: " << static_cast<int>(accepted_);
return ss.str();
}
size_t ControlPackageStartRequest::generateSerializedRequest(uint8_t* buffer)
{
return PackageHeader::serializeHeader(buffer, PACKAGE_TYPE, PAYLOAD_SIZE);
}
} // namespace rtde_interface
} // namespace ur_driver

222
ur_rtde_driver/src/rtde/data_package.cpp Normal file
View File

@@ -0,0 +1,222 @@
// 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-10
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/rtde/data_package.h"
namespace ur_driver
{
namespace rtde_interface
{
std::unordered_map<std::string, DataPackage::_rtde_type_variant> DataPackage::type_list_{
{ "timestamp", double() },
{ "target_q", vector6d_t() },
{ "target_qd", vector6d_t() },
{ "target_qdd", vector6d_t() },
{ "target_current", vector6d_t() },
{ "target_moment", vector6d_t() },
{ "actual_q", vector6d_t() },
{ "actual_qd", vector6d_t() },
{ "actual_qdd", vector6d_t() },
{ "actual_current", vector6d_t() },
{ "actual_moment", vector6d_t() },
{ "joint_control_output", vector6d_t() },
{ "actual_TCP_pose", vector6d_t() },
{ "actual_TCP_speed", vector6d_t() },
{ "actual_TCP_force", vector6d_t() },
{ "target_TCP_pose", vector6d_t() },
{ "target_TCP_speed", vector6d_t() },
{ "actual_digital_input_bits", uint64_t() },
{ "joint_temperatures", vector6d_t() },
{ "actual_execution_time", double() },
{ "robot_mode", int32_t() },
{ "joint_mode", vector6int32_t() },
{ "safety_mode", int32_t() },
{ "actual_tool_accelerometer", vector3d_t() },
{ "speed_scaling", double() },
{ "target_speed_fraction", double() },
{ "actual_momentum", double() },
{ "actial_main_voltage", double() },
{ "actual_robot_voltage", double() },
{ "actual_robot_current", double() },
{ "actual_joint_voltage", vector6d_t() },
{ "actual_digital_output_bits", uint64_t() },
{ "runtime_state", uint32_t() },
{ "elbow_position", vector3d_t() },
{ "elbow_velocity", vector3d_t() },
{ "robot_status_bits", uint32_t() },
{ "safety_status_bits", uint32_t() },
{ "analog_io_types", uint32_t() },
{ "standard_analog_input0", double() },
{ "standard_analog_input1", double() },
{ "standard_analog_output0", double() },
{ "standard_analog_output1", double() },
{ "io_current", double() },
{ "euromap67_input_bits", uint32_t() },
{ "euromap67_output_bits", uint32_t() },
{ "euromap67_24V_voltage", double() },
{ "euromap67_24V_current", double() },
{ "tool_mode", uint32_t() },
{ "tool_analog_input_types", uint32_t() },
{ "tool_analog_input0", double() },
{ "tool_analog_input1", double() },
{ "tool_output_voltage", int32_t() },
{ "tool_output_current", double() },
{ "tool_temperature", double() },
{ "tool_force_scalar", double() },
{ "output_bit_registers0_to_31", uint32_t() },
{ "output_bit_registers32_to_63", uint32_t() },
{ "output_int_register_0", int32_t() },
{ "output_int_register_1", int32_t() },
{ "output_int_register_2", int32_t() },
{ "output_int_register_3", int32_t() },
{ "output_int_register_4", int32_t() },
{ "output_int_register_5", int32_t() },
{ "output_int_register_6", int32_t() },
{ "output_int_register_7", int32_t() },
{ "output_int_register_8", int32_t() },
{ "output_int_register_9", int32_t() },
{ "output_int_register_10", int32_t() },
{ "output_int_register_11", int32_t() },
{ "output_int_register_12", int32_t() },
{ "output_int_register_13", int32_t() },
{ "output_int_register_14", int32_t() },
{ "output_int_register_15", int32_t() },
{ "output_int_register_16", int32_t() },
{ "output_int_register_17", int32_t() },
{ "output_int_register_18", int32_t() },
{ "output_int_register_19", int32_t() },
{ "output_int_register_20", int32_t() },
{ "output_int_register_21", int32_t() },
{ "output_int_register_22", int32_t() },
{ "output_int_register_23", int32_t() },
{ "output_double_register_0", double() },
{ "output_double_register_1", double() },
{ "output_double_register_2", double() },
{ "output_double_register_3", double() },
{ "output_double_register_4", double() },
{ "output_double_register_5", double() },
{ "output_double_register_6", double() },
{ "output_double_register_7", double() },
{ "output_double_register_8", double() },
{ "output_double_register_9", double() },
{ "output_double_register_10", double() },
{ "output_double_register_11", double() },
{ "output_double_register_12", double() },
{ "output_double_register_13", double() },
{ "output_double_register_14", double() },
{ "output_double_register_15", double() },
{ "output_double_register_16", double() },
{ "output_double_register_17", double() },
{ "output_double_register_18", double() },
{ "output_double_register_19", double() },
{ "output_double_register_20", double() },
{ "output_double_register_21", double() },
{ "output_double_register_22", double() },
{ "output_double_register_23", double() },
{ "input_bit_registers0_to_31", uint32_t() },
{ "input_bit_registers32_to_63", uint32_t() },
{ "input_int_register_0", int32_t() },
{ "input_int_register_1", int32_t() },
{ "input_int_register_2", int32_t() },
{ "input_int_register_3", int32_t() },
{ "input_int_register_4", int32_t() },
{ "input_int_register_5", int32_t() },
{ "input_int_register_6", int32_t() },
{ "input_int_register_7", int32_t() },
{ "input_int_register_8", int32_t() },
{ "input_int_register_9", int32_t() },
{ "input_int_register_10", int32_t() },
{ "input_int_register_11", int32_t() },
{ "input_int_register_12", int32_t() },
{ "input_int_register_13", int32_t() },
{ "input_int_register_14", int32_t() },
{ "input_int_register_15", int32_t() },
{ "input_int_register_16", int32_t() },
{ "input_int_register_17", int32_t() },
{ "input_int_register_18", int32_t() },
{ "input_int_register_19", int32_t() },
{ "input_int_register_20", int32_t() },
{ "input_int_register_21", int32_t() },
{ "input_int_register_22", int32_t() },
{ "input_int_register_23", int32_t() },
{ "input_double_register_0", double() },
{ "input_double_register_1", double() },
{ "input_double_register_2", double() },
{ "input_double_register_3", double() },
{ "input_double_register_4", double() },
{ "input_double_register_5", double() },
{ "input_double_register_6", double() },
{ "input_double_register_7", double() },
{ "input_double_register_8", double() },
{ "input_double_register_9", double() },
{ "input_double_register_10", double() },
{ "input_double_register_11", double() },
{ "input_double_register_12", double() },
{ "input_double_register_13", double() },
{ "input_double_register_14", double() },
{ "input_double_register_15", double() },
{ "input_double_register_16", double() },
{ "input_double_register_17", double() },
{ "input_double_register_18", double() },
{ "input_double_register_19", double() },
{ "input_double_register_20", double() },
{ "input_double_register_21", double() },
{ "input_double_register_22", double() },
{ "input_double_register_23", double() }
};
bool rtde_interface::DataPackage ::parseWith(comm::BinParser& bp)
{
bp.parse(recipe_id_);
for (auto& item : recipe_)
{
if (type_list_.find(item) != type_list_.end())
{
_rtde_type_variant entry = type_list_[item];
auto bound_visitor = std::bind(ParseVisitor(), std::placeholders::_1, bp);
boost::apply_visitor(bound_visitor, entry);
data_[item] = entry;
}
else
{
return false;
}
}
return true;
}
std::string rtde_interface::DataPackage::toString() const
{
std::stringstream ss;
for (auto& item : data_)
{
ss << item.first << ": ";
ss << boost::apply_visitor(StringVisitor{}, item.second) << std::endl;
}
return ss.str();
}
} // namespace rtde_interface
} // namespace ur_driver

57
ur_rtde_driver/src/rtde/get_urcontrol_version.cpp Normal file
View File

@@ -0,0 +1,57 @@
// 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_rtde_driver/rtde/get_urcontrol_version.h"
namespace ur_driver
{
namespace rtde_interface
{
bool GetUrcontrolVersion::parseWith(comm::BinParser& bp)
{
bp.parse(major_);
bp.parse(minor_);
bp.parse(bugfix_);
bp.parse(build_);
return true;
}
std::string GetUrcontrolVersion::toString() const
{
std::stringstream ss;
ss << "version: " << major_ << ".";
ss << minor_ << "." << bugfix_ << "." << build_;
return ss.str();
}
size_t GetUrcontrolVersionRequest::generateSerializedRequest(uint8_t* buffer)
{
return PackageHeader::serializeHeader(buffer, PACKAGE_TYPE, PAYLOAD_SIZE);
}
} // namespace rtde_interface
} // namespace ur_driver

57
ur_rtde_driver/src/rtde/request_protocol_version.cpp Normal file
View File

@@ -0,0 +1,57 @@
// 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_rtde_driver/rtde/request_protocol_version.h"
namespace ur_driver
{
namespace rtde_interface
{
bool RequestProtocolVersion::parseWith(comm::BinParser& bp)
{
bp.parse(accepted_);
return true;
}
std::string RequestProtocolVersion::toString() const
{
std::stringstream ss;
ss << "accepted: " << static_cast<int>(accepted_);
return ss.str();
}
size_t RequestProtocolVersionRequest::generateSerializedRequest(uint8_t* buffer, uint16_t version)
{
size_t size = 0;
size += PackageHeader::serializeHeader(buffer, PACKAGE_TYPE, PAYLOAD_SIZE);
size += comm::PackageSerializer::serialize(buffer + size, version);
return size;
}
} // namespace rtde_interface
} // namespace ur_driver

124
ur_rtde_driver/src/rtde/rtde_client.cpp Normal file
View File

@@ -0,0 +1,124 @@
// 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
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/rtde/rtde_client.h"
namespace ur_driver
{
namespace rtde_interface
{
RTDEClient::RTDEClient(std::string ROBOT_IP, comm::INotifier& notifier)
: stream_(ROBOT_IP, UR_RTDE_PORT)
, parser_(readRecipe())
, prod_(stream_, parser_)
, pipeline_(prod_, PIPELINE_NAME, notifier)
{
}
bool RTDEClient::init()
{
pipeline_.run();
uint8_t buffer[4096];
size_t size;
size_t written;
// negotiate version
uint16_t protocol_version = 2;
size = RequestProtocolVersionRequest::generateSerializedRequest(buffer, protocol_version);
stream_.write(buffer, size, written);
std::unique_ptr<comm::URPackage<PackageHeader>> package;
pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000));
rtde_interface::RequestProtocolVersion* tmp_version =
dynamic_cast<rtde_interface::RequestProtocolVersion*>(package.get());
if (!tmp_version->accepted_)
{
protocol_version = 1;
size = RequestProtocolVersionRequest::generateSerializedRequest(buffer, protocol_version);
stream_.write(buffer, size, written);
pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000));
tmp_version = dynamic_cast<rtde_interface::RequestProtocolVersion*>(package.get());
if (!tmp_version->accepted_)
{
LOG_ERROR("Could not negotiate protocol version");
return false;
}
}
// determine maximum frequency from ur-control version
double max_frequency = URE_MAX_FREQUENCY;
size = GetUrcontrolVersionRequest::generateSerializedRequest(buffer);
stream_.write(buffer, size, written);
pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000));
rtde_interface::GetUrcontrolVersion* tmp_control_version =
dynamic_cast<rtde_interface::GetUrcontrolVersion*>(package.get());
if (tmp_control_version->major_ < 5)
{
max_frequency = CB3_MAX_FREQUENCY;
}
// sending output recipe
LOG_INFO("Setting up RTDE communication with frequency %f", max_frequency);
if (protocol_version == 2)
{
size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, max_frequency, readRecipe());
}
else
{
size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, readRecipe());
}
stream_.write(buffer, size, written);
return pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000));
}
bool RTDEClient::start()
{
uint8_t buffer[4096];
size_t size;
size_t written;
size = ControlPackageStartRequest::generateSerializedRequest(buffer);
std::unique_ptr<comm::URPackage<PackageHeader>> package;
stream_.write(buffer, size, written);
pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000));
rtde_interface::ControlPackageStart* tmp = dynamic_cast<rtde_interface::ControlPackageStart*>(package.get());
return tmp->accepted_;
}
std::vector<std::string> RTDEClient::readRecipe()
{
std::vector<std::string> recipe;
recipe.push_back("timestamp");
recipe.push_back("actual_q");
recipe.push_back("actual_qd");
recipe.push_back("speed_scaling");
recipe.push_back("target_speed_fraction");
return recipe;
}
bool RTDEClient::getDataPackage(std::unique_ptr<comm::URPackage<PackageHeader>>& data_package,
std::chrono::milliseconds timeout)
{
return pipeline_.getLatestProduct(data_package, timeout);
}
} // namespace rtde_interface
} // namespace ur_driver

53
ur_rtde_driver/src/rtde/rtde_package.cpp Normal file
View File

@@ -0,0 +1,53 @@
// 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-10
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/rtde/rtde_package.h"
namespace ur_driver
{
namespace rtde_interface
{
bool RTDEPackage::parseWith(comm::BinParser& bp)
{
bp.rawData(buffer_, buffer_length_);
return true;
}
std::string rtde_interface::RTDEPackage ::toString() const
{
std::stringstream ss;
ss << "Type: " << static_cast<int>(type_) << std::endl;
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 rtde_interface
} // namespace ur_driver

54
ur_rtde_driver/src/rtde/text_message.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 Tristan Schnell schnell@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/rtde/text_message.h"
namespace ur_driver
{
namespace rtde_interface
{
bool TextMessage::parseWith(comm::BinParser& bp)
{
bp.parse(message_length_);
bp.parse(message_, message_length_);
bp.parse(source_length_);
bp.parse(source_, source_length_);
bp.parse(warning_level_);
return true;
}
std::string TextMessage::toString() const
{
std::stringstream ss;
ss << "message: " << message_ << std::endl;
ss << "source: " << source_ << std::endl;
ss << "warning level: " << static_cast<int>(warning_level_);
return ss.str();
}
} // namespace rtde_interface
} // namespace ur_driver

173
ur_rtde_driver/src/ur/commander.cpp Normal file
View File

@@ -0,0 +1,173 @@
/*
* 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_rtde_driver/ur/commander.h"
#include "ur_rtde_driver/log.h"
namespace ur_driver
{
bool URCommander::write(const std::string& s)
{
size_t len = s.size();
const uint8_t* data = reinterpret_cast<const uint8_t*>(s.c_str());
size_t written;
return stream_.write(data, len, written);
}
void URCommander::formatArray(std::ostringstream& out, std::array<double, 6>& values)
{
std::string mod("[");
for (auto const& val : values)
{
out << mod << val;
mod = ",";
}
out << "]";
}
bool URCommander::uploadProg(const std::string& s)
{
LOG_DEBUG("Sending program [%s]", s.c_str());
return write(s);
}
bool URCommander::setToolVoltage(uint8_t voltage)
{
if (voltage != 0 || voltage != 12 || voltage != 24)
return false;
std::ostringstream out;
out << "set_tool_voltage(" << (int)voltage << ")\n";
std::string s(out.str());
return write(s);
}
bool URCommander::setFlag(uint8_t pin, bool value)
{
std::ostringstream out;
out << "set_flag(" << (int)pin << "," << (value ? "True" : "False") << ")\n";
std::string s(out.str());
return write(s);
}
bool URCommander::setPayload(double value)
{
std::ostringstream out;
out << "set_payload(" << std::fixed << std::setprecision(5) << value << ")\n";
std::string s(out.str());
return write(s);
}
bool URCommander::stopj(double a)
{
std::ostringstream out;
out << "stopj(" << std::fixed << std::setprecision(5) << a << ")\n";
std::string s(out.str());
return write(s);
}
bool URCommander_V1_X::speedj(std::array<double, 6>& speeds, double acceleration)
{
std::ostringstream out;
out << std::fixed << std::setprecision(5);
out << "speedj(";
formatArray(out, speeds);
out << "," << acceleration << "," << 0.02 << ")\n";
std::string s(out.str());
return write(s);
}
bool URCommander_V1_X::setAnalogOut(uint8_t pin, double value)
{
std::ostringstream out;
out << "sec io_fun():\n"
<< "set_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(4) << value << ")\n"
<< "end\n";
std::string s(out.str());
return write(s);
}
bool URCommander_V1_X::setDigitalOut(uint8_t pin, bool value)
{
std::ostringstream out;
out << "sec io_fun():\n"
<< "set_digital_out(" << (int)pin << "," << (value ? "True" : "False") << ")\n"
<< "end\n";
std::string s(out.str());
return write(s);
}
bool URCommander_V3_X::setAnalogOut(uint8_t pin, double value)
{
std::ostringstream out;
out << "sec io_fun():\n"
<< "set_standard_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(5) << value << ")\n"
<< "end\n";
std::string s(out.str());
return write(s);
}
bool URCommander_V3_X::setDigitalOut(uint8_t pin, bool value)
{
std::ostringstream out;
std::string func;
if (pin < 8)
{
func = "set_standard_digital_out";
}
else if (pin < 16)
{
func = "set_configurable_digital_out";
pin -= 8;
}
else if (pin < 18)
{
func = "set_tool_digital_out";
pin -= 16;
}
else
return false;
out << "sec io_fun():\n"
<< func << "(" << (int)pin << "," << (value ? "True" : "False") << ")\n"
<< "end\n";
std::string s(out.str());
return write(s);
}
bool URCommander_V3_1__2::speedj(std::array<double, 6>& speeds, double acceleration)
{
std::ostringstream out;
out << std::fixed << std::setprecision(5);
out << "speedj(";
formatArray(out, speeds);
out << "," << acceleration << ")\n";
std::string s(out.str());
return write(s);
}
bool URCommander_V3_3::speedj(std::array<double, 6>& speeds, double acceleration)
{
std::ostringstream out;
out << std::fixed << std::setprecision(5);
out << "speedj(";
formatArray(out, speeds);
out << "," << acceleration << "," << 0.008 << ")\n";
std::string s(out.str());
return write(s);
}
} // namespace ur_driver

124
ur_rtde_driver/src/ur/master_board.cpp Normal file
View File

@@ -0,0 +1,124 @@
/*
* 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_rtde_driver/ur/master_board.h"
#include "ur_rtde_driver/ur/consumer.h"
namespace ur_driver
{
bool SharedMasterBoardData::parseWith(BinParser& bp)
{
bp.parse(analog_input_range0);
bp.parse(analog_input_range1);
bp.parse(analog_input0);
bp.parse(analog_input1);
bp.parse(analog_output_domain0);
bp.parse(analog_output_domain1);
bp.parse(analog_output0);
bp.parse(analog_output1);
bp.parse(master_board_temperature);
bp.parse(robot_voltage_48V);
bp.parse(robot_current);
bp.parse(master_IO_current);
return true;
}
bool MasterBoardData_V1_X::parseWith(BinParser& bp)
{
if (!bp.checkSize<MasterBoardData_V1_X>())
return false;
bp.parse<uint16_t, 10>(digital_input_bits);
bp.parse<uint16_t, 10>(digital_output_bits);
SharedMasterBoardData::parseWith(bp);
bp.parse(master_safety_state);
bp.parse(master_on_off_state);
bp.parse(euromap67_interface_installed);
if (euromap67_interface_installed)
{
if (!bp.checkSize(MasterBoardData_V1_X::EURO_SIZE))
return false;
bp.parse(euromap_input_bits);
bp.parse(euromap_output_bits);
bp.parse(euromap_voltage);
bp.parse(euromap_current);
}
return true;
}
bool MasterBoardData_V3_0__1::parseWith(BinParser& bp)
{
if (!bp.checkSize<MasterBoardData_V3_0__1>())
return false;
bp.parse<uint32_t, 18>(digital_input_bits);
bp.parse<uint32_t, 18>(digital_output_bits);
SharedMasterBoardData::parseWith(bp);
bp.parse(safety_mode);
bp.parse(in_reduced_mode);
bp.parse(euromap67_interface_installed);
if (euromap67_interface_installed)
{
if (!bp.checkSize(MasterBoardData_V3_0__1::EURO_SIZE))
return false;
bp.parse(euromap_input_bits);
bp.parse(euromap_output_bits);
bp.parse(euromap_voltage);
bp.parse(euromap_current);
}
bp.consume(sizeof(uint32_t));
return true;
}
bool MasterBoardData_V3_2::parseWith(BinParser& bp)
{
if (!bp.checkSize<MasterBoardData_V3_2>())
return false;
MasterBoardData_V3_0__1::parseWith(bp);
bp.parse(operational_mode_selector_input);
bp.parse(three_position_enabling_device_input);
return true;
}
bool MasterBoardData_V1_X::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool MasterBoardData_V3_0__1::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool MasterBoardData_V3_2::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
} // namespace ur_driver

40
ur_rtde_driver/src/ur/messages.cpp Normal file
View File

@@ -0,0 +1,40 @@
/*
* 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_rtde_driver/ur/messages.h"
#include "ur_rtde_driver/ur/consumer.h"
namespace ur_driver
{
bool VersionMessage::parseWith(BinParser& bp)
{
bp.parse(project_name);
bp.parse(major_version);
bp.parse(minor_version);
bp.parse(svn_version);
bp.consume(sizeof(uint32_t)); // undocumented field??
bp.parseRemainder(build_date);
return true; // not really possible to check dynamic size packets
}
bool VersionMessage::consumeWith(URMessagePacketConsumer& consumer)
{
return consumer.consume(*this);
}
} // namespace ur_driver

105
ur_rtde_driver/src/ur/robot_mode.cpp Normal file
View File

@@ -0,0 +1,105 @@
/*
* 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_rtde_driver/ur/robot_mode.h"
#include "ur_rtde_driver/ur/consumer.h"
namespace ur_driver
{
bool SharedRobotModeData::parseWith(BinParser& bp)
{
bp.parse(timestamp);
bp.parse(physical_robot_connected);
bp.parse(real_robot_enabled);
bp.parse(robot_power_on);
bp.parse(emergency_stopped);
bp.parse(protective_stopped);
bp.parse(program_running);
bp.parse(program_paused);
return true;
}
bool RobotModeData_V1_X::parseWith(BinParser& bp)
{
if (!bp.checkSize<RobotModeData_V1_X>())
return false;
SharedRobotModeData::parseWith(bp);
bp.parse(robot_mode);
bp.parse(speed_fraction);
return true;
}
bool RobotModeData_V3_0__1::parseWith(BinParser& bp)
{
if (!bp.checkSize<RobotModeData_V3_0__1>())
return false;
SharedRobotModeData::parseWith(bp);
bp.parse(robot_mode);
bp.parse(control_mode);
bp.parse(target_speed_fraction);
bp.parse(speed_scaling);
return true;
}
bool RobotModeData_V3_2::parseWith(BinParser& bp)
{
if (!bp.checkSize<RobotModeData_V3_2>())
return false;
RobotModeData_V3_0__1::parseWith(bp);
bp.parse(target_speed_fraction_limit);
return true;
}
bool RobotModeData_V3_5::parseWith(BinParser& bp)
{
if (!bp.checkSize<RobotModeData_V3_5>())
return false;
RobotModeData_V3_2::parseWith(bp);
bp.parse(unknown_internal_use);
return true;
}
bool RobotModeData_V1_X::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool RobotModeData_V3_0__1::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool RobotModeData_V3_2::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool RobotModeData_V3_5::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
} // namespace ur_driver

138
ur_rtde_driver/src/ur/rt_state.cpp Normal file
View File

@@ -0,0 +1,138 @@
/*
* 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_rtde_driver/ur/rt_state.h"
#include "ur_rtde_driver/ur/consumer.h"
namespace ur_driver
{
void RTShared::parse_shared1(BinParser& bp)
{
bp.parse(time);
bp.parse(q_target);
bp.parse(qd_target);
bp.parse(qdd_target);
bp.parse(i_target);
bp.parse(m_target);
bp.parse(q_actual);
bp.parse(qd_actual);
bp.parse(i_actual);
}
void RTShared::parse_shared2(BinParser& bp)
{
bp.parse(digital_inputs);
bp.parse(motor_temperatures);
bp.parse(controller_time);
bp.consume(sizeof(double)); // Unused "Test value" field
bp.parse(robot_mode);
}
bool RTState_V1_6__7::parseWith(BinParser& bp)
{
if (!bp.checkSize<RTState_V1_6__7>())
return false;
parse_shared1(bp);
bp.parse(tool_accelerometer_values);
bp.consume(sizeof(double) * 15);
bp.parse(tcp_force);
bp.parse(tool_vector_actual);
bp.parse(tcp_speed_actual);
parse_shared2(bp);
return true;
}
bool RTState_V1_8::parseWith(BinParser& bp)
{
if (!bp.checkSize<RTState_V1_8>())
return false;
RTState_V1_6__7::parseWith(bp);
bp.parse(joint_modes);
return true;
}
bool RTState_V3_0__1::parseWith(BinParser& bp)
{
if (!bp.checkSize<RTState_V3_0__1>())
return false;
parse_shared1(bp);
bp.parse(i_control);
bp.parse(tool_vector_actual);
bp.parse(tcp_speed_actual);
bp.parse(tcp_force);
bp.parse(tool_vector_target);
bp.parse(tcp_speed_target);
parse_shared2(bp);
bp.parse(joint_modes);
bp.parse(safety_mode);
bp.consume(sizeof(double[6])); // skip undocumented
bp.parse(tool_accelerometer_values);
bp.consume(sizeof(double[6])); // skip undocumented
bp.parse(speed_scaling);
bp.parse(linear_momentum_norm);
bp.consume(sizeof(double)); // skip undocumented
bp.consume(sizeof(double)); // skip undocumented
bp.parse(v_main);
bp.parse(v_robot);
bp.parse(i_robot);
bp.parse(v_actual);
return true;
}
bool RTState_V3_2__3::parseWith(BinParser& bp)
{
if (!bp.checkSize<RTState_V3_2__3>())
return false;
RTState_V3_0__1::parseWith(bp);
bp.parse(digital_outputs);
bp.parse(program_state);
return true;
}
bool RTState_V1_6__7::consumeWith(URRTPacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool RTState_V1_8::consumeWith(URRTPacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool RTState_V3_0__1::consumeWith(URRTPacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool RTState_V3_2__3::consumeWith(URRTPacketConsumer& consumer)
{
return consumer.consume(*this);
}
} // namespace ur_driver

156
ur_rtde_driver/src/ur/ur_driver.cpp Normal file
View File

@@ -0,0 +1,156 @@
// 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_rtde_driver/ur/ur_driver.h"
#include <memory>
namespace ur_driver
{
static const std::string POSITION_PROGRAM = R"(
def myProg():
textmsg("hello")
MULT_jointstate = 1000000
SERVO_IDLE = 0
SERVO_RUNNING = 1
cmd_servo_state = SERVO_IDLE
cmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
def set_servo_setpoint(q):
enter_critical
cmd_servo_state = SERVO_RUNNING
cmd_servo_q = q
exit_critical
end
thread servoThread():
state = SERVO_IDLE
while True:
enter_critical
q = cmd_servo_q
do_brake = False
if (state == SERVO_RUNNING) and (cmd_servo_state == SERVO_IDLE):
do_brake = True
end
state = cmd_servo_state
cmd_servo_state = SERVO_IDLE
exit_critical
if do_brake:
stopj(1.0)
sync()
elif state == SERVO_RUNNING:
servoj(q, t=0.008, lookahead_time=0.03, gain=300)
else:
sync()
end
end
end
socket_open("192.168.56.1", 50001)
thread_servo = run servoThread()
keepalive = -2
params_mult = socket_read_binary_integer(6+1)
keepalive = params_mult[7]
while keepalive > 0:
params_mult = socket_read_binary_integer(6+1)
keepalive = params_mult[7]
if keepalive > 0:
if params_mult[0] > 0:
q = [params_mult[1] / MULT_jointstate, 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]
set_servo_setpoint(q)
end
end
end
sleep(.1)
socket_close()
kill thread_servo
end
)";
ur_driver::UrDriver::UrDriver(const std::string& ROBOT_IP) : rtde_frequency_(125) // conservative CB3 default.
{
ROS_INFO_STREAM("Initializing RTDE client");
rtde_client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_));
if (!rtde_client_->init())
{
throw std::runtime_error("initialization went wrong"); // TODO: be less harsh
}
comm::URStream<rtde_interface::PackageHeader> stream(ROBOT_IP, 30001);
stream.connect();
size_t len = POSITION_PROGRAM.size();
const uint8_t* data = reinterpret_cast<const uint8_t*>(POSITION_PROGRAM.c_str());
size_t written;
if (stream.write(data, len, written))
{
LOG_INFO("Sent program to robot");
}
else
{
LOG_ERROR("Could not send program to robot");
}
uint32_t reverse_port = 50001; // TODO: Make this a parameter
reverse_interface_.reset(new comm::ReverseInterface(reverse_port));
rtde_client_->start(); // TODO: Add extra start method (also to HW-Interface)
}
std::unique_ptr<rtde_interface::DataPackage> ur_driver::UrDriver::getDataPackage()
{
// TODO: This goes into the rtde_client
std::unique_ptr<comm::URPackage<rtde_interface::PackageHeader>> urpackage;
uint32_t period_ms = (1.0 / rtde_frequency_) * 1000;
std::chrono::milliseconds timeout(period_ms);
if (rtde_client_->getDataPackage(urpackage, timeout))
{
rtde_interface::DataPackage* tmp = dynamic_cast<rtde_interface::DataPackage*>(urpackage.get());
if (tmp != nullptr)
{
urpackage.release();
return std::move(std::unique_ptr<rtde_interface::DataPackage>(tmp));
}
}
return nullptr;
}
bool UrDriver::writeJointCommand(const vector6d_t& values)
{
if (reverse_interface_)
{
reverse_interface_->write(values);
}
else
{
return false;
}
return true;
}
} // namespace ur_driver

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