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

Merge pull request #120 from Zagitta/master

Browse Source 
ur_modern_driver refactor
This commit is contained in:
G.A. vd. Hoorn
2018-09-28 23:41:06 +02:00
committed by GitHub
parent 9397f47ac1 70e2c614aa
commit b4bb0d4a65
93 changed files with 7406 additions and 3576 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
.clang-format Normal file
View File

@@ -0,0 +1,48 @@
---
BasedOnStyle: Google
AccessModifierOffset: -2
ConstructorInitializerIndentWidth: 2
AlignEscapedNewlinesLeft: false
AlignTrailingComments: true
AllowAllParametersOfDeclarationOnNextLine: false
AllowShortIfStatementsOnASingleLine: false
AllowShortLoopsOnASingleLine: false
AllowShortFunctionsOnASingleLine: None
AllowShortLoopsOnASingleLine: false
AlwaysBreakTemplateDeclarations: true
AlwaysBreakBeforeMultilineStrings: false
BreakBeforeBinaryOperators: false
BreakBeforeTernaryOperators: false
BreakConstructorInitializersBeforeComma: true
BinPackParameters: true
ColumnLimit: 120
ConstructorInitializerAllOnOneLineOrOnePerLine: true
DerivePointerBinding: true
ExperimentalAutoDetectBinPacking: false
IndentCaseLabels: true
MaxEmptyLinesToKeep: 1
NamespaceIndentation: None
ObjCSpaceBeforeProtocolList: true
PenaltyBreakBeforeFirstCallParameter: 19
PenaltyBreakComment: 60
PenaltyBreakString: 1
PenaltyBreakFirstLessLess: 1000
PenaltyExcessCharacter: 1000
PenaltyReturnTypeOnItsOwnLine: 90
PointerBindsToType: false
SpacesBeforeTrailingComments: 2
Cpp11BracedListStyle: false
Standard: Auto
IndentWidth: 2
TabWidth: 2
UseTab: Never
BreakBeforeBraces: Allman
IndentFunctionDeclarationAfterType: false
SpacesInParentheses: false
SpacesInAngles: false
SpaceInEmptyParentheses: false
SpacesInCStyleCastParentheses: false
SpaceAfterControlStatementKeyword: true
SpaceBeforeAssignmentOperators: true
ContinuationIndentWidth: 4
...

40
.github/issue_template.md vendored Normal file
View File

@@ -0,0 +1,40 @@
Your issue may already be reported!
Please search on the [issue track](../) before creating one.
## Expected Behavior
<!--- If you're describing a bug, tell us what should happen -->
<!--- If you're suggesting a change/improvement, tell us how it should work -->
## Current Behavior
<!--- If describing a bug, tell us what happens instead of the expected behavior -->
<!--- If suggesting a change/improvement, explain the difference from current behavior -->
## Possible Solution
<!--- Not obligatory, but suggest a fix/reason for the bug, -->
<!--- or ideas how to implement the addition or change -->
## Steps to Reproduce (for bugs)
<!--- Provide a link to a live example, or an unambiguous set of steps to -->
<!--- reproduce this bug. Include code to reproduce, if relevant -->
1.
2.
3.
4.
## Context
<!--- How has this issue affected you? What are you trying to accomplish? -->
<!--- Providing context helps us come up with a solution that is most useful in the real world -->
## Your Environment
<!--- Include as many relevant details about the environment you experienced the bug in -->
* Operating System and version:
* Platform (ie: PC or embedded arm):
* Version used:
* ROS version:
* UR CB version:
* Connection setup (wired or wireless):
## Log file
```
Paste your DEBUG logfile content here
```

1
.gitignore vendored
View File

@@ -4,3 +4,4 @@ Makefile
cmake_install.cmake
install_manifest.txt
*~
.idea

20
.travis.yml Normal file
View File

@@ -0,0 +1,20 @@
# This config file for Travis CI utilizes ros-industrial/industrial_ci package.
# For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst
sudo: required
dist: trusty
services:
- docker
language: generic
compiler:
- gcc
env:
matrix:
- ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true
- ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true
- ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_INSTALL=true
- ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu NOT_TEST_INSTALL=true
install:
- git clone https://github.com/ros-industrial/industrial_ci.git .ci_config
script:
- source .ci_config/travis.sh
# - source ./travis.sh # Enable this when you have a package-local script

72
CMakeLists.txt
View File

@@ -1,6 +1,10 @@
cmake_minimum_required(VERSION 2.8.3)
cmake_minimum_required(VERSION 2.8.12)
project(ur_modern_driver)
add_definitions( -DROS_BUILD )
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
@@ -10,9 +14,10 @@ find_package(catkin REQUIRED COMPONENTS
actionlib
control_msgs
geometry_msgs
industrial_msgs
roscpp
sensor_msgs
std_msgs
std_srvs
trajectory_msgs
ur_msgs
tf
@@ -109,9 +114,8 @@ find_package(catkin REQUIRED COMPONENTS
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
# LIBRARIES ur_modern_driver
LIBRARIES ur_hardware_interface
CATKIN_DEPENDS hardware_interface controller_manager actionlib control_msgs geometry_msgs roscpp sensor_msgs trajectory_msgs ur_msgs
DEPENDS ur_hardware_interface
)
###########
@@ -123,13 +127,22 @@ 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)
set(CMAKE_CXX_FLAGS "-std=c++11")
add_compile_options(-std=c++11)
elseif(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "-std=c++0x")
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)
# support indigo's ros_control - This can be removed upon EOL indigo
if("${controller_manager_msgs_VERSION}" VERSION_LESS "0.10.0")
add_definitions(-DUR_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL)
endif()
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
@@ -140,7 +153,9 @@ include_directories(include
## Declare a C++ library
# Hardware Interface
add_library(ur_hardware_interface src/ur_hardware_interface.cpp)
add_library(ur_hardware_interface
src/ros/hardware_interface.cpp
src/ros/controller.cpp)
target_link_libraries(ur_hardware_interface
${catkin_LIBRARIES}
)
@@ -152,18 +167,27 @@ target_link_libraries(ur_hardware_interface
## Declare a C++ executable
set(${PROJECT_NAME}_SOURCES
src/ur_ros_wrapper.cpp
src/ur_driver.cpp
src/ur_realtime_communication.cpp
src/ur_communication.cpp
src/robot_state.cpp
src/robot_state_RT.cpp
src/do_output.cpp)
add_executable(ur_driver ${${PROJECT_NAME}_SOURCES})
src/ros/action_server.cpp
src/ros/mb_publisher.cpp
src/ros/rt_publisher.cpp
src/ros/service_stopper.cpp
src/ros/trajectory_follower.cpp
src/ros/lowbandwidth_trajectory_follower.cpp
src/ros/urscript_handler.cpp
src/ur/stream.cpp
src/ur/server.cpp
src/ur/commander.cpp
src/ur/robot_mode.cpp
src/ur/master_board.cpp
src/ur/rt_state.cpp
src/ur/messages.cpp
src/tcp_socket.cpp)
add_executable(ur_driver ${${PROJECT_NAME}_SOURCES} src/ros_main.cpp)
## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(ur_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(ur_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(ur_driver
@@ -201,11 +225,13 @@ install(DIRECTORY include/${PROJECT_NAME}/
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_ur_modern_driver.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
set(${PROJECT_NAME}_TEST_SOURCES
tests/ur/rt_state.cpp
tests/ur/master_board.cpp
tests/ur/robot_mode.cpp)
if (CATKIN_ENABLE_TESTING)
catkin_add_gtest(ur_modern_driver_test ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_TEST_SOURCES} tests/main.cpp)
target_link_libraries(ur_modern_driver_test ur_hardware_interface ${catkin_LIBRARIES})
endif()

67
README.md
View File

@@ -1,6 +1,7 @@
# ur_modern_driver
# ur_modern_driver - Refactored
[![Build Status](https://travis-ci.org/Zagitta/ur_modern_driver.svg?branch=master)](https://travis-ci.org/Zagitta/ur_modern_driver)
A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is designed to replace the old driver transparently, while solving some issues, improving usability as well as enabling compatibility of ros_control.
A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is designed to replace the old driver transparently, while solving some issues, improving usability as well as enabling compatibility of ros_control.
## Improvements
@@ -21,12 +22,12 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design
* Besides this, the driver subscribes to two new topics:
* */ur\_driver/URScript* : Takes messages of type _std\_msgs/String_ and directly forwards it to the robot. Note that no control is done on the input, so use at your own risk! Inteded for sending movel/movej commands directly to the robot, conveyor tracking and the like.
* */ur\_driver/URScript* : Takes messages of type _std\_msgs/String_ and directly forwards it to the robot. Note that no control is done on the input, so use at your own risk! Intended for sending movel/movej commands directly to the robot, conveyor tracking and the like.
* */joint\_speed* : Takes messages of type _trajectory\_msgs/JointTrajectory_. Parses the first JointTracetoryPoint and sends the specified joint speeds and accelerations to the robot. This interface is intended for doing visual servoing and other kind of control that requires speed control rather than position control of the robot. Remember to set values for all 6 joints. Ignores the field joint\_names, so set the values in the correct order.
* */joint\_speed* : Takes messages of type _trajectory\_msgs/JointTrajectory_. Parses the first JointTrajectoryPoint and sends the specified joint speeds and accelerations to the robot. This interface is intended for doing visual servoing and other kind of control that requires speed control rather than position control of the robot. Remember to set values for all 6 joints. Ignores the field joint\_names, so set the values in the correct order.
* Added support for ros_control.
* As ros_control wants to have control over the robot at all times, ros_control compatibility is set via a parameter at launch-time.
* Added support for ros_control.
* As ros_control wants to have control over the robot at all times, ros_control compatibility is set via a parameter at launch-time.
* With ros_control active, the driver doesn't open the action_lib interface nor publish joint_states or wrench msgs. This is handled by ros_control instead.
* Currently two controllers are available, both controlling the joint position of the robot, useable for trajectroy execution
* The velocity based controller sends joint speed commands to the robot, using the speedj command
@@ -35,7 +36,41 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design
* As ros_control continuesly controls the robot, using the teach pendant while a controller is running will cause the controller **on the robot** to crash, as it obviously can't handle conflicting control input from two sources. Thus be sure to stop the running controller **before** moving the robot via the teach pendant:
* A list of the loaded and running controllers can be found by a call to the controller_manager ```rosservice call /controller_manager/list_controllers {} ```
* The running position trajectory controller can be stopped with a call to ```rosservice call /universal_robot/controller_manager/switch_controller "start_controllers: - '' stop_controllers: - 'pos_based_pos_traj_controller' strictness: 1" ``` (Remember you can use tab-completion for this)
* Added activation modes: `Never`, `Always` and `OnStartup`. Sets wether a call to the `ur_driver/robot_enable` service is required at startup, at startup + on errors or never. Is intended as a safety feature to require some sort of manual intervention in case of driver crashes and robot safety faults.
* **Low Bandwidth Trajectory Follower** mode of execution. (only works when `ros_control` is set to `false`)In this mode the real-time control loop for the robot is shifted from the client PC running the driver to URscript executed on the URControl PC of the robot with the following features:
* It works only with */follow\_joint\_trajectory* action for MoveIt integration. It is mutually exclusive with *ros_control*
* It only implements "position + velocity" based control - it uses coarse positions and velocities calculated by MoveIt and performs cubic interpolation of joint positions (Bezier' curve) of positions. The positions are set using servoj command of URScript.
* It is much more resilient to connectivity problems than other methods of ur_modern_driver. It is resilient to additional load on client PC, latency of network and kernel of the PC the driver runs on. This makes it much better suitable for development/research quick iteration loops without separate dedicated physical setup.
* Other methods of controlling the robot by ur_modern_driver are very fragile and require low-latency kernel, separated wired network and ideally no other network activity on the PC where ur_modern_driver runs.
* **Low Bandwidth Trajectory Follower** will never make dangerous "catch-up" when move is delayed. Other methods prioritise execution time of a move which sometimes might lead to situation where robot gets out of control and speeds up to catch-up - often resulting in Protective Stop. For Low Bandwith Trajectory Follower predictability of the move has priority over move execution time.
* The amount of TCP/IP communication between the driver and UR robot (URControl PC) is two orders of magnitude (around 100x) less with **Low Bandwidth Trajectory Follower** than with other methods- coarse MoveIt generated trajectory is sent to robot and cubic interpolation of the move is calculated by the URScript program run on URControl PC.
* Due to communication optimisations and **Low Bandwidth Trajectory Follower** works reliably even over WiFi connection (!) which is impossible for other methods.
* Time flow for the URScript program might be independent from "real time" and can be controlled to speed up or slow down execution of planned moves.
* The Low Bandwidth Trajectory Follower requires 3.0+ version firmware of the robot (servoj command must support lookahead_time and servoj_gain parameters)
* There are several parameters that you can use to control Low Bandwidth Trajectory Follower's behaviour:
* **use_lowbandwidth_trajectory_follower** - should be set to `true` to enable the follower
* **time_interval** - time interval (in seconds) this is 'simulated' time for interpolation steps of the move. Together with *servoj_time* it can be used to change speed of moves even after they are planned by MoveIt. Default value is 0.008
* **servoj_time** - time interval (real time) for which each interpolation step controls the robot (using servoj command). See below on examples of setting different time parameters. Default value is 0.008 (corresponds to expected 125Hz frequency of UR robot control)
* **servoj_time_waiting** - time in seconds (real time) of internal active loop while the robot waits for new instructions in case of delays in communication. The smaller value, the faster robot restarts move after delay (but more stress is put on URControl processor). Default value is 0.001 (1000 Hz check frequency)
* **max_waiting_time** - maximum time in seconds (real time) to wait for instructions from the drive before move is aborted. Defaults to 2 seconds.
* **servoj_gain** and **servoj_lookahead_time** - useful to control precision and speed of the position moves with servoj command (see URScript documentation for detailes)
* **max_joint_difference** - maximum allowed difference between target and actual joints - checked at every trajectory step
Here are some examples of manipulating the time flow for **Low Bandwidth Trajectory Follower** mode. You can use other settings but you should do it on your own risk.
* Default mode: *servoj_time* = 0.008, *time_interval* = 0.008 : interpolation time flows with the same speed as real time - moves are executed as planned
* Slow-motion mode: *servoj_time* = 0.008, *time_interval* = 0.004 : interpolation time flows 2x slower than real time, so the move is executed 2x slower than planned. Requires configuring MoveIt to accept much slower moves than expected (otherwise MoveIt cancels such move mid-way)
* Fast-forward mode: *servoj_time* = 0.004, *time_interval* = 0.012 : interpolation time flows 3x faster than real time, so the move is 3x faster than planned. Might violate limits of the robot speed so use carefully and make sure you gradually increase the speed in-between to check how safe it is.
NOTE! In case you use Low Bandwidth Trajectory Follower and you experience MoveIt to cancel robot moves prematurely
because of too long move duration, you should increase tolerance of duration monitoring of MoveIt trajectory execution
You can find the configuration usually in trajectory_execution.launch.xml in generated moveit config - there are
parameters that configure scaling and margin for allowed execution time among others.
The relevant parameters are `trajectory_execution/allowed_execution_duration_scaling` (default 1.2) and
`trajectory_execution/allowed_goal_duration_margin` (default 0.5). The first one is factor that scales execution time,
the second is margin that is added on top of the scaled one. You can increase either of those values to make moveit
executor more "tolerant" to execution delays. There is also another parameter:
`trajectory_execution/execution_duration_monitoring`. You can set it to false to disable duration monitoring completely.
## Installation
@@ -87,7 +122,7 @@ The position based controller *should* stay closer to the commanded path, while
**Note** that the PID values are not optimally tweaked as of this moment.
To use ros_control together with MoveIt, be sure to add the desired controller to the ```controllers.yaml``` in the urXX_moveit_config/config folder. Add the following
To use ros_control together with MoveIt, be sure to add the desired controller to the ```controllers.yaml``` in the urXX_moveit_config/config folder. Add the following
```
controller_list:
- name: /vel_based_pos_traj_controller #or /pos_based_pos_traj_controller
@@ -108,7 +143,7 @@ controller_list:
Each robot from UR is calibrated individually, so there is a small error (in the order of millimeters) between the end-effector reported by the URDF models in https://github.com/ros-industrial/universal_robot/tree/indigo-devel/ur_description and
the end-effector as reported by the controller itself.
This driver broadcasts a transformation between the base link and the end-effector as reported by the UR. The default frame names are: *base* and *tool0_controller*.
This driver broadcasts a transformation between the base link and the end-effector as reported by the UR. The default frame names are: *base* and *tool0_controller*.
To use the *tool0_controller* frame in a URDF, there needs to be a link with that name connected to *base*. For example:
@@ -146,16 +181,16 @@ Should be compatible with all robots and control boxes with the newest firmware.
*Simulated UR5 running 1.6.08725
#Credits
Please cite the following report if using this driver
@techreport{andersen2015optimizing,
title = {Optimizing the Universal Robots ROS driver.},
institution = {Technical University of Denmark, Department of Electrical Engineering},
author = {Andersen, Thomas Timm},
year = {2015},
url = {http://orbit.dtu.dk/en/publications/optimizing-the-universal-robots-ros-driver(20dde139-7e87-4552-8658-dbf2cdaab24b).html}
@techreport{andersen2015optimizing,
title = {Optimizing the Universal Robots ROS driver.},
institution = {Technical University of Denmark, Department of Electrical Engineering},
author = {Andersen, Thomas Timm},
year = {2015},
url = {http://orbit.dtu.dk/en/publications/optimizing-the-universal-robots-ros-driver(20dde139-7e87-4552-8658-dbf2cdaab24b).html}
}

185
include/ur_modern_driver/bin_parser.h Normal file
View File

@@ -0,0 +1,185 @@
#pragma once
#include <assert.h>
#include <endian.h>
#include <inttypes.h>
#include <array>
#include <bitset>
#include <cstddef>
#include <cstring>
#include <string>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/types.h"
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);
}
// Explicit parsing order of fields to avoid issues with struct layout
void parse(cartesian_coord_t& val)
{
parse(val.position);
parse(val.rotation);
}
void parse_remainder(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_);
}
};

34
include/ur_modern_driver/do_output.h
View File

@@ -1,34 +0,0 @@
/*
* do_output.h
*
* Copyright 2015 Thomas Timm Andersen
*
* 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.
*/
#ifndef UR_DO_OUTPUT_H_
#define UR_DO_OUTPUT_H_
#ifdef ROS_BUILD
#include <ros/ros.h>
#endif
#include <string>
void print_debug(std::string inp);
void print_info(std::string inp);
void print_warning(std::string inp);
void print_error(std::string inp);
void print_fatal(std::string inp);
#endif /* UR_DO_OUTPUT_H_ */

84
include/ur_modern_driver/event_counter.h Normal file
View File

@@ -0,0 +1,84 @@
#pragma once
#include <chrono>
#include <cstdlib>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/consumer.h"
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()
{
}
};

21
include/ur_modern_driver/log.h Normal file
View File

@@ -0,0 +1,21 @@
#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

215
include/ur_modern_driver/pipeline.h Normal file
View File

@@ -0,0 +1,215 @@
#pragma once
#include <atomic>
#include <chrono>
#include <thread>
#include <vector>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/queue/readerwriterqueue.h"
using namespace moodycamel;
using namespace std;
template <typename T>
class IConsumer
{
public:
virtual void setupConsumer()
{
}
virtual void teardownConsumer()
{
}
virtual void stopConsumer()
{
}
virtual void onTimeout()
{
}
virtual bool consume(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(shared_ptr<T> product)
{
bool res = true;
for (auto& con : consumers_)
{
if (!con->consume(product))
res = false;
}
return res;
}
};
template <typename T>
class IProducer
{
public:
virtual void setupProducer()
{
}
virtual void teardownProducer()
{
}
virtual void stopProducer()
{
}
virtual bool tryGet(std::vector<unique_ptr<T>>& products) = 0;
};
class INotifier
{
public:
virtual void started(std::string name)
{
}
virtual void stopped(std::string name)
{
}
};
template <typename T>
class Pipeline
{
private:
typedef std::chrono::high_resolution_clock Clock;
typedef Clock::time_point Time;
IProducer<T>& producer_;
IConsumer<T>& consumer_;
std::string name_;
INotifier& notifier_;
BlockingReaderWriterQueue<unique_ptr<T>> queue_;
atomic<bool> running_;
thread pThread_, cThread_;
void run_producer()
{
producer_.setupProducer();
std::vector<unique_ptr<T>> 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());
consumer_.stopConsumer();
running_ = false;
notifier_.stopped(name_);
}
void run_consumer()
{
consumer_.setupConsumer();
unique_ptr<T> 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_);
}
public:
Pipeline(IProducer<T>& producer, IConsumer<T>& consumer, std::string name, INotifier& notifier)
: producer_(producer), consumer_(consumer), name_(name), notifier_(notifier), queue_{ 32 }, running_{ false }
{
}
void run()
{
if (running_)
return;
running_ = true;
pThread_ = thread(&Pipeline::run_producer, this);
cThread_ = thread(&Pipeline::run_consumer, this);
notifier_.started(name_);
}
void stop()
{
if (!running_)
return;
LOG_DEBUG("Stopping pipeline! <%s>",name_.c_str());
consumer_.stopConsumer();
producer_.stopProducer();
running_ = false;
pThread_.join();
cThread_.join();
notifier_.stopped(name_);
}
};

28
include/ur_modern_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
include/ur_modern_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
include/ur_modern_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

218
include/ur_modern_driver/robot_state.h
View File

@@ -1,218 +0,0 @@
/*
* robot_state.h
*
* Copyright 2015 Thomas Timm Andersen
*
* 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.
*/
#ifndef ROBOT_STATE_H_
#define ROBOT_STATE_H_
#include <inttypes.h>
#include <vector>
#include <stdlib.h>
#include <string.h>
#include <mutex>
#include <condition_variable>
#include <netinet/in.h>
namespace message_types {
enum message_type {
ROBOT_STATE = 16, ROBOT_MESSAGE = 20, PROGRAM_STATE_MESSAGE = 25
};
}
typedef message_types::message_type messageType;
namespace package_types {
enum package_type {
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
};
}
typedef package_types::package_type packageType;
namespace robot_message_types {
enum robot_message_type {
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
};
}
typedef robot_message_types::robot_message_type robotMessageType;
namespace robot_state_type_v18 {
enum robot_state_type {
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
};
}
typedef robot_state_type_v18::robot_state_type robotStateTypeV18;
namespace robot_state_type_v30 {
enum robot_state_type {
ROBOT_MODE_DISCONNECTED = 0,
ROBOT_MODE_CONFIRM_SAFETY = 1,
ROBOT_MODE_BOOTING = 2,
ROBOT_MODE_POWER_OFF = 3,
ROBOT_MODE_POWER_ON = 4,
ROBOT_MODE_IDLE = 5,
ROBOT_MODE_BACKDRIVE = 6,
ROBOT_MODE_RUNNING = 7,
ROBOT_MODE_UPDATING_FIRMWARE = 8
};
}
typedef robot_state_type_v30::robot_state_type robotStateTypeV30;
struct version_message {
uint64_t timestamp;
int8_t source;
int8_t robot_message_type;
int8_t project_name_size;
char project_name[15];
uint8_t major_version;
uint8_t minor_version;
int svn_revision;
char build_date[25];
};
struct masterboard_data {
int digitalInputBits;
int digitalOutputBits;
char analogInputRange0;
char analogInputRange1;
double analogInput0;
double analogInput1;
char analogOutputDomain0;
char analogOutputDomain1;
double analogOutput0;
double analogOutput1;
float masterBoardTemperature;
float robotVoltage48V;
float robotCurrent;
float masterIOCurrent;
unsigned char safetyMode;
unsigned char masterOnOffState;
char euromap67InterfaceInstalled;
int euromapInputBits;
int euromapOutputBits;
float euromapVoltage;
float euromapCurrent;
};
struct robot_mode_data {
uint64_t timestamp;
bool isRobotConnected;
bool isRealRobotEnabled;
bool isPowerOnRobot;
bool isEmergencyStopped;
bool isProtectiveStopped;
bool isProgramRunning;
bool isProgramPaused;
unsigned char robotMode;
unsigned char controlMode;
double targetSpeedFraction;
double speedScaling;
};
class RobotState {
private:
version_message version_msg_;
masterboard_data mb_data_;
robot_mode_data robot_mode_;
std::recursive_mutex val_lock_; // Locks the variables while unpack parses data;
std::condition_variable* pMsg_cond_; //Signals that new vars are available
bool new_data_available_; //to avoid spurious wakes
unsigned char robot_mode_running_;
double ntohd(uint64_t nf);
public:
RobotState(std::condition_variable& msg_cond);
~RobotState();
double getVersion();
double getTime();
std::vector<double> getQTarget();
int getDigitalInputBits();
int getDigitalOutputBits();
char getAnalogInputRange0();
char getAnalogInputRange1();
double getAnalogInput0();
double getAnalogInput1();
char getAnalogOutputDomain0();
char getAnalogOutputDomain1();
double getAnalogOutput0();
double getAnalogOutput1();
std::vector<double> getVActual();
float getMasterBoardTemperature();
float getRobotVoltage48V();
float getRobotCurrent();
float getMasterIOCurrent();
unsigned char getSafetyMode();
unsigned char getInReducedMode();
char getEuromap67InterfaceInstalled();
int getEuromapInputBits();
int getEuromapOutputBits();
float getEuromapVoltage();
float getEuromapCurrent();
bool isRobotConnected();
bool isRealRobotEnabled();
bool isPowerOnRobot();
bool isEmergencyStopped();
bool isProtectiveStopped();
bool isProgramRunning();
bool isProgramPaused();
unsigned char getRobotMode();
bool isReady();
void setDisconnected();
bool getNewDataAvailable();
void finishedReading();
void unpack(uint8_t * buf, unsigned int buf_length);
void unpackRobotMessage(uint8_t * buf, unsigned int offset, uint32_t len);
void unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
uint32_t len);
void unpackRobotState(uint8_t * buf, unsigned int offset, uint32_t len);
void unpackRobotStateMasterboard(uint8_t * buf, unsigned int offset);
void unpackRobotMode(uint8_t * buf, unsigned int offset);
};
#endif /* ROBOT_STATE_H_ */

116
include/ur_modern_driver/robot_state_RT.h
View File

@@ -1,116 +0,0 @@
/*
* robotStateRT.h
*
* Copyright 2015 Thomas Timm Andersen
*
* 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.
*/
#ifndef ROBOT_STATE_RT_H_
#define ROBOT_STATE_RT_H_
#include <inttypes.h>
#include <vector>
#include <stdlib.h>
#include <string.h>
#include <mutex>
#include <netinet/in.h>
#include <condition_variable>
class RobotStateRT {
private:
double version_; //protocol version
double time_; //Time elapsed since the controller was started
std::vector<double> q_target_; //Target joint positions
std::vector<double> qd_target_; //Target joint velocities
std::vector<double> qdd_target_; //Target joint accelerations
std::vector<double> i_target_; //Target joint currents
std::vector<double> m_target_; //Target joint moments (torques)
std::vector<double> q_actual_; //Actual joint positions
std::vector<double> qd_actual_; //Actual joint velocities
std::vector<double> i_actual_; //Actual joint currents
std::vector<double> i_control_; //Joint control currents
std::vector<double> tool_vector_actual_; //Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
std::vector<double> tcp_speed_actual_; //Actual speed of the tool given in Cartesian coordinates
std::vector<double> tcp_force_; //Generalised forces in the TC
std::vector<double> tool_vector_target_; //Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
std::vector<double> tcp_speed_target_; //Target speed of the tool given in Cartesian coordinates
std::vector<bool> digital_input_bits_; //Current state of the digital inputs. NOTE: these are bits encoded as int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high
std::vector<double> motor_temperatures_; //Temperature of each joint in degrees celsius
double controller_timer_; //Controller realtime thread execution time
double robot_mode_; //Robot mode
std::vector<double> joint_modes_; //Joint control modes
double safety_mode_; //Safety mode
std::vector<double> tool_accelerometer_values_; //Tool x,y and z accelerometer values (software version 1.7)
double speed_scaling_; //Speed scaling of the trajectory limiter
double linear_momentum_norm_; //Norm of Cartesian linear momentum
double v_main_; //Masterboard: Main voltage
double v_robot_; //Matorborad: Robot voltage (48V)
double i_robot_; //Masterboard: Robot current
std::vector<double> v_actual_; //Actual joint voltages
std::mutex val_lock_; // Locks the variables while unpack parses data;
std::condition_variable* pMsg_cond_; //Signals that new vars are available
bool data_published_; //to avoid spurious wakes
bool controller_updated_; //to avoid spurious wakes
std::vector<double> unpackVector(uint8_t * buf, int start_index,
int nr_of_vals);
std::vector<bool> unpackDigitalInputBits(int64_t data);
double ntohd(uint64_t nf);
public:
RobotStateRT(std::condition_variable& msg_cond);
~RobotStateRT();
double getVersion();
double getTime();
std::vector<double> getQTarget();
std::vector<double> getQdTarget();
std::vector<double> getQddTarget();
std::vector<double> getITarget();
std::vector<double> getMTarget();
std::vector<double> getQActual();
std::vector<double> getQdActual();
std::vector<double> getIActual();
std::vector<double> getIControl();
std::vector<double> getToolVectorActual();
std::vector<double> getTcpSpeedActual();
std::vector<double> getTcpForce();
std::vector<double> getToolVectorTarget();
std::vector<double> getTcpSpeedTarget();
std::vector<bool> getDigitalInputBits();
std::vector<double> getMotorTemperatures();
double getControllerTimer();
double getRobotMode();
std::vector<double> getJointModes();
double getSafety_mode();
std::vector<double> getToolAccelerometerValues();
double getSpeedScaling();
double getLinearMomentumNorm();
double getVMain();
double getVRobot();
double getIRobot();
void setVersion(double ver);
void setDataPublished();
bool getDataPublished();
bool getControllerUpdated();
void setControllerUpdated();
std::vector<double> getVActual();
void unpack(uint8_t * buf);
};
#endif /* ROBOT_STATE_RT_H_ */

73
include/ur_modern_driver/ros/action_server.h Normal file
View File

@@ -0,0 +1,73 @@
#pragma once
#include <actionlib/server/action_server.h>
#include <actionlib/server/server_goal_handle.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <ros/ros.h>
#include <atomic>
#include <chrono>
#include <condition_variable>
#include <mutex>
#include <set>
#include <thread>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ros/service_stopper.h"
#include "ur_modern_driver/ros/trajectory_follower.h"
#include "ur_modern_driver/ur/consumer.h"
#include "ur_modern_driver/ur/master_board.h"
#include "ur_modern_driver/ur/state.h"
class ActionServer : public Service, public URRTPacketConsumer
{
private:
typedef control_msgs::FollowJointTrajectoryAction Action;
typedef control_msgs::FollowJointTrajectoryResult Result;
typedef actionlib::ServerGoalHandle<Action> GoalHandle;
typedef actionlib::ActionServer<Action> Server;
ros::NodeHandle nh_;
Server as_;
std::vector<std::string> joint_names_;
std::set<std::string> joint_set_;
double max_velocity_;
GoalHandle curr_gh_;
std::atomic<bool> interrupt_traj_;
std::atomic<bool> has_goal_, running_;
std::mutex tj_mutex_;
std::condition_variable tj_cv_;
std::thread tj_thread_;
ActionTrajectoryFollowerInterface& follower_;
RobotState state_;
std::array<double, 6> q_actual_, qd_actual_;
void onGoal(GoalHandle gh);
void onCancel(GoalHandle gh);
bool validate(GoalHandle& gh, Result& res);
bool validateState(GoalHandle& gh, Result& res);
bool validateJoints(GoalHandle& gh, Result& res);
bool validateTrajectory(GoalHandle& gh, Result& res);
bool try_execute(GoalHandle& gh, Result& res);
void interruptGoal(GoalHandle& gh);
std::vector<size_t> reorderMap(std::vector<std::string> goal_joints);
void trajectoryThread();
bool updateState(RTShared& data);
public:
ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names, double max_velocity);
void start();
virtual void onRobotStateChange(RobotState state);
virtual bool consume(RTState_V1_6__7& state);
virtual bool consume(RTState_V1_8& state);
virtual bool consume(RTState_V3_0__1& state);
virtual bool consume(RTState_V3_2__3& state);
};

32
include/ur_modern_driver/ros/action_trajectory_follower_interface.h Normal file
View File

@@ -0,0 +1,32 @@
#pragma once
#include <inttypes.h>
#include <array>
#include <atomic>
#include <cstddef>
#include <vector>
struct TrajectoryPoint
{
std::array<double, 6> positions;
std::array<double, 6> velocities;
std::chrono::microseconds time_from_start;
TrajectoryPoint()
{
}
TrajectoryPoint(std::array<double, 6> &pos, std::array<double, 6> &vel, std::chrono::microseconds tfs)
: positions(pos), velocities(vel), time_from_start(tfs)
{
}
};
class ActionTrajectoryFollowerInterface
{
public:
virtual bool start() = 0;
virtual bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt) = 0;
virtual void stop() = 0;
virtual ~ActionTrajectoryFollowerInterface() {};
};

91
include/ur_modern_driver/ros/controller.h Normal file
View File

@@ -0,0 +1,91 @@
#pragma once
#include <controller_manager/controller_manager.h>
#include <hardware_interface/force_torque_sensor_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>
#include <ros/ros.h>
#include <atomic>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ros/hardware_interface.h"
#include "ur_modern_driver/ros/service_stopper.h"
#include "ur_modern_driver/ur/commander.h"
#include "ur_modern_driver/ur/consumer.h"
#include "ur_modern_driver/ur/rt_state.h"
class ROSController : private hardware_interface::RobotHW, public URRTPacketConsumer, public Service
{
private:
ros::NodeHandle nh_;
ros::Time lastUpdate_;
controller_manager::ControllerManager controller_;
// state interfaces
JointInterface joint_interface_;
WrenchInterface wrench_interface_;
// controller interfaces
PositionInterface position_interface_;
VelocityInterface velocity_interface_;
// currently activated controller
HardwareInterface* active_interface_;
// map of switchable controllers
std::map<std::string, HardwareInterface*> available_interfaces_;
std::atomic<bool> service_enabled_;
std::atomic<uint32_t> service_cooldown_;
// helper functions to map interfaces
template <typename T>
void registerInterface(T* interface)
{
RobotHW::registerInterface<typename T::parent_type>(interface);
}
template <typename T>
void registerControllereInterface(T* interface)
{
registerInterface(interface);
available_interfaces_[T::INTERFACE_NAME] = interface;
}
void read(RTShared& state);
bool update();
bool write();
void reset();
public:
ROSController(URCommander& commander, TrajectoryFollower& follower, std::vector<std::string>& joint_names,
double max_vel_change, std::string tcp_link);
virtual ~ROSController()
{
}
// from RobotHW
void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list);
// from URRTPacketConsumer
virtual void setupConsumer();
virtual bool consume(RTState_V1_6__7& state)
{
read(state);
return update();
}
virtual bool consume(RTState_V1_8& state)
{
read(state);
return update();
}
virtual bool consume(RTState_V3_0__1& state)
{
read(state);
return update();
}
virtual bool consume(RTState_V3_2__3& state)
{
read(state);
return update();
}
virtual void onTimeout();
virtual void onRobotStateChange(RobotState state);
};

83
include/ur_modern_driver/ros/hardware_interface.h Normal file
View File

@@ -0,0 +1,83 @@
#pragma once
#include <controller_manager/controller_manager.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_modern_driver/ros/trajectory_follower.h"
#include "ur_modern_driver/ur/commander.h"
#include "ur_modern_driver/ur/rt_state.h"
class HardwareInterface
{
public:
virtual bool write() = 0;
virtual void start()
{
}
virtual void stop()
{
}
virtual void reset()
{
}
};
using hardware_interface::JointHandle;
class JointInterface : public hardware_interface::JointStateInterface
{
private:
std::array<double, 6> velocities_, positions_, efforts_;
public:
JointInterface(std::vector<std::string> &joint_names);
void update(RTShared &packet);
typedef hardware_interface::JointStateInterface parent_type;
static const std::string INTERFACE_NAME;
};
class WrenchInterface : public hardware_interface::ForceTorqueSensorInterface
{
std::array<double, 6> tcp_;
public:
WrenchInterface(std::string tcp_link);
void update(RTShared &packet);
typedef hardware_interface::ForceTorqueSensorInterface parent_type;
static const std::string INTERFACE_NAME;
};
class VelocityInterface : public HardwareInterface, public hardware_interface::VelocityJointInterface
{
private:
URCommander &commander_;
std::array<double, 6> velocity_cmd_, prev_velocity_cmd_;
double max_vel_change_;
public:
VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface,
std::vector<std::string> &joint_names, double max_vel_change);
virtual bool write();
virtual void reset();
typedef hardware_interface::VelocityJointInterface parent_type;
static const std::string INTERFACE_NAME;
};
class PositionInterface : public HardwareInterface, public hardware_interface::PositionJointInterface
{
private:
TrajectoryFollower &follower_;
std::array<double, 6> position_cmd_;
public:
PositionInterface(TrajectoryFollower &follower, hardware_interface::JointStateInterface &js_interface,
std::vector<std::string> &joint_names);
virtual bool write();
virtual void start();
virtual void stop();
typedef hardware_interface::PositionJointInterface parent_type;
static const std::string INTERFACE_NAME;
};

61
include/ur_modern_driver/ros/io_service.h Normal file
View File

@@ -0,0 +1,61 @@
#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_modern_driver/log.h"
#include "ur_modern_driver/ur/commander.h"
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))
{
}
};

41
include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h Normal file
View File

@@ -0,0 +1,41 @@
#pragma once
#include <inttypes.h>
#include <array>
#include <atomic>
#include <cstddef>
#include <cstring>
#include <string>
#include <thread>
#include <vector>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/commander.h"
#include "ur_modern_driver/ur/server.h"
#include "ur_modern_driver/ros/action_trajectory_follower_interface.h"
class LowBandwidthTrajectoryFollower: public ActionTrajectoryFollowerInterface
{
private:
std::atomic<bool> running_;
std::array<double, 6> last_positions_;
URCommander &commander_;
URServer server_;
double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, \
servoj_gain_, servoj_lookahead_time_, max_joint_difference_;
std::string program_;
bool execute(const std::array<double, 6> &positions,
const std::array<double, 6> &velocities,
double sample_number, double time_in_seconds);
public:
LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3);
bool start();
bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt);
void stop();
virtual ~LowBandwidthTrajectoryFollower() {};
};

60
include/ur_modern_driver/ros/mb_publisher.h Normal file
View File

@@ -0,0 +1,60 @@
#pragma once
#include <industrial_msgs/RobotStatus.h>
#include <ros/ros.h>
#include <ur_msgs/Analog.h>
#include <ur_msgs/Digital.h>
#include <ur_msgs/IOStates.h>
#include "ur_modern_driver/ur/consumer.h"
using namespace ros;
class MBPublisher : public URStatePacketConsumer
{
private:
NodeHandle nh_;
Publisher io_pub_;
Publisher status_pub_;
template <size_t N>
inline void appendDigital(std::vector<ur_msgs::Digital>& vec, std::bitset<N> bits)
{
for (size_t i = 0; i < N; i++)
{
ur_msgs::Digital digi;
digi.pin = static_cast<uint8_t>(i);
digi.state = bits.test(i);
vec.push_back(digi);
}
}
void publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data);
void publishRobotStatus(industrial_msgs::RobotStatus& status, const SharedRobotModeData& data) const;
void publishRobotStatus(const RobotModeData_V1_X& data) const;
void publishRobotStatus(const RobotModeData_V3_0__1& data) const;
public:
MBPublisher() : io_pub_(nh_.advertise<ur_msgs::IOStates>("ur_driver/io_states", 1))
, status_pub_(nh_.advertise<industrial_msgs::RobotStatus>("ur_driver/robot_status", 1))
{
}
virtual bool consume(MasterBoardData_V1_X& data);
virtual bool consume(MasterBoardData_V3_0__1& data);
virtual bool consume(MasterBoardData_V3_2& data);
virtual bool consume(RobotModeData_V1_X& data);
virtual bool consume(RobotModeData_V3_0__1& data);
virtual bool consume(RobotModeData_V3_2& data);
virtual void setupConsumer()
{
}
virtual void teardownConsumer()
{
}
virtual void stopConsumer()
{
}
};

74
include/ur_modern_driver/ros/rt_publisher.h Normal file
View File

@@ -0,0 +1,74 @@
#pragma once
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/WrenchStamped.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/Temperature.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <cstdlib>
#include <vector>
#include "ur_modern_driver/ur/consumer.h"
using namespace ros;
using namespace tf;
const std::string JOINTS[] = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint" };
class RTPublisher : public URRTPacketConsumer
{
private:
NodeHandle nh_;
Publisher joint_pub_;
Publisher wrench_pub_;
Publisher tool_vel_pub_;
Publisher joint_temperature_pub_;
TransformBroadcaster transform_broadcaster_;
std::vector<std::string> joint_names_;
std::string base_frame_;
std::string tool_frame_;
bool temp_only_;
bool publishJoints(RTShared& packet, Time& t);
bool publishWrench(RTShared& packet, Time& t);
bool publishTool(RTShared& packet, Time& t);
bool publishTransform(RTShared& packet, Time& t);
bool publishTemperature(RTShared& packet, Time& t);
bool publish(RTShared& packet);
public:
RTPublisher(std::string& joint_prefix, std::string& base_frame, std::string& tool_frame, bool temp_only = false)
: joint_pub_(nh_.advertise<sensor_msgs::JointState>("joint_states", 1))
, wrench_pub_(nh_.advertise<geometry_msgs::WrenchStamped>("wrench", 1))
, tool_vel_pub_(nh_.advertise<geometry_msgs::TwistStamped>("tool_velocity", 1))
, joint_temperature_pub_(nh_.advertise<sensor_msgs::Temperature>("joint_temperature", 1))
, base_frame_(base_frame)
, tool_frame_(tool_frame)
, temp_only_(temp_only)
{
for (auto const& j : JOINTS)
{
joint_names_.push_back(joint_prefix + j);
}
}
virtual bool consume(RTState_V1_6__7& state);
virtual bool consume(RTState_V1_8& state);
virtual bool consume(RTState_V3_0__1& state);
virtual bool consume(RTState_V3_2__3& state);
virtual void setupConsumer()
{
}
virtual void teardownConsumer()
{
}
virtual void stopConsumer()
{
}
};

70
include/ur_modern_driver/ros/service_stopper.h Normal file
View File

@@ -0,0 +1,70 @@
#pragma once
#include <ros/ros.h>
#include <std_srvs/Empty.h>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/consumer.h"
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;
}
};

47
include/ur_modern_driver/ros/trajectory_follower.h Normal file
View File

@@ -0,0 +1,47 @@
#pragma once
#include <inttypes.h>
#include <array>
#include <atomic>
#include <cstddef>
#include <cstring>
#include <string>
#include <thread>
#include <vector>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/commander.h"
#include "ur_modern_driver/ur/server.h"
#include "ur_modern_driver/ros/action_trajectory_follower_interface.h"
class TrajectoryFollower : public ActionTrajectoryFollowerInterface
{
private:
std::atomic<bool> running_;
std::array<double, 6> last_positions_;
URCommander &commander_;
URServer server_;
double servoj_time_, servoj_lookahead_time_, servoj_gain_;
std::string program_;
template <typename T>
size_t append(uint8_t *buffer, T &val)
{
size_t s = sizeof(T);
std::memcpy(buffer, &val, s);
return s;
}
bool execute(std::array<double, 6> &positions, bool keep_alive);
double interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel);
public:
TrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3);
bool start();
bool execute(std::array<double, 6> &positions);
bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt);
void stop();
virtual ~TrajectoryFollower() {};
};

24
include/ur_modern_driver/ros/urscript_handler.h Normal file
View File

@@ -0,0 +1,24 @@
#pragma once
#include <ros/ros.h>
#include <string>
#include "std_msgs/String.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ros/service_stopper.h"
#include "ur_modern_driver/ur/commander.h"
class URScriptHandler: public Service
{
private:
ros::NodeHandle nh_;
URCommander &commander_;
ros::Subscriber urscript_sub_;
RobotState state_;
public:
URScriptHandler(URCommander &commander);
void urscriptInterface(const std_msgs::String::ConstPtr& msg);
void onRobotStateChange(RobotState state);
};

54
include/ur_modern_driver/tcp_socket.h Normal file
View File

@@ -0,0 +1,54 @@
#pragma once
#include <netdb.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <atomic>
#include <mutex>
#include <string>
enum class SocketState
{
Invalid,
Connected,
Disconnected,
Closed
};
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);
std::string getIP();
bool read(char *character);
bool read(uint8_t *buf, size_t buf_len, size_t &read);
bool write(const uint8_t *buf, size_t buf_len, size_t &written);
void close();
};

62
include/ur_modern_driver/test/random_data.h Normal file
View File

@@ -0,0 +1,62 @@
#pragma once
#include <inttypes.h>
#include <algorithm>
#include <climits>
#include <cstddef>
#include <functional>
#include <random>
#include <bitset>
#include "ur_modern_driver/bin_parser.h"
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);
}
};

7
include/ur_modern_driver/test/utils.h Normal file
View File

@@ -0,0 +1,7 @@
#pragma once
#define ASSERT_DOUBLE_ARRAY_EQ(fn, name) \
for (auto const& v : name) \
{ \
ASSERT_EQ(fn, v) << #name " failed parsing"; \
}

24
include/ur_modern_driver/types.h Normal file
View File

@@ -0,0 +1,24 @@
#pragma once
#include <inttypes.h>
struct double3_t
{
double x, y, z;
};
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;
}

75
include/ur_modern_driver/ur/commander.h Normal file
View File

@@ -0,0 +1,75 @@
#pragma once
#include <array>
#include <iomanip>
#include <sstream>
#include "ur_modern_driver/ur/stream.h"
class URCommander
{
private:
URStream &stream_;
protected:
bool write(const std::string &s);
void formatArray(std::ostringstream &out, std::array<double, 6> &values);
public:
URCommander(URStream &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(URStream &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(URStream &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(URStream &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(URStream &stream) : URCommander_V3_X(stream)
{
}
virtual bool speedj(std::array<double, 6> &speeds, double acceleration);
};

50
include/ur_modern_driver/ur/consumer.h Normal file
View File

@@ -0,0 +1,50 @@
#pragma once
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ur/master_board.h"
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/robot_mode.h"
#include "ur_modern_driver/ur/rt_state.h"
#include "ur_modern_driver/ur/state.h"
class URRTPacketConsumer : public IConsumer<RTPacket>
{
public:
virtual bool consume(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 IConsumer<StatePacket>
{
public:
virtual bool consume(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 IConsumer<MessagePacket>
{
public:
virtual bool consume(shared_ptr<MessagePacket> packet)
{
return packet->consumeWith(*this);
}
virtual bool consume(VersionMessage& message) = 0;
};

123
include/ur_modern_driver/ur/factory.h Normal file
View File

@@ -0,0 +1,123 @@
#pragma once
#include <cstdlib>
#include "ur_modern_driver/ur/consumer.h"
#include "ur_modern_driver/ur/messages_parser.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/producer.h"
#include "ur_modern_driver/ur/rt_parser.h"
#include "ur_modern_driver/ur/state_parser.h"
#include "ur_modern_driver/ur/stream.h"
static const int UR_PRIMARY_PORT = 30001;
class URFactory : private URMessagePacketConsumer
{
private:
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<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(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<URParser<StatePacket>> getStateParser()
{
if (major_version_ == 1)
{
return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V1_X);
}
else
{
if (minor_version_ < 3)
return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_0__1);
else if (minor_version_ < 5)
return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_2);
else
return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_5);
}
}
std::unique_ptr<URParser<RTPacket>> getRTParser()
{
if (major_version_ == 1)
{
if (minor_version_ < 8)
return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V1_6__7);
else
return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V1_8);
}
else
{
if (minor_version_ < 3)
return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_0__1);
else
return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_2__3);
}
}
};

92
include/ur_modern_driver/ur/master_board.h Normal file
View File

@@ -0,0 +1,92 @@
#pragma once
#include <inttypes.h>
#include <bitset>
#include <cstddef>
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/ur/state.h"
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;
};

51
include/ur_modern_driver/ur/messages.h Normal file
View File

@@ -0,0 +1,51 @@
#pragma once
#include <inttypes.h>
#include <cstddef>
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/pipeline.h"
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;
};

53
include/ur_modern_driver/ur/messages_parser.h Normal file
View File

@@ -0,0 +1,53 @@
#pragma once
#include <vector>
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/parser.h"
class URMessageParser : public URParser<MessagePacket>
{
public:
bool parse(BinParser& bp, std::vector<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;
}
};

11
include/ur_modern_driver/ur/parser.h Normal file
View File

@@ -0,0 +1,11 @@
#pragma once
#include <vector>
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/pipeline.h"
template <typename T>
class URParser
{
public:
virtual bool parse(BinParser& bp, std::vector<std::unique_ptr<T>>& results) = 0;
};

65
include/ur_modern_driver/ur/producer.h Normal file
View File

@@ -0,0 +1,65 @@
#pragma once
#include <chrono>
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/stream.h"
template <typename T>
class URProducer : public IProducer<T>
{
private:
URStream& stream_;
URParser<T>& parser_;
std::chrono::seconds timeout_;
public:
URProducer(URStream& stream, URParser<T>& parser) : stream_(stream), parser_(parser), timeout_(1)
{
}
void setupProducer()
{
stream_.connect();
}
void teardownProducer()
{
stream_.disconnect();
}
void stopProducer()
{
stream_.disconnect();
}
bool tryGet(std::vector<unique_ptr<T>>& 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);
}
};

118
include/ur_modern_driver/ur/robot_mode.h Normal file
View File

@@ -0,0 +1,118 @@
#pragma once
#include <inttypes.h>
#include <cstddef>
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/ur/state.h"
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");
};

38
include/ur_modern_driver/ur/rt_parser.h Normal file
View File

@@ -0,0 +1,38 @@
#pragma once
#include <vector>
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/rt_state.h"
template <typename T>
class URRTStateParser : public 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;

119
include/ur_modern_driver/ur/rt_state.h Normal file
View File

@@ -0,0 +1,119 @@
#pragma once
#include <inttypes.h>
#include <cstddef>
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/types.h"
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!");
};

31
include/ur_modern_driver/ur/server.h Normal file
View File

@@ -0,0 +1,31 @@
#pragma once
#include <netdb.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <atomic>
#include <cstdlib>
#include <mutex>
#include <string>
#include "ur_modern_driver/tcp_socket.h"
#define MAX_SERVER_BUF_LEN 50
class URServer : private TCPSocket
{
private:
int port_;
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);
};

43
include/ur_modern_driver/ur/state.h Normal file
View File

@@ -0,0 +1,43 @@
#pragma once
#include <inttypes.h>
#include <cstddef>
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h"
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;
};

99
include/ur_modern_driver/ur/state_parser.h Normal file
View File

@@ -0,0 +1,99 @@
#pragma once
#include <vector>
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ur/master_board.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/robot_mode.h"
#include "ur_modern_driver/ur/state.h"
template <typename RMD, typename MBD>
class URStateParser : public 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;

46
include/ur_modern_driver/ur/stream.h Normal file
View File

@@ -0,0 +1,46 @@
#pragma once
#include <netdb.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <atomic>
#include <mutex>
#include <string>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/tcp_socket.h"
class URStream : public TCPSocket
{
private:
std::string host_;
int port_;
std::mutex write_mutex_, read_mutex_;
protected:
virtual bool open(int socket_fd, struct sockaddr* address, size_t address_len)
{
return ::connect(socket_fd, address, address_len) == 0;
}
public:
URStream(std::string& host, int port) : host_(host), port_(port)
{
}
bool connect()
{
return TCPSocket::setup(host_, port_);
}
void disconnect()
{
LOG_INFO("Disconnecting from %s:%d", host_.c_str(), port_);
TCPSocket::close();
}
bool closed()
{
return getState() == SocketState::Closed;
}
bool read(uint8_t* buf, size_t buf_len, size_t& read);
bool write(const uint8_t* buf, size_t buf_len, size_t& written);
};

64
include/ur_modern_driver/ur_communication.h
View File

@@ -1,64 +0,0 @@
/*
* ur_communication.h
*
* Copyright 2015 Thomas Timm Andersen
*
* 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.
*/
#ifndef UR_COMMUNICATION_H_
#define UR_COMMUNICATION_H_
#include "robot_state.h"
#include "do_output.h"
#include <vector>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <netdb.h>
#include <iostream>
#include <unistd.h>
#include <chrono>
#include <fcntl.h>
#include <sys/types.h>
class UrCommunication {
private:
int pri_sockfd_, sec_sockfd_;
struct sockaddr_in pri_serv_addr_, sec_serv_addr_;
struct hostent *server_;
bool keepalive_;
std::thread comThread_;
int flag_;
void run();
public:
bool connected_;
RobotState* robot_state_;
UrCommunication(std::condition_variable& msg_cond, std::string host);
bool start();
void halt();
};
#endif /* UR_COMMUNICATION_H_ */

101
include/ur_modern_driver/ur_driver.h
View File

@@ -1,101 +0,0 @@
/*
* ur_driver
*
* Copyright 2015 Thomas Timm Andersen
*
* 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.
*/
#ifndef UR_DRIVER_H_
#define UR_DRIVER_H_
#include <mutex>
#include <condition_variable>
#include "ur_realtime_communication.h"
#include "ur_communication.h"
#include "do_output.h"
#include <vector>
#include <math.h>
#include <string>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <chrono>
class UrDriver {
private:
double maximum_time_step_;
double minimum_payload_;
double maximum_payload_;
std::vector<std::string> joint_names_;
std::string ip_addr_;
const int MULT_JOINTSTATE_ = 1000000;
const int MULT_TIME_ = 1000000;
const unsigned int REVERSE_PORT_;
int incoming_sockfd_;
int new_sockfd_;
bool reverse_connected_;
double servoj_time_;
bool executing_traj_;
double firmware_version_;
double servoj_lookahead_time_;
double servoj_gain_;
public:
UrRealtimeCommunication* rt_interface_;
UrCommunication* sec_interface_;
UrDriver(std::condition_variable& rt_msg_cond,
std::condition_variable& msg_cond, std::string host,
unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max =
12, double max_time_step = 0.08, double min_payload = 0.,
double max_payload = 1., double servoj_lookahead_time=0.03, double servoj_gain=300.);
bool start();
void halt();
void setSpeed(double q0, double q1, double q2, double q3, double q4,
double q5, double acc = 100.);
bool doTraj(std::vector<double> inp_timestamps,
std::vector<std::vector<double> > inp_positions,
std::vector<std::vector<double> > inp_velocities);
void servoj(std::vector<double> positions, int keepalive = 1);
void stopTraj();
bool uploadProg();
bool openServo();
void closeServo(std::vector<double> positions);
std::vector<double> interp_cubic(double t, double T,
std::vector<double> p0_pos, std::vector<double> p1_pos,
std::vector<double> p0_vel, std::vector<double> p1_vel);
std::vector<std::string> getJointNames();
void setJointNames(std::vector<std::string> jn);
void setToolVoltage(unsigned int v);
void setFlag(unsigned int n, bool b);
void setDigitalOut(unsigned int n, bool b);
void setAnalogOut(unsigned int n, double f);
bool setPayload(double m);
void setMinPayload(double m);
void setMaxPayload(double m);
void setServojTime(double t);
void setServojLookahead(double t);
void setServojGain(double g);
};
#endif /* UR_DRIVER_H_ */

139
include/ur_modern_driver/ur_hardware_interface.h
View File

@@ -1,139 +0,0 @@
/*
* ur_hardware_control_loop.cpp
*
* Copyright 2015 Thomas Timm Andersen
*
* 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.
*/
/* Based on original source from University of Colorado, Boulder. License copied below. */
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2015, University of Colorado, Boulder
* 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.
* * Neither the name of the Univ of CO, Boulder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************
Author: Dave Coleman
*/
#ifndef UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H
#define UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/force_torque_sensor_interface.h>
#include <hardware_interface/robot_hw.h>
#include <controller_manager/controller_manager.h>
#include <boost/scoped_ptr.hpp>
#include <ros/ros.h>
#include <math.h>
#include "do_output.h"
#include "ur_driver.h"
namespace ros_control_ur {
// For simulation only - determines how fast a trajectory is followed
static const double POSITION_STEP_FACTOR = 1;
static const double VELOCITY_STEP_FACTOR = 1;
/// \brief Hardware interface for a robot
class UrHardwareInterface: public hardware_interface::RobotHW {
public:
/**
* \brief Constructor
* \param nh - Node handle for topics.
*/
UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot);
/// \brief Initialize the hardware interface
virtual void init();
/// \brief Read the state from the robot hardware.
virtual void read();
/// \brief write the command to the robot hardware.
virtual void write();
void setMaxVelChange(double inp);
bool canSwitch(
const std::list<hardware_interface::ControllerInfo> &start_list,
const std::list<hardware_interface::ControllerInfo> &stop_list) const;
void doSwitch(const std::list<hardware_interface::ControllerInfo>&start_list,
const std::list<hardware_interface::ControllerInfo>&stop_list);
protected:
// Startup and shutdown of the internal node inside a roscpp program
ros::NodeHandle nh_;
// Interfaces
hardware_interface::JointStateInterface joint_state_interface_;
hardware_interface::ForceTorqueSensorInterface force_torque_interface_;
hardware_interface::PositionJointInterface position_joint_interface_;
hardware_interface::VelocityJointInterface velocity_joint_interface_;
bool velocity_interface_running_;
bool position_interface_running_;
// Shared memory
std::vector<std::string> joint_names_;
std::vector<double> joint_position_;
std::vector<double> joint_velocity_;
std::vector<double> joint_effort_;
std::vector<double> joint_position_command_;
std::vector<double> joint_velocity_command_;
std::vector<double> prev_joint_velocity_command_;
std::size_t num_joints_;
double robot_force_[3] = { 0., 0., 0. };
double robot_torque_[3] = { 0., 0., 0. };
double max_vel_change_;
// Robot API
UrDriver* robot_;
};
// class
}// namespace
#endif

76
include/ur_modern_driver/ur_realtime_communication.h
View File

@@ -1,76 +0,0 @@
/*
* ur_realtime_communication.h
*
* Copyright 2015 Thomas Timm Andersen
*
* 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.
*/
#ifndef UR_REALTIME_COMMUNICATION_H_
#define UR_REALTIME_COMMUNICATION_H_
#include "robot_state_RT.h"
#include "do_output.h"
#include <vector>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <netdb.h>
#include <iostream>
#include <unistd.h>
#include <arpa/inet.h>
#include <errno.h>
#include <fcntl.h>
#include <sys/types.h>
class UrRealtimeCommunication {
private:
unsigned int safety_count_max_;
int sockfd_;
struct sockaddr_in serv_addr_;
struct hostent *server_;
std::string local_ip_;
bool keepalive_;
std::thread comThread_;
int flag_;
std::recursive_mutex command_string_lock_;
std::string command_;
unsigned int safety_count_;
void run();
public:
bool connected_;
RobotStateRT* robot_state_;
UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host,
unsigned int safety_count_max = 12);
bool start();
void halt();
void setSpeed(double q0, double q1, double q2, double q3, double q4,
double q5, double acc = 100.);
void addCommandToQueue(std::string inp);
void setSafetyCountMax(uint inp);
std::string getLocalIp();
};
#endif /* UR_REALTIME_COMMUNICATION_H_ */

27
launch/ur10_bringup.launch
View File

@@ -7,23 +7,46 @@
ur10_bringup.launch robot_ip:=<value>
-->
<launch>
<!-- robot_ip: IP-address of the robot's socket-messaging server -->
<arg name="robot_ip"/>
<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_modern_driver)/launch/ur_common.launch">
<arg name="robot_ip" value="$(arg robot_ip)"/>
<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>

29
launch/ur10_bringup_compatible.launch
View File

@@ -7,24 +7,47 @@
ur10_bringup.launch robot_ip:=<value>
-->
<launch>
<!-- robot_ip: IP-address of the robot's socket-messaging server -->
<arg name="robot_ip"/>
<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_modern_driver)/launch/ur_common.launch">
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/>
<arg name="servoj_time" value="0.08" />
<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>

26
launch/ur10_bringup_joint_limited.launch
View File

@@ -2,17 +2,28 @@
<!--
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="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_modern_driver)/launch/ur10_bringup.launch">
<arg name="robot_ip" value="$(arg robot_ip)"/>
@@ -20,5 +31,16 @@
<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>

2
launch/ur10_ros_control.launch
View File

@@ -14,6 +14,7 @@
<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" />
<!-- robot model -->
<include file="$(find ur_description)/launch/ur10_upload.launch">
<arg name="limited" value="$(arg limited)"/>
@@ -31,6 +32,7 @@
<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 -->

22
launch/ur3_bringup.launch
View File

@@ -14,6 +14,17 @@
<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)"/>
@@ -25,6 +36,17 @@
<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>

22
launch/ur3_bringup_joint_limited.launch
View File

@@ -13,6 +13,17 @@
<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_modern_driver)/launch/ur3_bringup.launch">
<arg name="robot_ip" value="$(arg robot_ip)"/>
@@ -20,5 +31,16 @@
<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>

6
launch/ur3_ros_control.launch
View File

@@ -14,6 +14,7 @@
<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" />
<!-- robot model -->
<include file="$(find ur_description)/launch/ur3_upload.launch">
<arg name="limited" value="$(arg limited)"/>
@@ -31,6 +32,7 @@
<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 -->
@@ -38,11 +40,11 @@
<!-- spawn controller manager -->
<node name="ros_control_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" args="joint_state_controller force_torque_sensor_controller pos_based_pos_traj_controller" />
output="screen" args="joint_state_controller force_torque_sensor_controller vel_based_pos_traj_controller" />
<!-- load other controller -->
<node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
output="screen" args="load vel_based_pos_traj_controller" />
output="screen" args="load pos_based_pos_traj_controller" />
<!-- Convert joint states to /tf tranforms -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

28
launch/ur5_bringup.launch
View File

@@ -7,24 +7,46 @@
ur5_bringup.launch robot_ip:=<value>
-->
<launch>
<!-- robot_ip: IP-address of the robot's socket-messaging server -->
<arg name="robot_ip"/>
<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_modern_driver)/launch/ur_common.launch">
<arg name="prefix" value="$(arg prefix)" />
<arg name="robot_ip" value="$(arg robot_ip)"/>
<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>

29
launch/ur5_bringup_compatible.launch
View File

@@ -7,24 +7,47 @@
ur5_bringup.launch robot_ip:=<value>
-->
<launch>
<!-- robot_ip: IP-address of the robot's socket-messaging server -->
<arg name="robot_ip"/>
<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_modern_driver)/launch/ur_common.launch">
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/>
<arg name="servoj_time" value="0.08" />
<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>

28
launch/ur5_bringup_joint_limited.launch
View File

@@ -2,23 +2,45 @@
<!--
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="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_modern_driver)/launch/ur5_bringup.launch">
<arg name="robot_ip" value="$(arg robot_ip)"/>
<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>

2
launch/ur5_ros_control.launch
View File

@@ -14,6 +14,7 @@
<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" />
<!-- robot model -->
<include file="$(find ur_description)/launch/ur5_upload.launch">
<arg name="limited" value="$(arg limited)"/>
@@ -31,6 +32,7 @@
<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 -->

28
launch/ur_common.launch
View File

@@ -2,7 +2,7 @@
<!--
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>
-->
@@ -12,10 +12,22 @@
<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] -->
@@ -26,11 +38,21 @@
<!-- 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="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="base_frame" type="str" value="$(arg base_frame)"/>
<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>

13
package.xml
View File

@@ -45,27 +45,32 @@
<build_depend>actionlib</build_depend>
<build_depend>control_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>industrial_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>trajectory_msgs</build_depend>
<build_depend>ur_msgs</build_depend>
<build_depend>tf</build_depend>
<build_depend>realtime_tools</build_depend>
<build_depend>std_srvs</build_depend>
<run_depend>hardware_interface</run_depend>
<run_depend>controller_manager</run_depend>
<run_depend>force_torque_sensor_controller</run_depend>
<run_depend>joint_state_controller</run_depend>
<run_depend>joint_trajectory_controller</run_depend>
<run_depend>actionlib</run_depend>
<run_depend>control_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>industrial_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>trajectory_msgs</run_depend>
<run_depend>ur_msgs</run_depend>
<run_depend>ur_description</run_depend>
<run_depend>tf</run_depend>
<run_depend>realtime_tools</run_depend>
<run_depend>std_srvs</run_depend>
<run_depend>robot_state_publisher</run_depend>
<test_depend>rosunit</test_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>

57
src/do_output.cpp
View File

@@ -1,57 +0,0 @@
/*
* do_output.cpp
*
* Copyright 2015 Thomas Timm Andersen
*
* 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_modern_driver/do_output.h"
void print_debug(std::string inp) {
#ifdef ROS_BUILD
ROS_DEBUG("%s", inp.c_str());
#else
printf("DEBUG: %s\n", inp.c_str());
#endif
}
void print_info(std::string inp) {
#ifdef ROS_BUILD
ROS_INFO("%s", inp.c_str());
#else
printf("INFO: %s\n", inp.c_str());
#endif
}
void print_warning(std::string inp) {
#ifdef ROS_BUILD
ROS_WARN("%s", inp.c_str());
#else
printf("WARNING: %s\n", inp.c_str());
#endif
}
void print_error(std::string inp) {
#ifdef ROS_BUILD
ROS_ERROR("%s", inp.c_str());
#else
printf("ERROR: %s\n", inp.c_str());
#endif
}
void print_fatal(std::string inp) {
#ifdef ROS_BUILD
ROS_FATAL("%s", inp.c_str());
ros::shutdown();
#else
printf("FATAL: %s\n", inp.c_str());
exit(1);
#endif
}

387
src/robot_state.cpp
View File

@@ -1,387 +0,0 @@
/*
* robot_state.cpp
*
* Copyright 2015 Thomas Timm Andersen
*
* 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_modern_driver/robot_state.h"
RobotState::RobotState(std::condition_variable& msg_cond) {
version_msg_.major_version = 0;
version_msg_.minor_version = 0;
new_data_available_ = false;
pMsg_cond_ = &msg_cond;
RobotState::setDisconnected();
robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING;
}
double RobotState::ntohd(uint64_t nf) {
double x;
nf = be64toh(nf);
memcpy(&x, &nf, sizeof(x));
return x;
}
void RobotState::unpack(uint8_t* buf, unsigned int buf_length) {
/* Returns missing bytes to unpack a message, or 0 if all data was parsed */
unsigned int offset = 0;
while (buf_length > offset) {
int len;
unsigned char message_type;
memcpy(&len, &buf[offset], sizeof(len));
len = ntohl(len);
if (len + offset > buf_length) {
return;
}
memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type));
switch (message_type) {
case messageType::ROBOT_MESSAGE:
RobotState::unpackRobotMessage(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType
break;
case messageType::ROBOT_STATE:
RobotState::unpackRobotState(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType
break;
case messageType::PROGRAM_STATE_MESSAGE:
//Don't do anything atm...
default:
break;
}
offset += len;
}
return;
}
void RobotState::unpackRobotMessage(uint8_t * buf, unsigned int offset,
uint32_t len) {
offset += 5;
uint64_t timestamp;
int8_t source, robot_message_type;
memcpy(&timestamp, &buf[offset], sizeof(timestamp));
offset += sizeof(timestamp);
memcpy(&source, &buf[offset], sizeof(source));
offset += sizeof(source);
memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type));
offset += sizeof(robot_message_type);
switch (robot_message_type) {
case robotMessageType::ROBOT_MESSAGE_VERSION:
val_lock_.lock();
version_msg_.timestamp = timestamp;
version_msg_.source = source;
version_msg_.robot_message_type = robot_message_type;
RobotState::unpackRobotMessageVersion(buf, offset, len);
val_lock_.unlock();
break;
default:
break;
}
}
void RobotState::unpackRobotState(uint8_t * buf, unsigned int offset,
uint32_t len) {
offset += 5;
while (offset < len) {
int32_t length;
uint8_t package_type;
memcpy(&length, &buf[offset], sizeof(length));
length = ntohl(length);
memcpy(&package_type, &buf[offset + sizeof(length)],
sizeof(package_type));
switch (package_type) {
case packageType::ROBOT_MODE_DATA:
val_lock_.lock();
RobotState::unpackRobotMode(buf, offset + 5);
val_lock_.unlock();
break;
case packageType::MASTERBOARD_DATA:
val_lock_.lock();
RobotState::unpackRobotStateMasterboard(buf, offset + 5);
val_lock_.unlock();
break;
default:
break;
}
offset += length;
}
new_data_available_ = true;
pMsg_cond_->notify_all();
}
void RobotState::unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
uint32_t len) {
memcpy(&version_msg_.project_name_size, &buf[offset],
sizeof(version_msg_.project_name_size));
offset += sizeof(version_msg_.project_name_size);
memcpy(&version_msg_.project_name, &buf[offset],
sizeof(char) * version_msg_.project_name_size);
offset += version_msg_.project_name_size;
version_msg_.project_name[version_msg_.project_name_size] = '\0';
memcpy(&version_msg_.major_version, &buf[offset],
sizeof(version_msg_.major_version));
offset += sizeof(version_msg_.major_version);
memcpy(&version_msg_.minor_version, &buf[offset],
sizeof(version_msg_.minor_version));
offset += sizeof(version_msg_.minor_version);
memcpy(&version_msg_.svn_revision, &buf[offset],
sizeof(version_msg_.svn_revision));
offset += sizeof(version_msg_.svn_revision);
version_msg_.svn_revision = ntohl(version_msg_.svn_revision);
memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset);
version_msg_.build_date[len - offset] = '\0';
if (version_msg_.major_version < 2) {
robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE;
}
}
void RobotState::unpackRobotMode(uint8_t * buf, unsigned int offset) {
memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp));
offset += sizeof(robot_mode_.timestamp);
uint8_t tmp;
memcpy(&tmp, &buf[offset], sizeof(tmp));
if (tmp > 0)
robot_mode_.isRobotConnected = true;
else
robot_mode_.isRobotConnected = false;
offset += sizeof(tmp);
memcpy(&tmp, &buf[offset], sizeof(tmp));
if (tmp > 0)
robot_mode_.isRealRobotEnabled = true;
else
robot_mode_.isRealRobotEnabled = false;
offset += sizeof(tmp);
memcpy(&tmp, &buf[offset], sizeof(tmp));
//printf("PowerOnRobot: %d\n", tmp);
if (tmp > 0)
robot_mode_.isPowerOnRobot = true;
else
robot_mode_.isPowerOnRobot = false;
offset += sizeof(tmp);
memcpy(&tmp, &buf[offset], sizeof(tmp));
if (tmp > 0)
robot_mode_.isEmergencyStopped = true;
else
robot_mode_.isEmergencyStopped = false;
offset += sizeof(tmp);
memcpy(&tmp, &buf[offset], sizeof(tmp));
if (tmp > 0)
robot_mode_.isProtectiveStopped = true;
else
robot_mode_.isProtectiveStopped = false;
offset += sizeof(tmp);
memcpy(&tmp, &buf[offset], sizeof(tmp));
if (tmp > 0)
robot_mode_.isProgramRunning = true;
else
robot_mode_.isProgramRunning = false;
offset += sizeof(tmp);
memcpy(&tmp, &buf[offset], sizeof(tmp));
if (tmp > 0)
robot_mode_.isProgramPaused = true;
else
robot_mode_.isProgramPaused = false;
offset += sizeof(tmp);
memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode));
offset += sizeof(robot_mode_.robotMode);
uint64_t temp;
if (RobotState::getVersion() > 2.) {
memcpy(&robot_mode_.controlMode, &buf[offset],
sizeof(robot_mode_.controlMode));
offset += sizeof(robot_mode_.controlMode);
memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp);
robot_mode_.targetSpeedFraction = RobotState::ntohd(temp);
}
memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp);
robot_mode_.speedScaling = RobotState::ntohd(temp);
}
void RobotState::unpackRobotStateMasterboard(uint8_t * buf,
unsigned int offset) {
if (RobotState::getVersion() < 3.0) {
int16_t digital_input_bits, digital_output_bits;
memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
offset += sizeof(digital_input_bits);
memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits));
offset += sizeof(digital_output_bits);
mb_data_.digitalInputBits = ntohs(digital_input_bits);
mb_data_.digitalOutputBits = ntohs(digital_output_bits);
} else {
memcpy(&mb_data_.digitalInputBits, &buf[offset],
sizeof(mb_data_.digitalInputBits));
offset += sizeof(mb_data_.digitalInputBits);
mb_data_.digitalInputBits = ntohl(mb_data_.digitalInputBits);
memcpy(&mb_data_.digitalOutputBits, &buf[offset],
sizeof(mb_data_.digitalOutputBits));
offset += sizeof(mb_data_.digitalOutputBits);
mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits);
}
memcpy(&mb_data_.analogInputRange0, &buf[offset],
sizeof(mb_data_.analogInputRange0));
offset += sizeof(mb_data_.analogInputRange0);
memcpy(&mb_data_.analogInputRange1, &buf[offset],
sizeof(mb_data_.analogInputRange1));
offset += sizeof(mb_data_.analogInputRange1);
uint64_t temp;
memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp);
mb_data_.analogInput0 = RobotState::ntohd(temp);
memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp);
mb_data_.analogInput1 = RobotState::ntohd(temp);
memcpy(&mb_data_.analogOutputDomain0, &buf[offset],
sizeof(mb_data_.analogOutputDomain0));
offset += sizeof(mb_data_.analogOutputDomain0);
memcpy(&mb_data_.analogOutputDomain1, &buf[offset],
sizeof(mb_data_.analogOutputDomain1));
offset += sizeof(mb_data_.analogOutputDomain1);
memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp);
mb_data_.analogOutput0 = RobotState::ntohd(temp);
memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp);
mb_data_.analogOutput1 = RobotState::ntohd(temp);
memcpy(&mb_data_.masterBoardTemperature, &buf[offset],
sizeof(mb_data_.masterBoardTemperature));
offset += sizeof(mb_data_.masterBoardTemperature);
mb_data_.masterBoardTemperature = ntohl(mb_data_.masterBoardTemperature);
memcpy(&mb_data_.robotVoltage48V, &buf[offset],
sizeof(mb_data_.robotVoltage48V));
offset += sizeof(mb_data_.robotVoltage48V);
mb_data_.robotVoltage48V = ntohl(mb_data_.robotVoltage48V);
memcpy(&mb_data_.robotCurrent, &buf[offset], sizeof(mb_data_.robotCurrent));
offset += sizeof(mb_data_.robotCurrent);
mb_data_.robotCurrent = ntohl(mb_data_.robotCurrent);
memcpy(&mb_data_.masterIOCurrent, &buf[offset],
sizeof(mb_data_.masterIOCurrent));
offset += sizeof(mb_data_.masterIOCurrent);
mb_data_.masterIOCurrent = ntohl(mb_data_.masterIOCurrent);
memcpy(&mb_data_.safetyMode, &buf[offset], sizeof(mb_data_.safetyMode));
offset += sizeof(mb_data_.safetyMode);
memcpy(&mb_data_.masterOnOffState, &buf[offset],
sizeof(mb_data_.masterOnOffState));
offset += sizeof(mb_data_.masterOnOffState);
memcpy(&mb_data_.euromap67InterfaceInstalled, &buf[offset],
sizeof(mb_data_.euromap67InterfaceInstalled));
offset += sizeof(mb_data_.euromap67InterfaceInstalled);
if (mb_data_.euromap67InterfaceInstalled != 0) {
memcpy(&mb_data_.euromapInputBits, &buf[offset],
sizeof(mb_data_.euromapInputBits));
offset += sizeof(mb_data_.euromapInputBits);
mb_data_.euromapInputBits = ntohl(mb_data_.euromapInputBits);
memcpy(&mb_data_.euromapOutputBits, &buf[offset],
sizeof(mb_data_.euromapOutputBits));
offset += sizeof(mb_data_.euromapOutputBits);
mb_data_.euromapOutputBits = ntohl(mb_data_.euromapOutputBits);
if (RobotState::getVersion() < 3.0) {
int16_t euromap_voltage, euromap_current;
memcpy(&euromap_voltage, &buf[offset], sizeof(euromap_voltage));
offset += sizeof(euromap_voltage);
memcpy(&euromap_current, &buf[offset], sizeof(euromap_current));
offset += sizeof(euromap_current);
mb_data_.euromapVoltage = ntohs(euromap_voltage);
mb_data_.euromapCurrent = ntohs(euromap_current);
} else {
memcpy(&mb_data_.euromapVoltage, &buf[offset],
sizeof(mb_data_.euromapVoltage));
offset += sizeof(mb_data_.euromapVoltage);
mb_data_.euromapVoltage = ntohl(mb_data_.euromapVoltage);
memcpy(&mb_data_.euromapCurrent, &buf[offset],
sizeof(mb_data_.euromapCurrent));
offset += sizeof(mb_data_.euromapCurrent);
mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent);
}
}
}
double RobotState::getVersion() {
double ver;
val_lock_.lock();
ver = version_msg_.major_version + 0.1 * version_msg_.minor_version
+ .0000001 * version_msg_.svn_revision;
val_lock_.unlock();
return ver;
}
void RobotState::finishedReading() {
new_data_available_ = false;
}
bool RobotState::getNewDataAvailable() {
return new_data_available_;
}
int RobotState::getDigitalInputBits() {
return mb_data_.digitalInputBits;
}
int RobotState::getDigitalOutputBits() {
return mb_data_.digitalOutputBits;
}
double RobotState::getAnalogInput0() {
return mb_data_.analogInput0;
}
double RobotState::getAnalogInput1() {
return mb_data_.analogInput1;
}
double RobotState::getAnalogOutput0() {
return mb_data_.analogOutput0;
}
double RobotState::getAnalogOutput1() {
return mb_data_.analogOutput1;
}
bool RobotState::isRobotConnected() {
return robot_mode_.isRobotConnected;
}
bool RobotState::isRealRobotEnabled() {
return robot_mode_.isRealRobotEnabled;
}
bool RobotState::isPowerOnRobot() {
return robot_mode_.isPowerOnRobot;
}
bool RobotState::isEmergencyStopped() {
return robot_mode_.isEmergencyStopped;
}
bool RobotState::isProtectiveStopped() {
return robot_mode_.isProtectiveStopped;
}
bool RobotState::isProgramRunning() {
return robot_mode_.isProgramRunning;
}
bool RobotState::isProgramPaused() {
return robot_mode_.isProgramPaused;
}
unsigned char RobotState::getRobotMode() {
return robot_mode_.robotMode;
}
bool RobotState::isReady() {
if (robot_mode_.robotMode == robot_mode_running_) {
return true;
}
return false;
}
void RobotState::setDisconnected() {
robot_mode_.isRobotConnected = false;
robot_mode_.isRealRobotEnabled = false;
robot_mode_.isPowerOnRobot = false;
}

437
src/robot_state_RT.cpp
View File

@@ -1,437 +0,0 @@
/*
* robotStateRT.cpp
*
* Copyright 2015 Thomas Timm Andersen
*
* 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_modern_driver/robot_state_RT.h"
RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) {
version_ = 0.0;
time_ = 0.0;
q_target_.assign(6, 0.0);
qd_target_.assign(6, 0.0);
qdd_target_.assign(6, 0.0);
i_target_.assign(6, 0.0);
m_target_.assign(6, 0.0);
q_actual_.assign(6, 0.0);
qd_actual_.assign(6, 0.0);
i_actual_.assign(6, 0.0);
i_control_.assign(6, 0.0);
tool_vector_actual_.assign(6, 0.0);
tcp_speed_actual_.assign(6, 0.0);
tcp_force_.assign(6, 0.0);
tool_vector_target_.assign(6, 0.0);
tcp_speed_target_.assign(6, 0.0);
digital_input_bits_.assign(64, false);
motor_temperatures_.assign(6, 0.0);
controller_timer_ = 0.0;
robot_mode_ = 0.0;
joint_modes_.assign(6, 0.0);
safety_mode_ = 0.0;
tool_accelerometer_values_.assign(3, 0.0);
speed_scaling_ = 0.0;
linear_momentum_norm_ = 0.0;
v_main_ = 0.0;
v_robot_ = 0.0;
i_robot_ = 0.0;
v_actual_.assign(6, 0.0);
data_published_ = false;
controller_updated_ = false;
pMsg_cond_ = &msg_cond;
}
RobotStateRT::~RobotStateRT() {
/* Make sure nobody is waiting after this thread is destroyed */
data_published_ = true;
controller_updated_ = true;
pMsg_cond_->notify_all();
}
void RobotStateRT::setDataPublished() {
data_published_ = false;
}
bool RobotStateRT::getDataPublished() {
return data_published_;
}
void RobotStateRT::setControllerUpdated() {
controller_updated_ = false;
}
bool RobotStateRT::getControllerUpdated() {
return controller_updated_;
}
double RobotStateRT::ntohd(uint64_t nf) {
double x;
nf = be64toh(nf);
memcpy(&x, &nf, sizeof(x));
return x;
}
std::vector<double> RobotStateRT::unpackVector(uint8_t * buf, int start_index,
int nr_of_vals) {
uint64_t q;
std::vector<double> ret;
for (int i = 0; i < nr_of_vals; i++) {
memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q));
ret.push_back(ntohd(q));
}
return ret;
}
std::vector<bool> RobotStateRT::unpackDigitalInputBits(int64_t data) {
std::vector<bool> ret;
for (int i = 0; i < 64; i++) {
ret.push_back((data & (1 << i)) >> i);
}
return ret;
}
void RobotStateRT::setVersion(double ver) {
val_lock_.lock();
version_ = ver;
val_lock_.unlock();
}
double RobotStateRT::getVersion() {
double ret;
val_lock_.lock();
ret = version_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getTime() {
double ret;
val_lock_.lock();
ret = time_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getQTarget() {
std::vector<double> ret;
val_lock_.lock();
ret = q_target_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getQdTarget() {
std::vector<double> ret;
val_lock_.lock();
ret = qd_target_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getQddTarget() {
std::vector<double> ret;
val_lock_.lock();
ret = qdd_target_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getITarget() {
std::vector<double> ret;
val_lock_.lock();
ret = i_target_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getMTarget() {
std::vector<double> ret;
val_lock_.lock();
ret = m_target_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getQActual() {
std::vector<double> ret;
val_lock_.lock();
ret = q_actual_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getQdActual() {
std::vector<double> ret;
val_lock_.lock();
ret = qd_actual_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getIActual() {
std::vector<double> ret;
val_lock_.lock();
ret = i_actual_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getIControl() {
std::vector<double> ret;
val_lock_.lock();
ret = i_control_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getToolVectorActual() {
std::vector<double> ret;
val_lock_.lock();
ret = tool_vector_actual_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getTcpSpeedActual() {
std::vector<double> ret;
val_lock_.lock();
ret = tcp_speed_actual_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getTcpForce() {
std::vector<double> ret;
val_lock_.lock();
ret = tcp_force_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getToolVectorTarget() {
std::vector<double> ret;
val_lock_.lock();
ret = tool_vector_target_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getTcpSpeedTarget() {
std::vector<double> ret;
val_lock_.lock();
ret = tcp_speed_target_;
val_lock_.unlock();
return ret;
}
std::vector<bool> RobotStateRT::getDigitalInputBits() {
std::vector<bool> ret;
val_lock_.lock();
ret = digital_input_bits_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getMotorTemperatures() {
std::vector<double> ret;
val_lock_.lock();
ret = motor_temperatures_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getControllerTimer() {
double ret;
val_lock_.lock();
ret = controller_timer_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getRobotMode() {
double ret;
val_lock_.lock();
ret = robot_mode_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getJointModes() {
std::vector<double> ret;
val_lock_.lock();
ret = joint_modes_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getSafety_mode() {
double ret;
val_lock_.lock();
ret = safety_mode_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getToolAccelerometerValues() {
std::vector<double> ret;
val_lock_.lock();
ret = tool_accelerometer_values_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getSpeedScaling() {
double ret;
val_lock_.lock();
ret = speed_scaling_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getLinearMomentumNorm() {
double ret;
val_lock_.lock();
ret = linear_momentum_norm_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getVMain() {
double ret;
val_lock_.lock();
ret = v_main_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getVRobot() {
double ret;
val_lock_.lock();
ret = v_robot_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getIRobot() {
double ret;
val_lock_.lock();
ret = i_robot_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getVActual() {
std::vector<double> ret;
val_lock_.lock();
ret = v_actual_;
val_lock_.unlock();
return ret;
}
void RobotStateRT::unpack(uint8_t * buf) {
int64_t digital_input_bits;
uint64_t unpack_to;
uint16_t offset = 0;
val_lock_.lock();
int len;
memcpy(&len, &buf[offset], sizeof(len));
offset += sizeof(len);
len = ntohl(len);
//Check the correct message length is received
bool len_good = true;
if (version_ >= 1.6 && version_ < 1.7) { //v1.6
if (len != 756)
len_good = false;
} else if (version_ >= 1.7 && version_ < 1.8) { //v1.7
if (len != 764)
len_good = false;
} else if (version_ >= 1.8 && version_ < 1.9) { //v1.8
if (len != 812)
len_good = false;
} else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1
if (len != 1044)
len_good = false;
} else if (version_ >= 3.2 && version_ < 3.3) { //v3.2
if (len != 1060)
len_good = false;
}
if (!len_good) {
printf("Wrong length of message on RT interface: %i\n", len);
val_lock_.unlock();
return;
}
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
time_ = RobotStateRT::ntohd(unpack_to);
offset += sizeof(double);
q_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
qd_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
qdd_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
i_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
m_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
q_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
qd_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
i_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
if (version_ <= 1.9) {
if (version_ > 1.6)
tool_accelerometer_values_ = unpackVector(buf, offset, 3);
offset += sizeof(double) * (3 + 15);
tcp_force_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tool_vector_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tcp_speed_actual_ = unpackVector(buf, offset, 6);
} else {
i_control_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tool_vector_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tcp_speed_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tcp_force_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tool_vector_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tcp_speed_target_ = unpackVector(buf, offset, 6);
}
offset += sizeof(double) * 6;
memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
digital_input_bits_ = unpackDigitalInputBits(be64toh(digital_input_bits));
offset += sizeof(double);
motor_temperatures_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
controller_timer_ = ntohd(unpack_to);
if (version_ > 1.6) {
offset += sizeof(double) * 2;
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
robot_mode_ = ntohd(unpack_to);
if (version_ > 1.7) {
offset += sizeof(double);
joint_modes_ = unpackVector(buf, offset, 6);
}
}
if (version_ > 1.8) {
offset += sizeof(double) * 6;
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
safety_mode_ = ntohd(unpack_to);
offset += sizeof(double);
tool_accelerometer_values_ = unpackVector(buf, offset, 3);
offset += sizeof(double) * 3;
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
speed_scaling_ = ntohd(unpack_to);
offset += sizeof(double);
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
linear_momentum_norm_ = ntohd(unpack_to);
offset += sizeof(double);
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
v_main_ = ntohd(unpack_to);
offset += sizeof(double);
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
v_robot_ = ntohd(unpack_to);
offset += sizeof(double);
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
i_robot_ = ntohd(unpack_to);
offset += sizeof(double);
v_actual_ = unpackVector(buf, offset, 6);
}
val_lock_.unlock();
controller_updated_ = true;
data_published_ = true;
pMsg_cond_->notify_all();
}

346
src/ros/action_server.cpp Normal file
View File

@@ -0,0 +1,346 @@
#include "ur_modern_driver/ros/action_server.h"
#include <cmath>
ActionServer::ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names, double max_velocity)
: as_(nh_, "follow_joint_trajectory", boost::bind(&ActionServer::onGoal, this, _1),
boost::bind(&ActionServer::onCancel, this, _1), false)
, joint_names_(joint_names)
, joint_set_(joint_names.begin(), joint_names.end())
, max_velocity_(max_velocity)
, interrupt_traj_(false)
, has_goal_(false)
, running_(false)
, follower_(follower)
, state_(RobotState::Error)
{
}
void ActionServer::start()
{
if (running_)
return;
LOG_INFO("Starting ActionServer");
running_ = true;
tj_thread_ = thread(&ActionServer::trajectoryThread, this);
as_.start();
}
void ActionServer::onRobotStateChange(RobotState state)
{
state_ = state;
// don't interrupt if everything is fine
if (state == RobotState::Running)
return;
// don't retry interrupts
if (interrupt_traj_ || !has_goal_)
return;
// on successful lock we're not executing a goal so don't interrupt
if (tj_mutex_.try_lock())
{
tj_mutex_.unlock();
return;
}
interrupt_traj_ = true;
// wait for goal to be interrupted and automagically unlock when going out of scope
std::lock_guard<std::mutex> lock(tj_mutex_);
Result res;
res.error_code = -100;
res.error_string = "Robot safety stop";
curr_gh_.setAborted(res, res.error_string);
}
bool ActionServer::updateState(RTShared& data)
{
q_actual_ = data.q_actual;
qd_actual_ = data.qd_actual;
return true;
}
bool ActionServer::consume(RTState_V1_6__7& state)
{
return updateState(state);
}
bool ActionServer::consume(RTState_V1_8& state)
{
return updateState(state);
}
bool ActionServer::consume(RTState_V3_0__1& state)
{
return updateState(state);
}
bool ActionServer::consume(RTState_V3_2__3& state)
{
return updateState(state);
}
void ActionServer::onGoal(GoalHandle gh)
{
Result res;
res.error_code = -100;
LOG_INFO("Received new goal");
if (!validate(gh, res) || !try_execute(gh, res))
{
LOG_WARN("Goal error: %s", res.error_string.c_str());
gh.setRejected(res, res.error_string);
}
}
void ActionServer::onCancel(GoalHandle gh)
{
interrupt_traj_ = true;
// wait for goal to be interrupted
std::lock_guard<std::mutex> lock(tj_mutex_);
Result res;
res.error_code = -100;
res.error_string = "Goal cancelled by client";
gh.setCanceled(res);
}
bool ActionServer::validate(GoalHandle& gh, Result& res)
{
return validateState(gh, res) && validateJoints(gh, res) && validateTrajectory(gh, res);
}
bool ActionServer::validateState(GoalHandle& gh, Result& res)
{
switch (state_)
{
case RobotState::EmergencyStopped:
res.error_string = "Robot is emergency stopped";
return false;
case RobotState::ProtectiveStopped:
res.error_string = "Robot is protective stopped";
return false;
case RobotState::Error:
res.error_string = "Robot is not ready, check robot_mode";
return false;
case RobotState::Running:
return true;
default:
res.error_string = "Undefined state";
return false;
}
}
bool ActionServer::validateJoints(GoalHandle& gh, Result& res)
{
auto goal = gh.getGoal();
auto const& joints = goal->trajectory.joint_names;
std::set<std::string> goal_joints(joints.begin(), joints.end());
if (goal_joints == joint_set_)
return true;
res.error_code = Result::INVALID_JOINTS;
res.error_string = "Invalid joint names for goal\n";
res.error_string += "Expected: ";
std::for_each(goal_joints.begin(), goal_joints.end(), [&res](std::string joint){res.error_string += joint + ", ";});
res.error_string += "\nFound: ";
std::for_each(joint_set_.begin(), joint_set_.end(), [&res](std::string joint){res.error_string += joint + ", ";});
return false;
}
bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res)
{
auto goal = gh.getGoal();
res.error_code = Result::INVALID_GOAL;
// must at least have one point
if (goal->trajectory.points.size() < 1)
return false;
for (auto const& point : goal->trajectory.points)
{
if (point.velocities.size() != joint_names_.size())
{
res.error_code = Result::INVALID_GOAL;
res.error_string = "Received a goal with an invalid number of velocities";
return false;
}
if (point.positions.size() != joint_names_.size())
{
res.error_code = Result::INVALID_GOAL;
res.error_string = "Received a goal with an invalid number of positions";
return false;
}
for (auto const& velocity : point.velocities)
{
if (!std::isfinite(velocity))
{
res.error_string = "Received a goal with infinities or NaNs in velocity";
return false;
}
if (std::fabs(velocity) > max_velocity_)
{
res.error_string = "Received a goal with velocities that are higher than max_velocity_ " + std::to_string(max_velocity_);
return false;
}
}
for (auto const& position : point.positions)
{
if (!std::isfinite(position))
{
res.error_string = "Received a goal with infinities or NaNs in positions";
return false;
}
}
}
// todo validate start position?
return true;
}
inline std::chrono::microseconds convert(const ros::Duration& dur)
{
return std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::seconds(dur.sec) +
std::chrono::nanoseconds(dur.nsec));
}
bool ActionServer::try_execute(GoalHandle& gh, Result& res)
{
if (!running_)
{
res.error_string = "Internal error";
return false;
}
if (!tj_mutex_.try_lock())
{
interrupt_traj_ = true;
res.error_string = "Received another trajectory";
curr_gh_.setAborted(res, res.error_string);
tj_mutex_.lock();
// todo: make configurable
std::this_thread::sleep_for(std::chrono::milliseconds(250));
}
// locked here
curr_gh_ = gh;
interrupt_traj_ = false;
has_goal_ = true;
tj_mutex_.unlock();
tj_cv_.notify_one();
return true;
}
std::vector<size_t> ActionServer::reorderMap(std::vector<std::string> goal_joints)
{
std::vector<size_t> indecies;
for (auto const& aj : joint_names_)
{
size_t j = 0;
for (auto const& gj : goal_joints)
{
if (aj == gj)
break;
j++;
}
indecies.push_back(j);
}
return indecies;
}
void ActionServer::trajectoryThread()
{
LOG_INFO("Trajectory thread started");
while (running_)
{
std::unique_lock<std::mutex> lk(tj_mutex_);
if (!tj_cv_.wait_for(lk, std::chrono::milliseconds(100), [&] { return running_ && has_goal_; }))
continue;
LOG_INFO("Trajectory received and accepted");
curr_gh_.setAccepted();
auto goal = curr_gh_.getGoal();
std::vector<TrajectoryPoint> trajectory;
trajectory.reserve(goal->trajectory.points.size() + 1);
// joint names of the goal might have a different ordering compared
// to what URScript expects so need to map between the two
auto mapping = reorderMap(goal->trajectory.joint_names);
LOG_INFO("Translating trajectory");
auto const& fp = goal->trajectory.points[0];
auto fpt = convert(fp.time_from_start);
// make sure we have a proper t0 position
if (fpt > std::chrono::microseconds(0))
{
LOG_INFO("Trajectory without t0 recieved, inserting t0 at currrent position");
trajectory.push_back(TrajectoryPoint(q_actual_, qd_actual_, std::chrono::microseconds(0)));
}
for (auto const& point : goal->trajectory.points)
{
std::array<double, 6> pos, vel;
for (size_t i = 0; i < 6; i++)
{
size_t idx = mapping[i];
pos[idx] = point.positions[i];
vel[idx] = point.velocities[i];
}
auto t = convert(point.time_from_start);
trajectory.push_back(TrajectoryPoint(pos, vel, t));
}
double t =
std::chrono::duration_cast<std::chrono::duration<double>>(trajectory[trajectory.size() - 1].time_from_start)
.count();
LOG_INFO("Executing trajectory with %zu points and duration of %4.3fs", trajectory.size(), t);
Result res;
LOG_INFO("Attempting to start follower %p", &follower_);
if (follower_.start())
{
if (follower_.execute(trajectory, interrupt_traj_))
{
// interrupted goals must be handled by interrupt trigger
if (!interrupt_traj_)
{
LOG_INFO("Trajectory executed successfully");
res.error_code = Result::SUCCESSFUL;
curr_gh_.setSucceeded(res);
}
else
LOG_INFO("Trajectory interrupted");
}
else
{
LOG_INFO("Trajectory failed");
res.error_code = -100;
res.error_string = "Connection to robot was lost";
curr_gh_.setAborted(res, res.error_string);
}
follower_.stop();
}
else
{
LOG_ERROR("Failed to start trajectory follower!");
res.error_code = -100;
res.error_string = "Robot connection could not be established";
curr_gh_.setAborted(res, res.error_string);
}
has_goal_ = false;
lk.unlock();
}
}

125
src/ros/controller.cpp Normal file
View File

@@ -0,0 +1,125 @@
#include "ur_modern_driver/ros/controller.h"
ROSController::ROSController(URCommander& commander, TrajectoryFollower& follower,
std::vector<std::string>& joint_names, double max_vel_change, std::string tcp_link)
: controller_(this, nh_)
, joint_interface_(joint_names)
, wrench_interface_(tcp_link)
, position_interface_(follower, joint_interface_, joint_names)
, velocity_interface_(commander, joint_interface_, joint_names, max_vel_change)
{
registerInterface(&joint_interface_);
registerInterface(&wrench_interface_);
registerControllereInterface(&position_interface_);
registerControllereInterface(&velocity_interface_);
}
void ROSController::setupConsumer()
{
lastUpdate_ = ros::Time::now();
}
void ROSController::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list)
{
LOG_INFO("Switching hardware interface");
if (active_interface_ != nullptr && stop_list.size() > 0)
{
LOG_INFO("Stopping active interface");
active_interface_->stop();
active_interface_ = nullptr;
}
for (auto const& ci : start_list)
{
std::string requested_interface("");
#if defined(UR_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL)
requested_interface = ci.hardware_interface;
#else
if (!ci.claimed_resources.empty())
requested_interface = ci.claimed_resources[0].hardware_interface;
#endif
auto ait = available_interfaces_.find(requested_interface);
if (ait == available_interfaces_.end())
continue;
auto new_interface = ait->second;
LOG_INFO("Starting %s", ci.name.c_str());
active_interface_ = new_interface;
new_interface->start();
return;
}
if(start_list.size() > 0)
LOG_WARN("Failed to start interface!");
}
bool ROSController::write()
{
if (active_interface_ == nullptr)
return true;
return active_interface_->write();
}
void ROSController::reset()
{
if (active_interface_ == nullptr)
return;
active_interface_->reset();
}
void ROSController::read(RTShared& packet)
{
joint_interface_.update(packet);
wrench_interface_.update(packet);
}
bool ROSController::update()
{
auto time = ros::Time::now();
auto diff = time - lastUpdate_;
lastUpdate_ = time;
controller_.update(time, diff, !service_enabled_);
// emergency stop and such should not kill the pipeline
// but still prevent writes
if (!service_enabled_)
{
reset();
return true;
}
// allow the controller to update x times before allowing writes again
if (service_cooldown_ > 0)
{
service_cooldown_ -= 1;
return true;
}
return write();
}
void ROSController::onTimeout()
{
update();
}
void ROSController::onRobotStateChange(RobotState state)
{
bool next = (state == RobotState::Running);
if (next == service_enabled_)
return;
service_enabled_ = next;
service_cooldown_ = 125;
}

88
src/ros/hardware_interface.cpp Normal file
View File

@@ -0,0 +1,88 @@
#include "ur_modern_driver/ros/hardware_interface.h"
#include "ur_modern_driver/log.h"
const std::string JointInterface::INTERFACE_NAME = "hardware_interface::JointStateInterface";
JointInterface::JointInterface(std::vector<std::string> &joint_names)
{
for (size_t i = 0; i < 6; i++)
{
registerHandle(hardware_interface::JointStateHandle(joint_names[i], &positions_[i], &velocities_[i], &efforts_[i]));
}
}
void JointInterface::update(RTShared &packet)
{
positions_ = packet.q_actual;
velocities_ = packet.qd_actual;
efforts_ = packet.i_actual;
}
const std::string WrenchInterface::INTERFACE_NAME = "hardware_interface::ForceTorqueSensorInterface";
WrenchInterface::WrenchInterface(std::string tcp_link)
{
registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", tcp_link, tcp_.begin(), tcp_.begin() + 3));
}
void WrenchInterface::update(RTShared &packet)
{
tcp_ = packet.tcp_force;
}
const std::string VelocityInterface::INTERFACE_NAME = "hardware_interface::VelocityJointInterface";
VelocityInterface::VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface,
std::vector<std::string> &joint_names, double max_vel_change)
: commander_(commander), max_vel_change_(max_vel_change), prev_velocity_cmd_({ 0, 0, 0, 0, 0, 0 })
{
for (size_t i = 0; i < 6; i++)
{
registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &velocity_cmd_[i]));
}
}
bool VelocityInterface::write()
{
for (size_t i = 0; i < 6; i++)
{
// clamp value to ±max_vel_change
double prev = prev_velocity_cmd_[i];
double lo = prev - max_vel_change_;
double hi = prev + max_vel_change_;
prev_velocity_cmd_[i] = std::max(lo, std::min(velocity_cmd_[i], hi));
}
return commander_.speedj(prev_velocity_cmd_, max_vel_change_);
}
void VelocityInterface::reset()
{
for (auto &val : prev_velocity_cmd_)
{
val = 0;
}
}
const std::string PositionInterface::INTERFACE_NAME = "hardware_interface::PositionJointInterface";
PositionInterface::PositionInterface(TrajectoryFollower &follower,
hardware_interface::JointStateInterface &js_interface,
std::vector<std::string> &joint_names)
: follower_(follower)
{
for (size_t i = 0; i < 6; i++)
{
registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &position_cmd_[i]));
}
}
bool PositionInterface::write()
{
return follower_.execute(position_cmd_);
}
void PositionInterface::start()
{
follower_.start();
}
void PositionInterface::stop()
{
follower_.stop();
}

386
src/ros/lowbandwidth_trajectory_follower.cpp Normal file
View File

@@ -0,0 +1,386 @@
#include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h"
#include <endian.h>
#include <cmath>
#include <ros/ros.h>
static const std::array<double, 6> EMPTY_VALUES = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
static const std::string TIME_INTERVAL("{{TIME_INTERVAL}}");
static const std::string SERVOJ_TIME("{{SERVOJ_TIME}}");
static const std::string SERVOJ_TIME_WAITING("{{SERVOJ_TIME_WAITING}}");
static const std::string MAX_WAITING_TIME("{{MAX_WAITING_TIME}}");
static const std::string SERVOJ_GAIN("{{SERVOJ_GAIN}}");
static const std::string SERVOJ_LOOKAHEAD_TIME("{{SERVOJ_LOOKAHEAD_TIME}}");
static const std::string REVERSE_IP("{{REVERSE_IP}}");
static const std::string REVERSE_PORT("{{REVERSE_PORT}}");
static const std::string MAX_JOINT_DIFFERENCE("{{MAX_JOINT_DIFFERENCE}}");
static const std::string POSITION_PROGRAM = R"(
def driveRobotLowBandwidthTrajectory():
global JOINT_NUM = 6
global TIME_INTERVAL = {{TIME_INTERVAL}}
global SERVOJ_TIME = {{SERVOJ_TIME}}
global SERVOJ_TIME_WAITING = {{SERVOJ_TIME_WAITING}}
global MAX_WAITING_TIME = {{MAX_WAITING_TIME}}
global EMPTY_VALUES = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
global SERVOJ_GAIN = {{SERVOJ_GAIN}}
global SERVOJ_LOOKAHEAD_TIME = {{SERVOJ_LOOKAHEAD_TIME}}
global CONNECTION_NAME = "reverse_connection"
global REVERSE_IP = "{{REVERSE_IP}}"
global REVERSE_PORT = {{REVERSE_PORT}}
global MAX_JOINT_DIFFERENCE = {{MAX_JOINT_DIFFERENCE}}
global g_position_previous = EMPTY_VALUES
global g_position_target = EMPTY_VALUES
global g_position_next = EMPTY_VALUES
global g_velocity_previous = EMPTY_VALUES
global g_velocity_target = EMPTY_VALUES
global g_velocity_next = EMPTY_VALUES
global g_time_previous = 0.0
global g_time_target = 0.0
global g_time_next = 0.0
global g_num_previous = -1
global g_num_target = -1
global g_num_next = -1
global g_received_waypoints_number = -1
global g_requested_waypoints_number = -1
global g_total_elapsed_time = 0
global g_stopping = False
def send_message(message):
socket_send_string(message, CONNECTION_NAME)
socket_send_byte(10, CONNECTION_NAME)
end
def is_waypoint_sentinel(waypoint):
local l_previous_index = 2
while l_previous_index < 1 + JOINT_NUM * 2 + 2:
if waypoint[l_previous_index] != 0.0:
return False
end
l_previous_index = l_previous_index + 1
end
return True
end
def is_final_position_reached(position):
local l_current_position = get_actual_joint_positions()
local l_index = 0
while l_index < JOINT_NUM:
if norm(position[l_index] - l_current_position[l_index]) > MAX_JOINT_DIFFERENCE:
return False
end
l_index = l_index + 1
end
return True
end
def interpolate(time_within_segment, total_segment_time, start_pos, l_end_pos, l_start_vel, end_vel):
local a = start_pos
local b = l_start_vel
local c = (-3 * a + 3 * l_end_pos - 2 * total_segment_time * b - total_segment_time * end_vel) / pow(total_segment_time, 2)
local d = (2 * a - 2 * l_end_pos + total_segment_time * b + total_segment_time * end_vel) / pow(total_segment_time, 3)
return a + b * time_within_segment + c * pow(time_within_segment, 2) + d * pow(time_within_segment, 3)
end
def add_next_waypoint(waypoint):
enter_critical
g_position_previous = g_position_target
g_velocity_previous = g_velocity_target
g_time_previous = g_time_target
g_num_previous = g_num_target
g_position_target = g_position_next
g_velocity_target = g_velocity_next
g_time_target = g_time_next
g_num_target = g_num_next
g_num_next = waypoint[1]
g_position_next = [waypoint[2], waypoint[3], waypoint[4], waypoint[5], waypoint[6], waypoint[7]]
g_velocity_next = [waypoint[8], waypoint[9], waypoint[10], waypoint[11], waypoint[12], waypoint[13]]
g_time_next = waypoint[14]
g_received_waypoints_number = g_num_next
exit_critical
end
thread controllingThread():
local l_received_waypoints_number = -1
local l_requested_waypoints_number = -1
local l_stopped = False
local l_current_position = get_actual_joint_positions()
enter_critical
g_requested_waypoints_number = 2
exit_critical
while True:
enter_critical
l_requested_waypoints_number = g_requested_waypoints_number
exit_critical
local l_max_waiting_time_left = MAX_WAITING_TIME
while l_received_waypoints_number < l_requested_waypoints_number and l_max_waiting_time_left > 0:
servoj(l_current_position,t=SERVOJ_TIME_WAITING,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN)
enter_critical
l_received_waypoints_number = g_received_waypoints_number
exit_critical
l_max_waiting_time_left = l_max_waiting_time_left - SERVOJ_TIME_WAITING
end
if l_max_waiting_time_left <= 0:
textmsg("Closing the connection on waiting too long.")
socket_close(CONNECTION_NAME)
halt
end
enter_critical
local l_start_pos = g_position_previous
local l_start_vel = g_velocity_previous
local l_start_time = g_time_previous
local l_start_num= g_num_previous
local l_end_pos = g_position_target
local l_end_vel = g_velocity_target
local l_end_time = g_time_target
local l_end_num = g_num_target
local l_total_elapsed_time = g_total_elapsed_time
local l_stopping_after_next_interpolation = g_stopping
g_requested_waypoints_number = g_requested_waypoints_number + 1
exit_critical
l_current_position = l_start_pos
local l_total_segment_time = l_end_time - l_start_time
while l_total_elapsed_time <= l_end_time:
local l_segment_elapsed_time = l_total_elapsed_time - l_start_time
j = 0
while j < JOINT_NUM:
l_current_position[j] = interpolate(l_segment_elapsed_time, l_total_segment_time, l_start_pos[j], l_end_pos[j], l_start_vel[j], l_end_vel[j])
j = j + 1
end
servoj(l_current_position,t=SERVOJ_TIME,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN)
enter_critical
g_total_elapsed_time = g_total_elapsed_time + TIME_INTERVAL
l_total_elapsed_time = g_total_elapsed_time
exit_critical
end
if l_stopping_after_next_interpolation:
while not is_final_position_reached(l_end_pos):
textmsg("Catching up on final position not reached first time.")
servoj(l_end_pos,t=SERVOJ_TIME,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN)
end
textmsg("Finishing the controlling thread. Final position reached.")
break
end
end
end
thread sendingThread():
local controlling_thread = run controllingThread()
local l_sent_waypoints_number = -1
local l_requested_waypoints_number = -1
local l_stopping = False
enter_critical
l_requested_waypoints_number = g_requested_waypoints_number
l_stopping = g_stopping
exit_critical
while not l_stopping:
while l_sent_waypoints_number == l_requested_waypoints_number and not l_stopping:
sleep(SERVOJ_TIME_WAITING)
enter_critical
l_requested_waypoints_number = g_requested_waypoints_number
l_stopping = g_stopping
exit_critical
end
if l_stopping:
break
end
send_message(l_sent_waypoints_number + 1)
l_sent_waypoints_number = l_sent_waypoints_number + 1
end
join controlling_thread
end
thread receivingThread():
local sending_thread = run sendingThread()
while True:
waypoint_received = socket_read_ascii_float(14, CONNECTION_NAME)
if waypoint_received[0] == 0:
textmsg("Not received trajectory for the last 2 seconds. Quitting")
enter_critical
g_stopping = True
exit_critical
break
elif waypoint_received[0] != JOINT_NUM * 2 + 2:
textmsg("Received wrong number of floats in trajectory. This is certainly not OK.")
textmsg(waypoint_received[0])
enter_critical
g_stopping = True
exit_critical
break
elif is_waypoint_sentinel(waypoint_received):
add_next_waypoint(waypoint_received)
enter_critical
g_stopping = True
g_received_waypoints_number = g_received_waypoints_number + 1
exit_critical
break
end
add_next_waypoint(waypoint_received)
end
join sending_thread
end
socket_open(REVERSE_IP, REVERSE_PORT, CONNECTION_NAME)
receiving_thread = run receivingThread()
join receiving_thread
socket_close(CONNECTION_NAME)
textmsg("Exiting the program")
end
)";
LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port,
bool version_3)
: running_(false)
, commander_(commander)
, server_(reverse_port)
, time_interval_(0.008)
, servoj_time_(0.008)
, servoj_time_waiting_(0.001)
, max_waiting_time_(2.0)
, servoj_gain_(300.0)
, servoj_lookahead_time_(0.03)
, max_joint_difference_(0.01)
{
ros::param::get("~time_interval", time_interval_);
ros::param::get("~servoj_time", servoj_time_);
ros::param::get("~servoj_time_waiting", servoj_time_waiting_);
ros::param::get("~max_waiting_time", max_waiting_time_);
ros::param::get("~servoj_gain", servoj_gain_);
ros::param::get("~servoj_lookahead_time", servoj_lookahead_time_);
ros::param::get("~max_joint_difference", max_joint_difference_);
std::string res(POSITION_PROGRAM);
std::ostringstream out;
if (!version_3) {
LOG_ERROR("Low Bandwidth Trajectory Follower only works for interface version > 3");
std::exit(-1);
}
res.replace(res.find(TIME_INTERVAL), TIME_INTERVAL.length(), std::to_string(time_interval_));
res.replace(res.find(SERVOJ_TIME_WAITING), SERVOJ_TIME_WAITING.length(), std::to_string(servoj_time_waiting_));
res.replace(res.find(SERVOJ_TIME), SERVOJ_TIME.length(), std::to_string(servoj_time_));
res.replace(res.find(MAX_WAITING_TIME), MAX_WAITING_TIME.length(), std::to_string(max_waiting_time_));
res.replace(res.find(SERVOJ_GAIN), SERVOJ_GAIN.length(), std::to_string(servoj_gain_));
res.replace(res.find(SERVOJ_LOOKAHEAD_TIME), SERVOJ_LOOKAHEAD_TIME.length(), std::to_string(servoj_lookahead_time_));
res.replace(res.find(REVERSE_IP), REVERSE_IP.length(), reverse_ip);
res.replace(res.find(REVERSE_PORT), REVERSE_PORT.length(), std::to_string(reverse_port));
res.replace(res.find(MAX_JOINT_DIFFERENCE), MAX_JOINT_DIFFERENCE.length(), std::to_string(max_joint_difference_));
program_ = res;
if (!server_.bind())
{
LOG_ERROR("Failed to bind server, the port %d is likely already in use", reverse_port);
std::exit(-1);
}
LOG_INFO("Low Bandwidth Trajectory Follower is initialized!");
}
bool LowBandwidthTrajectoryFollower::start()
{
LOG_INFO("Starting LowBandwidthTrajectoryFollower");
if (running_)
return true; // not sure
LOG_INFO("Uploading trajectory program to robot");
if (!commander_.uploadProg(program_))
{
LOG_ERROR("Program upload failed!");
return false;
}
LOG_DEBUG("Awaiting incoming robot connection");
if (!server_.accept())
{
LOG_ERROR("Failed to accept incoming robot connection");
return false;
}
LOG_DEBUG("Robot successfully connected");
return (running_ = true);
}
bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6> &positions,
const std::array<double, 6> &velocities,
double sample_number, double time_in_seconds)
{
if (!running_)
return false;
std::ostringstream out;
out << "(";
out << sample_number << ",";
for (auto const &pos: positions)
{
out << pos << ",";
}
for (auto const &vel: velocities)
{
out << vel << ",";
}
out << time_in_seconds << ")\r\n";
// I know it's ugly but it's the most efficient and fastest way
// We have only ASCII characters and we can cast char -> uint_8
const std::string tmp = out.str();
const char *formatted_message = tmp.c_str();
const uint8_t *buf = (uint8_t *) formatted_message;
size_t written;
LOG_DEBUG("Sending message %s", formatted_message);
return server_.write(buf, strlen(formatted_message) + 1, written);
}
bool LowBandwidthTrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt)
{
if (!running_)
return false;
bool finished = false;
char* line[MAX_SERVER_BUF_LEN];
bool res = true;
while (!finished && !interrupt)
{
if (!server_.readLine((char *)line, MAX_SERVER_BUF_LEN))
{
LOG_DEBUG("Connection closed. Finishing!");
finished = true;
break;
}
unsigned int message_num=atoi((const char *) line);
LOG_DEBUG("Received request %i", message_num);
if (message_num < trajectory.size())
{
res = execute(trajectory[message_num].positions, trajectory[message_num].velocities,
message_num, trajectory[message_num].time_from_start.count() / 1e6);
} else
{
res = execute(EMPTY_VALUES, EMPTY_VALUES, message_num, 0.0);
}
if (!res)
{
finished = true;
}
}
return res;
}
void LowBandwidthTrajectoryFollower::stop()
{
if (!running_)
return;
server_.disconnectClient();
running_ = false;
}

114
src/ros/mb_publisher.cpp Normal file
View File

@@ -0,0 +1,114 @@
#include "ur_modern_driver/ros/mb_publisher.h"
inline void appendAnalog(std::vector<ur_msgs::Analog>& vec, double val, uint8_t pin)
{
ur_msgs::Analog ana;
ana.pin = pin;
ana.state = val;
vec.push_back(ana);
}
void MBPublisher::publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data)
{
appendAnalog(io_msg.analog_in_states, data.analog_input0, 0);
appendAnalog(io_msg.analog_in_states, data.analog_input1, 1);
appendAnalog(io_msg.analog_out_states, data.analog_output0, 0);
appendAnalog(io_msg.analog_out_states, data.analog_output1, 1);
io_pub_.publish(io_msg);
}
void MBPublisher::publishRobotStatus(industrial_msgs::RobotStatus& status, const SharedRobotModeData& data) const
{
//note that this is true as soon as the drives are powered,
//even if the breakes are still closed
//which is in slight contrast to the comments in the
//message definition
status.drives_powered.val = data.robot_power_on;
status.e_stopped.val = data.emergency_stopped;
//I found no way to reliably get information if the robot is moving
//data.programm_running would be true when using this driver to move the robot
//but it would also be true when another programm is running that might not
//in fact contain any movement commands
status.in_motion.val = industrial_msgs::TriState::UNKNOWN;
//the error code, if any, is not transmitted by this protocol
//it can and should be fetched seperately
status.error_code = 0;
//note that e-stop is handled by a seperate variable
status.in_error.val = data.protective_stopped;
status_pub_.publish(status);
}
void MBPublisher::publishRobotStatus(const RobotModeData_V1_X& data) const
{
industrial_msgs::RobotStatus msg;
if (data.robot_mode == robot_mode_V1_X::ROBOT_FREEDRIVE_MODE)
msg.mode.val = industrial_msgs::RobotMode::MANUAL;
else
msg.mode.val = industrial_msgs::RobotMode::AUTO;
//todo: verify that this correct, there is also ROBOT_READY_MODE
msg.motion_possible.val = (data.robot_mode == robot_mode_V1_X::ROBOT_RUNNING_MODE)
? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF;
publishRobotStatus(msg, data);
}
void MBPublisher::publishRobotStatus(const RobotModeData_V3_0__1& data) const
{
industrial_msgs::RobotStatus msg;
msg.motion_possible.val = (data.robot_mode == robot_mode_V3_X::RUNNING)
? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF;
if (data.control_mode == robot_control_mode_V3_X::TEACH)
msg.mode.val = industrial_msgs::RobotMode::MANUAL;
else
msg.mode.val = industrial_msgs::RobotMode::AUTO;
publishRobotStatus(msg, data);
}
bool MBPublisher::consume(MasterBoardData_V1_X& data)
{
ur_msgs::IOStates io_msg;
appendDigital(io_msg.digital_in_states, data.digital_input_bits);
appendDigital(io_msg.digital_out_states, data.digital_output_bits);
publish(io_msg, data);
return true;
}
bool MBPublisher::consume(MasterBoardData_V3_0__1& data)
{
ur_msgs::IOStates io_msg;
appendDigital(io_msg.digital_in_states, data.digital_input_bits);
appendDigital(io_msg.digital_out_states, data.digital_output_bits);
publish(io_msg, data);
return true;
}
bool MBPublisher::consume(MasterBoardData_V3_2& data)
{
consume(static_cast<MasterBoardData_V3_0__1&>(data));
return true;
}
bool MBPublisher::consume(RobotModeData_V1_X& data)
{
publishRobotStatus(data);
return true;
}
bool MBPublisher::consume(RobotModeData_V3_0__1& data)
{
publishRobotStatus(data);
return true;
}
bool MBPublisher::consume(RobotModeData_V3_2& data)
{
publishRobotStatus(data);
return true;
}

119
src/ros/rt_publisher.cpp Normal file
View File

@@ -0,0 +1,119 @@
#include "ur_modern_driver/ros/rt_publisher.h"
bool RTPublisher::publishJoints(RTShared& packet, Time& t)
{
sensor_msgs::JointState joint_msg;
joint_msg.header.stamp = t;
joint_msg.name.assign(joint_names_.begin(), joint_names_.end());
joint_msg.position.assign(packet.q_actual.begin(), packet.q_actual.end());
joint_msg.velocity.assign(packet.qd_actual.begin(), packet.qd_actual.end());
joint_msg.effort.assign(packet.i_actual.begin(), packet.i_actual.end());
joint_pub_.publish(joint_msg);
return true;
}
bool RTPublisher::publishWrench(RTShared& packet, Time& t)
{
geometry_msgs::WrenchStamped wrench_msg;
wrench_msg.header.stamp = t;
wrench_msg.wrench.force.x = packet.tcp_force[0];
wrench_msg.wrench.force.y = packet.tcp_force[1];
wrench_msg.wrench.force.z = packet.tcp_force[2];
wrench_msg.wrench.torque.x = packet.tcp_force[3];
wrench_msg.wrench.torque.y = packet.tcp_force[4];
wrench_msg.wrench.torque.z = packet.tcp_force[5];
wrench_pub_.publish(wrench_msg);
return true;
}
bool RTPublisher::publishTool(RTShared& packet, Time& t)
{
geometry_msgs::TwistStamped tool_twist;
tool_twist.header.stamp = t;
tool_twist.header.frame_id = base_frame_;
tool_twist.twist.linear.x = packet.tcp_speed_actual.position.x;
tool_twist.twist.linear.y = packet.tcp_speed_actual.position.y;
tool_twist.twist.linear.z = packet.tcp_speed_actual.position.z;
tool_twist.twist.angular.x = packet.tcp_speed_actual.rotation.x;
tool_twist.twist.angular.y = packet.tcp_speed_actual.rotation.y;
tool_twist.twist.angular.z = packet.tcp_speed_actual.rotation.z;
tool_vel_pub_.publish(tool_twist);
return true;
}
bool RTPublisher::publishTransform(RTShared& packet, Time& t)
{
auto tv = packet.tool_vector_actual;
Transform transform;
transform.setOrigin(Vector3(tv.position.x, tv.position.y, tv.position.z));
// Create quaternion
Quaternion quat;
double angle = std::sqrt(std::pow(tv.rotation.x, 2) + std::pow(tv.rotation.y, 2) + std::pow(tv.rotation.z, 2));
if (angle < 1e-16)
{
quat.setValue(0, 0, 0, 1);
}
else
{
quat.setRotation(Vector3(tv.rotation.x / angle, tv.rotation.y / angle, tv.rotation.z / angle), angle);
}
transform.setRotation(quat);
transform_broadcaster_.sendTransform(StampedTransform(transform, t, base_frame_, tool_frame_));
return true;
}
bool RTPublisher::publishTemperature(RTShared& packet, Time& t)
{
size_t len = joint_names_.size();
for (size_t i = 0; i < len; i++)
{
sensor_msgs::Temperature msg;
msg.header.stamp = t;
msg.header.frame_id = joint_names_[i];
msg.temperature = packet.motor_temperatures[i];
joint_temperature_pub_.publish(msg);
}
return true;
}
bool RTPublisher::publish(RTShared& packet)
{
Time time = Time::now();
bool res = true;
if (!temp_only_)
{
res = publishJoints(packet, time) && publishWrench(packet, time) && publishTool(packet, time) &&
publishTransform(packet, time);
}
return res && publishTemperature(packet, time);
}
bool RTPublisher::consume(RTState_V1_6__7& state)
{
return publish(state);
}
bool RTPublisher::consume(RTState_V1_8& state)
{
return publish(state);
}
bool RTPublisher::consume(RTState_V3_0__1& state)
{
return publish(state);
}
bool RTPublisher::consume(RTState_V3_2__3& state)
{
return publish(state);
}

75
src/ros/service_stopper.cpp Normal file
View File

@@ -0,0 +1,75 @@
#include "ur_modern_driver/ros/service_stopper.h"
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;
}

243
src/ros/trajectory_follower.cpp Normal file
View File

@@ -0,0 +1,243 @@
#include "ur_modern_driver/ros/trajectory_follower.h"
#include <endian.h>
#include <cmath>
#include <ros/ros.h>
static const int32_t MULT_JOINTSTATE_ = 1000000;
static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}");
static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}");
static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}");
static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}");
static const std::string POSITION_PROGRAM = R"(
def driverProg():
MULT_jointstate = {{JOINT_STATE_REPLACE}}
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, {{SERVO_J_REPLACE}})
else:
sync()
end
end
end
socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}})
thread_servo = run servoThread()
keepalive = 1
while keepalive > 0:
params_mult = socket_read_binary_integer(6+1)
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]
keepalive = params_mult[7]
set_servo_setpoint(q)
end
end
sleep(.1)
socket_close()
kill thread_servo
end
)";
TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port,
bool version_3)
: running_(false)
, commander_(commander)
, server_(reverse_port)
, servoj_time_(0.008)
, servoj_lookahead_time_(0.03)
, servoj_gain_(300.)
{
ros::param::get("~servoj_time", servoj_time_);
ros::param::get("~servoj_lookahead_time", servoj_lookahead_time_);
ros::param::get("~servoj_gain", servoj_gain_);
std::string res(POSITION_PROGRAM);
res.replace(res.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_));
std::ostringstream out;
out << "t=" << std::fixed << std::setprecision(4) << servoj_time_;
if (version_3)
out << ", lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_;
res.replace(res.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
res.replace(res.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), reverse_ip);
res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port));
program_ = res;
if (!server_.bind())
{
LOG_ERROR("Failed to bind server, the port %d is likely already in use", reverse_port);
std::exit(-1);
}
}
bool TrajectoryFollower::start()
{
if (running_)
return true; // not sure
LOG_INFO("Uploading trajectory program to robot");
if (!commander_.uploadProg(program_))
{
LOG_ERROR("Program upload failed!");
return false;
}
LOG_DEBUG("Awaiting incoming robot connection");
if (!server_.accept())
{
LOG_ERROR("Failed to accept incoming robot connection");
return false;
}
LOG_DEBUG("Robot successfully connected");
return (running_ = true);
}
bool TrajectoryFollower::execute(std::array<double, 6> &positions, bool keep_alive)
{
if (!running_)
return false;
// LOG_INFO("servoj([%f,%f,%f,%f,%f,%f])", positions[0], positions[1], positions[2], positions[3], positions[4],
// positions[5]);
last_positions_ = positions;
uint8_t buf[sizeof(uint32_t) * 7];
uint8_t *idx = buf;
for (auto const &pos : positions)
{
int32_t val = static_cast<int32_t>(pos * MULT_JOINTSTATE_);
val = htobe32(val);
idx += append(idx, val);
}
int32_t val = htobe32(static_cast<int32_t>(keep_alive));
append(idx, val);
size_t written;
return server_.write(buf, sizeof(buf), written);
}
double TrajectoryFollower::interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel)
{
using std::pow;
double a = p0_pos;
double b = p0_vel;
double c = (-3 * a + 3 * p1_pos - 2 * T * b - T * p1_vel) / pow(T, 2);
double d = (2 * a - 2 * p1_pos + T * b + T * p1_vel) / pow(T, 3);
return a + b * t + c * pow(t, 2) + d * pow(t, 3);
}
bool TrajectoryFollower::execute(std::array<double, 6> &positions)
{
return execute(positions, true);
}
bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt)
{
if (!running_)
return false;
using namespace std::chrono;
typedef duration<double> double_seconds;
typedef high_resolution_clock Clock;
typedef Clock::time_point Time;
auto &last = trajectory[trajectory.size() - 1];
auto &prev = trajectory[0];
Time t0 = Clock::now();
Time latest = t0;
std::array<double, 6> positions;
for (auto const &point : trajectory)
{
// skip t0
if (&point == &prev)
continue;
if (interrupt)
break;
auto duration = point.time_from_start - prev.time_from_start;
double d_s = duration_cast<double_seconds>(duration).count();
// interpolation loop
while (!interrupt)
{
latest = Clock::now();
auto elapsed = latest - t0;
if (point.time_from_start <= elapsed)
break;
if (last.time_from_start <= elapsed)
return true;
double elapsed_s = duration_cast<double_seconds>(elapsed - prev.time_from_start).count();
for (size_t j = 0; j < positions.size(); j++)
{
positions[j] =
interpolate(elapsed_s, d_s, prev.positions[j], point.positions[j], prev.velocities[j], point.velocities[j]);
}
if (!execute(positions, true))
return false;
std::this_thread::sleep_for(std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.)));
}
prev = point;
}
// In theory it's possible the last position won't be sent by
// the interpolation loop above but rather some position between
// t[N-1] and t[N] where N is the number of trajectory points.
// To make sure this does not happen the last position is sent
return execute(last.positions, true);
}
void TrajectoryFollower::stop()
{
if (!running_)
return;
// std::array<double, 6> empty;
// execute(empty, false);
server_.disconnectClient();
running_ = false;
}

46
src/ros/urscript_handler.cpp Normal file
View File

@@ -0,0 +1,46 @@
#include "ur_modern_driver/ros/urscript_handler.h"
#include "ur_modern_driver/log.h"
URScriptHandler::URScriptHandler(URCommander &commander)
: commander_(commander)
, state_(RobotState::Error)
{
LOG_INFO("Initializing ur_driver/URScript subscriber");
urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, &URScriptHandler::urscriptInterface, this);
LOG_INFO("The ur_driver/URScript initialized");
}
void URScriptHandler::urscriptInterface(const std_msgs::String::ConstPtr& msg)
{
LOG_INFO("Message received");
std::string str(msg->data);
if (str.back() != '\n')
str.append("\n");
switch (state_)
{
case RobotState::Running:
if (!commander_.uploadProg(str))
{
LOG_ERROR("Program upload failed!");
}
break;
case RobotState::EmergencyStopped:
LOG_ERROR("Robot is emergency stopped");
break;
case RobotState::ProtectiveStopped:
LOG_ERROR("Robot is protective stopped");
break;
case RobotState::Error:
LOG_ERROR("Robot is not ready, check robot_mode");
break;
default:
LOG_ERROR("Robot is in undefined state");
break;
}
}
void URScriptHandler::onRobotStateChange(RobotState state)
{
state_ = state;
}

225
src/ros_main.cpp Normal file
View File

@@ -0,0 +1,225 @@
#include <ros/ros.h>
#include <chrono>
#include <cstdlib>
#include <string>
#include <thread>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ros/action_server.h"
#include "ur_modern_driver/ros/controller.h"
#include "ur_modern_driver/ros/io_service.h"
#include "ur_modern_driver/ros/mb_publisher.h"
#include "ur_modern_driver/ros/rt_publisher.h"
#include "ur_modern_driver/ros/service_stopper.h"
#include "ur_modern_driver/ros/trajectory_follower.h"
#include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h"
#include "ur_modern_driver/ros/urscript_handler.h"
#include "ur_modern_driver/ur/commander.h"
#include "ur_modern_driver/ur/factory.h"
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/producer.h"
#include "ur_modern_driver/ur/rt_state.h"
#include "ur_modern_driver/ur/state.h"
static const std::string IP_ADDR_ARG("~robot_ip_address");
static const std::string REVERSE_PORT_ARG("~reverse_port");
static const std::string ROS_CONTROL_ARG("~use_ros_control");
static const std::string LOW_BANDWIDTH_TRAJECTORY_FOLLOWER("~use_lowbandwidth_trajectory_follower");
static const std::string MAX_VEL_CHANGE_ARG("~max_vel_change");
static const std::string PREFIX_ARG("~prefix");
static const std::string BASE_FRAME_ARG("~base_frame");
static const std::string TOOL_FRAME_ARG("~tool_frame");
static const std::string TCP_LINK_ARG("~tcp_link");
static const std::string JOINT_NAMES_PARAM("hardware_interface/joints");
static const std::string SHUTDOWN_ON_DISCONNECT_ARG("~shutdown_on_disconnect");
static const std::vector<std::string> DEFAULT_JOINTS = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint" };
static const int UR_SECONDARY_PORT = 30002;
static const int UR_RT_PORT = 30003;
struct ProgArgs
{
public:
std::string host;
std::string prefix;
std::string base_frame;
std::string tool_frame;
std::string tcp_link;
std::vector<std::string> joint_names;
double max_acceleration;
double max_velocity;
double max_vel_change;
int32_t reverse_port;
bool use_ros_control;
bool use_lowbandwidth_trajectory_follower;
bool shutdown_on_disconnect;
};
class IgnorePipelineStoppedNotifier : public INotifier
{
public:
void started(std::string name){
LOG_INFO("Starting pipeline %s", name.c_str());
}
void stopped(std::string name){
LOG_INFO("Stopping pipeline %s", name.c_str());
}
};
class ShutdownOnPipelineStoppedNotifier : public INotifier
{
public:
void started(std::string name){
LOG_INFO("Starting pipeline %s", name.c_str());
}
void stopped(std::string name){
LOG_INFO("Shutting down on stopped pipeline %s", name.c_str());
ros::shutdown();
exit(1);
}
};
bool parse_args(ProgArgs &args)
{
if (!ros::param::get(IP_ADDR_ARG, args.host))
{
LOG_ERROR("robot_ip_address parameter must be set!");
return false;
}
ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001));
ros::param::param(MAX_VEL_CHANGE_ARG, args.max_vel_change, 15.0); // rad/s
ros::param::param(MAX_VEL_CHANGE_ARG, args.max_velocity, 10.0);
ros::param::param(ROS_CONTROL_ARG, args.use_ros_control, false);
ros::param::param(LOW_BANDWIDTH_TRAJECTORY_FOLLOWER, args.use_lowbandwidth_trajectory_follower, false);
ros::param::param(PREFIX_ARG, args.prefix, std::string());
ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link");
ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller");
ros::param::param(TCP_LINK_ARG, args.tcp_link, args.prefix + "tool0");
ros::param::param(JOINT_NAMES_PARAM, args.joint_names, DEFAULT_JOINTS);
ros::param::param(SHUTDOWN_ON_DISCONNECT_ARG, args.shutdown_on_disconnect, true);
return true;
}
std::string getLocalIPAccessibleFromHost(std::string &host)
{
URStream stream(host, UR_RT_PORT);
return stream.connect() ? stream.getIP() : std::string();
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "ur_driver");
ProgArgs args;
if (!parse_args(args))
{
return EXIT_FAILURE;
}
//Add prefix to joint names
std::transform (args.joint_names.begin(), args.joint_names.end(), args.joint_names.begin(),
[&args](std::string name){return args.prefix + name;});
std::string local_ip(getLocalIPAccessibleFromHost(args.host));
URFactory factory(args.host);
vector<Service *> services;
// RT packets
auto rt_parser = factory.getRTParser();
URStream rt_stream(args.host, UR_RT_PORT);
URProducer<RTPacket> rt_prod(rt_stream, *rt_parser);
RTPublisher rt_pub(args.prefix, args.base_frame, args.tool_frame, args.use_ros_control);
auto rt_commander = factory.getCommander(rt_stream);
vector<IConsumer<RTPacket> *> rt_vec{ &rt_pub };
INotifier *notifier(nullptr);
ROSController *controller(nullptr);
ActionServer *action_server(nullptr);
if (args.use_ros_control)
{
LOG_INFO("ROS control enabled");
TrajectoryFollower *traj_follower = new TrajectoryFollower(
*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
controller = new ROSController(*rt_commander, *traj_follower, args.joint_names, args.max_vel_change, args.tcp_link);
rt_vec.push_back(controller);
services.push_back(controller);
}
else
{
LOG_INFO("ActionServer enabled");
ActionTrajectoryFollowerInterface *traj_follower(nullptr);
if (args.use_lowbandwidth_trajectory_follower)
{
LOG_INFO("Use low bandwidth trajectory follower");
traj_follower = new LowBandwidthTrajectoryFollower(*rt_commander,
local_ip, args.reverse_port,factory.isVersion3());
}
else
{
LOG_INFO("Use standard trajectory follower");
traj_follower = new TrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
}
action_server = new ActionServer(*traj_follower, args.joint_names, args.max_velocity);
rt_vec.push_back(action_server);
services.push_back(action_server);
}
URScriptHandler urscript_handler(*rt_commander);
services.push_back(&urscript_handler);
if (args.shutdown_on_disconnect)
{
LOG_INFO("Notifier: Pipeline disconnect will shutdown the node");
notifier = new ShutdownOnPipelineStoppedNotifier();
}
else
{
LOG_INFO("Notifier: Pipeline disconnect will be ignored.");
notifier = new IgnorePipelineStoppedNotifier();
}
MultiConsumer<RTPacket> rt_cons(rt_vec);
Pipeline<RTPacket> rt_pl(rt_prod, rt_cons, "RTPacket", *notifier);
// Message packets
auto state_parser = factory.getStateParser();
URStream state_stream(args.host, UR_SECONDARY_PORT);
URProducer<StatePacket> state_prod(state_stream, *state_parser);
MBPublisher state_pub;
ServiceStopper service_stopper(services);
vector<IConsumer<StatePacket> *> state_vec{ &state_pub, &service_stopper };
MultiConsumer<StatePacket> state_cons(state_vec);
Pipeline<StatePacket> state_pl(state_prod, state_cons, "StatePacket", *notifier);
LOG_INFO("Starting main loop");
rt_pl.run();
state_pl.run();
auto state_commander = factory.getCommander(state_stream);
IOService io_service(*state_commander);
if (action_server)
action_server->start();
ros::spin();
LOG_INFO("ROS stopping, shutting down pipelines");
rt_pl.stop();
state_pl.stop();
if (controller)
delete controller;
LOG_INFO("Pipelines shutdown complete");
return EXIT_SUCCESS;
}

167
src/tcp_socket.cpp Normal file
View File

@@ -0,0 +1,167 @@
#include <arpa/inet.h>
#include <endian.h>
#include <netinet/tcp.h>
#include <unistd.h>
#include <cstring>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/tcp_socket.h"
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, 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, 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;
}

144
src/ur/commander.cpp Normal file
View File

@@ -0,0 +1,144 @@
#include "ur_modern_driver/ur/commander.h"
#include "ur_modern_driver/log.h"
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);
}

103
src/ur/master_board.cpp Normal file
View File

@@ -0,0 +1,103 @@
#include "ur_modern_driver/ur/master_board.h"
#include "ur_modern_driver/ur/consumer.h"
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);
}

19
src/ur/messages.cpp Normal file
View File

@@ -0,0 +1,19 @@
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/consumer.h"
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.parse_remainder(build_date);
return true; // not really possible to check dynamic size packets
}
bool VersionMessage::consumeWith(URMessagePacketConsumer& consumer)
{
return consumer.consume(*this);
}

84
src/ur/robot_mode.cpp Normal file
View File

@@ -0,0 +1,84 @@
#include "ur_modern_driver/ur/robot_mode.h"
#include "ur_modern_driver/ur/consumer.h"
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);
}

117
src/ur/rt_state.cpp Normal file
View File

@@ -0,0 +1,117 @@
#include "ur_modern_driver/ur/rt_state.h"
#include "ur_modern_driver/ur/consumer.h"
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);
}

129
src/ur/server.cpp Normal file
View File

@@ -0,0 +1,129 @@
#include "ur_modern_driver/ur/server.h"
#include <arpa/inet.h>
#include <netinet/tcp.h>
#include <unistd.h>
#include <cstring>
#include "ur_modern_driver/log.h"
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() != 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() != 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;
}

44
src/ur/stream.cpp Normal file
View File

@@ -0,0 +1,44 @@
#include <endian.h>
#include <netinet/tcp.h>
#include <unistd.h>
#include <cstring>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/stream.h"
bool URStream::write(const uint8_t* buf, size_t buf_len, size_t& written)
{
std::lock_guard<std::mutex> lock(write_mutex_);
return TCPSocket::write(buf, buf_len, written);
}
bool URStream::read(uint8_t* buf, 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(int32_t);
size_t read = 0;
while (remainder > 0 && TCPSocket::read(buf_pos, remainder, read))
{
TCPSocket::setOptions(getSocketFD());
if (initial)
{
remainder = be32toh(*(reinterpret_cast<int32_t*>(buf)));
if (remainder >= (buf_len - sizeof(int32_t)))
{
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;
}

181
src/ur_communication.cpp
View File

@@ -1,181 +0,0 @@
/*
* ur_communication.cpp
*
* Copyright 2015 Thomas Timm Andersen
*
* 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_modern_driver/ur_communication.h"
UrCommunication::UrCommunication(std::condition_variable& msg_cond,
std::string host) {
robot_state_ = new RobotState(msg_cond);
bzero((char *) &pri_serv_addr_, sizeof(pri_serv_addr_));
bzero((char *) &sec_serv_addr_, sizeof(sec_serv_addr_));
pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (pri_sockfd_ < 0) {
print_fatal("ERROR opening socket pri_sockfd");
}
sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sec_sockfd_ < 0) {
print_fatal("ERROR opening socket sec_sockfd");
}
server_ = gethostbyname(host.c_str());
if (server_ == NULL) {
print_fatal("ERROR, unknown host");
}
pri_serv_addr_.sin_family = AF_INET;
sec_serv_addr_.sin_family = AF_INET;
bcopy((char *) server_->h_addr, (char *)&pri_serv_addr_.sin_addr.s_addr, server_->h_length);
bcopy((char *) server_->h_addr, (char *)&sec_serv_addr_.sin_addr.s_addr, server_->h_length);
pri_serv_addr_.sin_port = htons(30001);
sec_serv_addr_.sin_port = htons(30002);
flag_ = 1;
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
sizeof(int));
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_,
sizeof(int));
setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_,
sizeof(int));
setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
sizeof(int));
fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
connected_ = false;
keepalive_ = false;
}
bool UrCommunication::start() {
keepalive_ = true;
uint8_t buf[512];
unsigned int bytes_read;
std::string cmd;
bzero(buf, 512);
print_debug("Acquire firmware version: Connecting...");
if (connect(pri_sockfd_, (struct sockaddr *) &pri_serv_addr_,
sizeof(pri_serv_addr_)) < 0) {
print_fatal("Error connecting to get firmware version");
return false;
}
print_debug("Acquire firmware version: Got connection");
bytes_read = read(pri_sockfd_, buf, 512);
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_,
sizeof(int));
robot_state_->unpack(buf, bytes_read);
//wait for some traffic so the UR socket doesn't die in version 3.1.
std::this_thread::sleep_for(std::chrono::milliseconds(500));
char tmp[64];
sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion());
print_debug(tmp);
close(pri_sockfd_);
print_debug(
"Switching to secondary interface for masterboard data: Connecting...");
fd_set writefds;
struct timeval timeout;
connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_,
sizeof(sec_serv_addr_));
FD_ZERO(&writefds);
FD_SET(sec_sockfd_, &writefds);
timeout.tv_sec = 10;
timeout.tv_usec = 0;
select(sec_sockfd_ + 1, NULL, &writefds, NULL, &timeout);
unsigned int flag_len;
getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
if (flag_ < 0) {
print_fatal("Error connecting to secondary interface");
return false;
}
print_debug("Secondary interface: Got connection");
comThread_ = std::thread(&UrCommunication::run, this);
return true;
}
void UrCommunication::halt() {
keepalive_ = false;
comThread_.join();
}
void UrCommunication::run() {
uint8_t buf[2048];
int bytes_read;
bzero(buf, 2048);
struct timeval timeout;
fd_set readfds;
FD_ZERO(&readfds);
FD_SET(sec_sockfd_, &readfds);
connected_ = true;
while (keepalive_) {
while (connected_ && keepalive_) {
timeout.tv_sec = 0; //do this each loop as selects modifies timeout
timeout.tv_usec = 500000; // timeout of 0.5 sec
select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout);
bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes
if (bytes_read > 0) {
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK,
(char *) &flag_, sizeof(int));
robot_state_->unpack(buf, bytes_read);
} else {
connected_ = false;
robot_state_->setDisconnected();
close(sec_sockfd_);
}
}
if (keepalive_) {
//reconnect
print_warning("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sec_sockfd_ < 0) {
print_fatal("ERROR opening secondary socket");
}
flag_ = 1;
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_,
sizeof(int));
setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
sizeof(int));
fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
while (keepalive_ && !connected_) {
std::this_thread::sleep_for(std::chrono::seconds(10));
fd_set writefds;
connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_,
sizeof(sec_serv_addr_));
FD_ZERO(&writefds);
FD_SET(sec_sockfd_, &writefds);
select(sec_sockfd_ + 1, NULL, &writefds, NULL, NULL);
unsigned int flag_len;
getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_,
&flag_len);
if (flag_ < 0) {
print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds...");
} else {
connected_ = true;
print_info("Secondary port: Reconnected");
}
}
}
}
//wait for some traffic so the UR socket doesn't die in version 3.1.
std::this_thread::sleep_for(std::chrono::milliseconds(500));
close(sec_sockfd_);
}

382
src/ur_driver.cpp
View File

@@ -1,382 +0,0 @@
/*
* ur_driver.cpp
*
* Copyright 2015 Thomas Timm Andersen
*
* 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_modern_driver/ur_driver.h"
UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
std::condition_variable& msg_cond, std::string host,
unsigned int reverse_port, double servoj_time,
unsigned int safety_count_max, double max_time_step, double min_payload,
double max_payload, double servoj_lookahead_time, double servoj_gain) :
REVERSE_PORT_(reverse_port), maximum_time_step_(max_time_step), minimum_payload_(
min_payload), maximum_payload_(max_payload), servoj_time_(
servoj_time), servoj_lookahead_time_(servoj_lookahead_time), servoj_gain_(servoj_gain) {
char buffer[256];
struct sockaddr_in serv_addr;
int n, flag;
firmware_version_ = 0;
reverse_connected_ = false;
executing_traj_ = false;
rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host,
safety_count_max);
new_sockfd_ = -1;
sec_interface_ = new UrCommunication(msg_cond, host);
incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (incoming_sockfd_ < 0) {
print_fatal("ERROR opening socket for reverse communication");
}
bzero((char *) &serv_addr, sizeof(serv_addr));
serv_addr.sin_family = AF_INET;
serv_addr.sin_addr.s_addr = INADDR_ANY;
serv_addr.sin_port = htons(REVERSE_PORT_);
flag = 1;
setsockopt(incoming_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag,
sizeof(int));
setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int));
if (bind(incoming_sockfd_, (struct sockaddr *) &serv_addr,
sizeof(serv_addr)) < 0) {
print_fatal("ERROR on binding socket for reverse communication");
}
listen(incoming_sockfd_, 5);
}
std::vector<double> UrDriver::interp_cubic(double t, double T,
std::vector<double> p0_pos, std::vector<double> p1_pos,
std::vector<double> p0_vel, std::vector<double> p1_vel) {
/*Returns positions of the joints at time 't' */
std::vector<double> positions;
for (unsigned int i = 0; i < p0_pos.size(); i++) {
double a = p0_pos[i];
double b = p0_vel[i];
double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i]
- T * p1_vel[i]) / pow(T, 2);
double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i]
+ T * p1_vel[i]) / pow(T, 3);
positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3));
}
return positions;
}
bool UrDriver::doTraj(std::vector<double> inp_timestamps,
std::vector<std::vector<double> > inp_positions,
std::vector<std::vector<double> > inp_velocities) {
std::chrono::high_resolution_clock::time_point t0, t;
std::vector<double> positions;
unsigned int j;
if (!UrDriver::uploadProg()) {
return false;
}
executing_traj_ = true;
t0 = std::chrono::high_resolution_clock::now();
t = t0;
j = 0;
while ((inp_timestamps[inp_timestamps.size() - 1]
>= std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count())
and executing_traj_) {
while (inp_timestamps[j]
<= std::chrono::duration_cast<std::chrono::duration<double>>(
t - t0).count() && j < inp_timestamps.size() - 1) {
j += 1;
}
positions = UrDriver::interp_cubic(
std::chrono::duration_cast<std::chrono::duration<double>>(
t - t0).count() - inp_timestamps[j - 1],
inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1],
inp_positions[j], inp_velocities[j - 1], inp_velocities[j]);
UrDriver::servoj(positions);
// oversample with 4 * sample_time
std::this_thread::sleep_for(
std::chrono::milliseconds((int) ((servoj_time_ * 1000) / 4.)));
t = std::chrono::high_resolution_clock::now();
}
executing_traj_ = false;
//Signal robot to stop driverProg()
UrDriver::closeServo(positions);
return true;
}
void UrDriver::servoj(std::vector<double> positions, int keepalive) {
if (!reverse_connected_) {
print_error(
"UrDriver::servoj called without a reverse connection present. Keepalive: "
+ std::to_string(keepalive));
return;
}
unsigned int bytes_written;
int tmp;
unsigned char buf[28];
for (int i = 0; i < 6; i++) {
tmp = htonl((int) (positions[i] * MULT_JOINTSTATE_));
buf[i * 4] = tmp & 0xff;
buf[i * 4 + 1] = (tmp >> 8) & 0xff;
buf[i * 4 + 2] = (tmp >> 16) & 0xff;
buf[i * 4 + 3] = (tmp >> 24) & 0xff;
}
tmp = htonl((int) keepalive);
buf[6 * 4] = tmp & 0xff;
buf[6 * 4 + 1] = (tmp >> 8) & 0xff;
buf[6 * 4 + 2] = (tmp >> 16) & 0xff;
buf[6 * 4 + 3] = (tmp >> 24) & 0xff;
bytes_written = write(new_sockfd_, buf, 28);
}
void UrDriver::stopTraj() {
executing_traj_ = false;
rt_interface_->addCommandToQueue("stopj(10)\n");
}
bool UrDriver::uploadProg() {
std::string cmd_str;
char buf[128];
cmd_str = "def driverProg():\n";
sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_);
cmd_str += buf;
cmd_str += "\tSERVO_IDLE = 0\n";
cmd_str += "\tSERVO_RUNNING = 1\n";
cmd_str += "\tcmd_servo_state = SERVO_IDLE\n";
cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n";
cmd_str += "\tdef set_servo_setpoint(q):\n";
cmd_str += "\t\tenter_critical\n";
cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n";
cmd_str += "\t\tcmd_servo_q = q\n";
cmd_str += "\t\texit_critical\n";
cmd_str += "\tend\n";
cmd_str += "\tthread servoThread():\n";
cmd_str += "\t\tstate = SERVO_IDLE\n";
cmd_str += "\t\twhile True:\n";
cmd_str += "\t\t\tenter_critical\n";
cmd_str += "\t\t\tq = cmd_servo_q\n";
cmd_str += "\t\t\tdo_brake = False\n";
cmd_str += "\t\t\tif (state == SERVO_RUNNING) and ";
cmd_str += "(cmd_servo_state == SERVO_IDLE):\n";
cmd_str += "\t\t\t\tdo_brake = True\n";
cmd_str += "\t\t\tend\n";
cmd_str += "\t\t\tstate = cmd_servo_state\n";
cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n";
cmd_str += "\t\t\texit_critical\n";
cmd_str += "\t\t\tif do_brake:\n";
cmd_str += "\t\t\t\tstopj(1.0)\n";
cmd_str += "\t\t\t\tsync()\n";
cmd_str += "\t\t\telif state == SERVO_RUNNING:\n";
if (sec_interface_->robot_state_->getVersion() >= 3.1)
sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n",
servoj_time_, servoj_lookahead_time_, servoj_gain_);
else
sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_);
cmd_str += buf;
cmd_str += "\t\t\telse:\n";
cmd_str += "\t\t\t\tsync()\n";
cmd_str += "\t\t\tend\n";
cmd_str += "\t\tend\n";
cmd_str += "\tend\n";
sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(),
REVERSE_PORT_);
cmd_str += buf;
cmd_str += "\tthread_servo = run servoThread()\n";
cmd_str += "\tkeepalive = 1\n";
cmd_str += "\twhile keepalive > 0:\n";
cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n";
cmd_str += "\t\tif params_mult[0] > 0:\n";
cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, ";
cmd_str += "params_mult[2] / MULT_jointstate, ";
cmd_str += "params_mult[3] / MULT_jointstate, ";
cmd_str += "params_mult[4] / MULT_jointstate, ";
cmd_str += "params_mult[5] / MULT_jointstate, ";
cmd_str += "params_mult[6] / MULT_jointstate]\n";
cmd_str += "\t\t\tkeepalive = params_mult[7]\n";
cmd_str += "\t\t\tset_servo_setpoint(q)\n";
cmd_str += "\t\tend\n";
cmd_str += "\tend\n";
cmd_str += "\tsleep(.1)\n";
cmd_str += "\tsocket_close()\n";
cmd_str += "\tkill thread_servo\n";
cmd_str += "end\n";
rt_interface_->addCommandToQueue(cmd_str);
return UrDriver::openServo();
}
bool UrDriver::openServo() {
struct sockaddr_in cli_addr;
socklen_t clilen;
clilen = sizeof(cli_addr);
new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr *) &cli_addr,
&clilen);
if (new_sockfd_ < 0) {
print_fatal("ERROR on accepting reverse communication");
return false;
}
reverse_connected_ = true;
return true;
}
void UrDriver::closeServo(std::vector<double> positions) {
if (positions.size() != 6)
UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0);
else
UrDriver::servoj(positions, 0);
reverse_connected_ = false;
close(new_sockfd_);
}
bool UrDriver::start() {
if (!sec_interface_->start())
return false;
firmware_version_ = sec_interface_->robot_state_->getVersion();
rt_interface_->robot_state_->setVersion(firmware_version_);
if (!rt_interface_->start())
return false;
ip_addr_ = rt_interface_->getLocalIp();
print_debug(
"Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_)
+ "\n");
return true;
}
void UrDriver::halt() {
if (executing_traj_) {
UrDriver::stopTraj();
}
sec_interface_->halt();
rt_interface_->halt();
close(incoming_sockfd_);
}
void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4,
double q5, double acc) {
rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc);
}
std::vector<std::string> UrDriver::getJointNames() {
return joint_names_;
}
void UrDriver::setJointNames(std::vector<std::string> jn) {
joint_names_ = jn;
}
void UrDriver::setToolVoltage(unsigned int v) {
char buf[256];
sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v);
rt_interface_->addCommandToQueue(buf);
print_debug(buf);
}
void UrDriver::setFlag(unsigned int n, bool b) {
char buf[256];
sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n,
b ? "True" : "False");
rt_interface_->addCommandToQueue(buf);
print_debug(buf);
}
void UrDriver::setDigitalOut(unsigned int n, bool b) {
char buf[256];
if (firmware_version_ < 2) {
sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n,
b ? "True" : "False");
} else if (n > 15) {
sprintf(buf,
"sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n",
n - 16, b ? "True" : "False");
} else if (n > 7) {
sprintf(buf, "sec setOut():\n\tset_configurable_digital_out(%d, %s)\nend\n",
n - 8, b ? "True" : "False");
} else {
sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n",
n, b ? "True" : "False");
}
rt_interface_->addCommandToQueue(buf);
print_debug(buf);
}
void UrDriver::setAnalogOut(unsigned int n, double f) {
char buf[256];
if (firmware_version_ < 2) {
sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f);
} else {
sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f);
}
rt_interface_->addCommandToQueue(buf);
print_debug(buf);
}
bool UrDriver::setPayload(double m) {
if ((m < maximum_payload_) && (m > minimum_payload_)) {
char buf[256];
sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m);
rt_interface_->addCommandToQueue(buf);
print_debug(buf);
return true;
} else
return false;
}
void UrDriver::setMinPayload(double m) {
if (m > 0) {
minimum_payload_ = m;
} else {
minimum_payload_ = 0;
}
}
void UrDriver::setMaxPayload(double m) {
maximum_payload_ = m;
}
void UrDriver::setServojTime(double t) {
if (t > 0.008) {
servoj_time_ = t;
} else {
servoj_time_ = 0.008;
}
}
void UrDriver::setServojLookahead(double t){
if (t > 0.03) {
if (t < 0.2) {
servoj_lookahead_time_ = t;
} else {
servoj_lookahead_time_ = 0.2;
}
} else {
servoj_lookahead_time_ = 0.03;
}
}
void UrDriver::setServojGain(double g){
if (g > 100) {
if (g < 2000) {
servoj_gain_ = g;
} else {
servoj_gain_ = 2000;
}
} else {
servoj_gain_ = 100;
}
}

283
src/ur_hardware_interface.cpp
View File

@@ -1,283 +0,0 @@
/*
* ur_hardware_control_loop.cpp
*
* Copyright 2015 Thomas Timm Andersen
*
* 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.
*/
/* Based on original source from University of Colorado, Boulder. License copied below. */
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2015, University of Colorado, Boulder
* 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.
* * Neither the name of the Univ of CO, Boulder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************
Author: Dave Coleman
*/
#include <ur_modern_driver/ur_hardware_interface.h>
namespace ros_control_ur {
UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) :
nh_(nh), robot_(robot) {
// Initialize shared memory and interfaces here
init(); // this implementation loads from rosparam
max_vel_change_ = 0.12; // equivalent of an acceleration of 15 rad/sec^2
ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface.");
}
void UrHardwareInterface::init() {
ROS_INFO_STREAM_NAMED("ur_hardware_interface",
"Reading rosparams from namespace: " << nh_.getNamespace());
// Get joint names
nh_.getParam("hardware_interface/joints", joint_names_);
if (joint_names_.size() == 0) {
ROS_FATAL_STREAM_NAMED("ur_hardware_interface",
"No joints found on parameter server for controller, did you load the proper yaml file?" << " Namespace: " << nh_.getNamespace());
exit(-1);
}
num_joints_ = joint_names_.size();
// Resize vectors
joint_position_.resize(num_joints_);
joint_velocity_.resize(num_joints_);
joint_effort_.resize(num_joints_);
joint_position_command_.resize(num_joints_);
joint_velocity_command_.resize(num_joints_);
prev_joint_velocity_command_.resize(num_joints_);
// Initialize controller
for (std::size_t i = 0; i < num_joints_; ++i) {
ROS_DEBUG_STREAM_NAMED("ur_hardware_interface",
"Loading joint name: " << joint_names_[i]);
// Create joint state interface
joint_state_interface_.registerHandle(
hardware_interface::JointStateHandle(joint_names_[i],
&joint_position_[i], &joint_velocity_[i],
&joint_effort_[i]));
// Create position joint interface
position_joint_interface_.registerHandle(
hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]),
&joint_position_command_[i]));
// Create velocity joint interface
velocity_joint_interface_.registerHandle(
hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]),
&joint_velocity_command_[i]));
prev_joint_velocity_command_[i] = 0.;
}
// Create force torque interface
force_torque_interface_.registerHandle(
hardware_interface::ForceTorqueSensorHandle("wrench", "",
robot_force_, robot_torque_));
registerInterface(&joint_state_interface_); // From RobotHW base class.
registerInterface(&position_joint_interface_); // From RobotHW base class.
registerInterface(&velocity_joint_interface_); // From RobotHW base class.
registerInterface(&force_torque_interface_); // From RobotHW base class.
velocity_interface_running_ = false;
position_interface_running_ = false;
}
void UrHardwareInterface::read() {
std::vector<double> pos, vel, current, tcp;
pos = robot_->rt_interface_->robot_state_->getQActual();
vel = robot_->rt_interface_->robot_state_->getQdActual();
current = robot_->rt_interface_->robot_state_->getIActual();
tcp = robot_->rt_interface_->robot_state_->getTcpForce();
for (std::size_t i = 0; i < num_joints_; ++i) {
joint_position_[i] = pos[i];
joint_velocity_[i] = vel[i];
joint_effort_[i] = current[i];
}
for (std::size_t i = 0; i < 3; ++i) {
robot_force_[i] = tcp[i];
robot_torque_[i] = tcp[i + 3];
}
}
void UrHardwareInterface::setMaxVelChange(double inp) {
max_vel_change_ = inp;
}
void UrHardwareInterface::write() {
if (velocity_interface_running_) {
std::vector<double> cmd;
//do some rate limiting
cmd.resize(joint_velocity_command_.size());
for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) {
cmd[i] = joint_velocity_command_[i];
if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_) {
cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_;
} else if (cmd[i]
< prev_joint_velocity_command_[i] - max_vel_change_) {
cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_;
}
prev_joint_velocity_command_[i] = cmd[i];
}
robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_*125);
} else if (position_interface_running_) {
robot_->servoj(joint_position_command_);
}
}
bool UrHardwareInterface::canSwitch(
const std::list<hardware_interface::ControllerInfo> &start_list,
const std::list<hardware_interface::ControllerInfo> &stop_list) const {
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
start_list.begin(); controller_it != start_list.end();
++controller_it) {
if (controller_it->hardware_interface
== "hardware_interface::VelocityJointInterface") {
if (velocity_interface_running_) {
ROS_ERROR(
"%s: An interface of that type (%s) is already running",
controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
return false;
}
if (position_interface_running_) {
bool error = true;
for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it =
stop_list.begin();
stop_controller_it != stop_list.end();
++stop_controller_it) {
if (stop_controller_it->hardware_interface
== "hardware_interface::PositionJointInterface") {
error = false;
break;
}
}
if (error) {
ROS_ERROR(
"%s (type %s) can not be run simultaneously with a PositionJointInterface",
controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
return false;
}
}
} else if (controller_it->hardware_interface
== "hardware_interface::PositionJointInterface") {
if (position_interface_running_) {
ROS_ERROR(
"%s: An interface of that type (%s) is already running",
controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
return false;
}
if (velocity_interface_running_) {
bool error = true;
for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it =
stop_list.begin();
stop_controller_it != stop_list.end();
++stop_controller_it) {
if (stop_controller_it->hardware_interface
== "hardware_interface::VelocityJointInterface") {
error = false;
break;
}
}
if (error) {
ROS_ERROR(
"%s (type %s) can not be run simultaneously with a VelocityJointInterface",
controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
return false;
}
}
}
}
// we can always stop a controller
return true;
}
void UrHardwareInterface::doSwitch(
const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list) {
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
stop_list.begin(); controller_it != stop_list.end();
++controller_it) {
if (controller_it->hardware_interface
== "hardware_interface::VelocityJointInterface") {
velocity_interface_running_ = false;
ROS_DEBUG("Stopping velocity interface");
}
if (controller_it->hardware_interface
== "hardware_interface::PositionJointInterface") {
position_interface_running_ = false;
std::vector<double> tmp;
robot_->closeServo(tmp);
ROS_DEBUG("Stopping position interface");
}
}
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
start_list.begin(); controller_it != start_list.end();
++controller_it) {
if (controller_it->hardware_interface
== "hardware_interface::VelocityJointInterface") {
velocity_interface_running_ = true;
ROS_DEBUG("Starting velocity interface");
}
if (controller_it->hardware_interface
== "hardware_interface::PositionJointInterface") {
position_interface_running_ = true;
robot_->uploadProg();
ROS_DEBUG("Starting position interface");
}
}
}
} // namespace

198
src/ur_realtime_communication.cpp
View File

@@ -1,198 +0,0 @@
/*
* ur_realtime_communication.cpp
*
* Copyright 2015 Thomas Timm Andersen
*
* 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_modern_driver/ur_realtime_communication.h"
UrRealtimeCommunication::UrRealtimeCommunication(
std::condition_variable& msg_cond, std::string host,
unsigned int safety_count_max) {
robot_state_ = new RobotStateRT(msg_cond);
bzero((char *) &serv_addr_, sizeof(serv_addr_));
sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd_ < 0) {
print_fatal("ERROR opening socket");
}
server_ = gethostbyname(host.c_str());
if (server_ == NULL) {
print_fatal("ERROR, no such host");
}
serv_addr_.sin_family = AF_INET;
bcopy((char *) server_->h_addr, (char *)&serv_addr_.sin_addr.s_addr, server_->h_length);
serv_addr_.sin_port = htons(30003);
flag_ = 1;
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int));
setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, sizeof(int));
setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int));
fcntl(sockfd_, F_SETFL, O_NONBLOCK);
connected_ = false;
keepalive_ = false;
safety_count_ = safety_count_max + 1;
safety_count_max_ = safety_count_max;
}
bool UrRealtimeCommunication::start() {
fd_set writefds;
struct timeval timeout;
keepalive_ = true;
print_debug("Realtime port: Connecting...");
connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_));
FD_ZERO(&writefds);
FD_SET(sockfd_, &writefds);
timeout.tv_sec = 10;
timeout.tv_usec = 0;
select(sockfd_ + 1, NULL, &writefds, NULL, &timeout);
unsigned int flag_len;
getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
if (flag_ < 0) {
print_fatal("Error connecting to RT port 30003");
return false;
}
sockaddr_in name;
socklen_t namelen = sizeof(name);
int err = getsockname(sockfd_, (sockaddr*) &name, &namelen);
if (err < 0) {
print_fatal("Could not get local IP");
close(sockfd_);
return false;
}
char str[18];
inet_ntop(AF_INET, &name.sin_addr, str, 18);
local_ip_ = str;
comThread_ = std::thread(&UrRealtimeCommunication::run, this);
return true;
}
void UrRealtimeCommunication::halt() {
keepalive_ = false;
comThread_.join();
}
void UrRealtimeCommunication::addCommandToQueue(std::string inp) {
int bytes_written;
if (inp.back() != '\n') {
inp.append("\n");
}
if (connected_)
bytes_written = write(sockfd_, inp.c_str(), inp.length());
else
print_error("Could not send command \"" +inp + "\". The robot is not connected! Command is discarded" );
}
void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
double q3, double q4, double q5, double acc) {
char cmd[1024];
if( robot_state_->getVersion() >= 3.3 ) {
sprintf(cmd,
"speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.008)\n",
q0, q1, q2, q3, q4, q5, acc);
}
else if( robot_state_->getVersion() >= 3.1 ) {
sprintf(cmd,
"speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n",
q0, q1, q2, q3, q4, q5, acc);
}
else {
sprintf(cmd,
"speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n",
q0, q1, q2, q3, q4, q5, acc);
}
addCommandToQueue((std::string) (cmd));
if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) {
//If a joint speed is set, make sure we stop it again after some time if the user doesn't
safety_count_ = 0;
}
}
void UrRealtimeCommunication::run() {
uint8_t buf[2048];
int bytes_read;
bzero(buf, 2048);
struct timeval timeout;
fd_set readfds;
FD_ZERO(&readfds);
FD_SET(sockfd_, &readfds);
print_debug("Realtime port: Got connection");
connected_ = true;
while (keepalive_) {
while (connected_ && keepalive_) {
timeout.tv_sec = 0; //do this each loop as selects modifies timeout
timeout.tv_usec = 500000; // timeout of 0.5 sec
select(sockfd_ + 1, &readfds, NULL, NULL, &timeout);
bytes_read = read(sockfd_, buf, 2048);
if (bytes_read > 0) {
setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_,
sizeof(int));
robot_state_->unpack(buf);
if (safety_count_ == safety_count_max_) {
setSpeed(0., 0., 0., 0., 0., 0.);
}
safety_count_ += 1;
} else {
connected_ = false;
close(sockfd_);
}
}
if (keepalive_) {
//reconnect
print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd_ < 0) {
print_fatal("ERROR opening socket");
}
flag_ = 1;
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
sizeof(int));
setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_,
sizeof(int));
setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
sizeof(int));
fcntl(sockfd_, F_SETFL, O_NONBLOCK);
while (keepalive_ && !connected_) {
std::this_thread::sleep_for(std::chrono::seconds(10));
fd_set writefds;
connect(sockfd_, (struct sockaddr *) &serv_addr_,
sizeof(serv_addr_));
FD_ZERO(&writefds);
FD_SET(sockfd_, &writefds);
select(sockfd_ + 1, NULL, &writefds, NULL, NULL);
unsigned int flag_len;
getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
if (flag_ < 0) {
print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds...");
} else {
connected_ = true;
print_info("Realtime port: Reconnected");
}
}
}
}
setSpeed(0., 0., 0., 0., 0., 0.);
close(sockfd_);
}
void UrRealtimeCommunication::setSafetyCountMax(uint inp) {
safety_count_max_ = inp;
}
std::string UrRealtimeCommunication::getLocalIp() {
return local_ip_;
}

839
src/ur_ros_wrapper.cpp
View File

@@ -1,839 +0,0 @@
/*
* ur_driver.cpp
*
* Copyright 2015 Thomas Timm Andersen
*
* 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_modern_driver/ur_driver.h"
#include "ur_modern_driver/ur_hardware_interface.h"
#include "ur_modern_driver/do_output.h"
#include <string.h>
#include <vector>
#include <mutex>
#include <condition_variable>
#include <thread>
#include <algorithm>
#include <cmath>
#include <chrono>
#include <time.h>
#include "ros/ros.h"
#include <ros/console.h>
#include "sensor_msgs/JointState.h"
#include "geometry_msgs/WrenchStamped.h"
#include "geometry_msgs/PoseStamped.h"
#include "control_msgs/FollowJointTrajectoryAction.h"
#include "actionlib/server/action_server.h"
#include "actionlib/server/server_goal_handle.h"
#include "trajectory_msgs/JointTrajectoryPoint.h"
#include "ur_msgs/SetIO.h"
#include "ur_msgs/SetPayload.h"
#include "ur_msgs/SetPayloadRequest.h"
#include "ur_msgs/SetPayloadResponse.h"
#include "ur_msgs/SetIORequest.h"
#include "ur_msgs/SetIOResponse.h"
#include "ur_msgs/IOStates.h"
#include "ur_msgs/Digital.h"
#include "ur_msgs/Analog.h"
#include "std_msgs/String.h"
#include <controller_manager/controller_manager.h>
#include <realtime_tools/realtime_publisher.h>
/// TF
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
class RosWrapper {
protected:
UrDriver robot_;
std::condition_variable rt_msg_cond_;
std::condition_variable msg_cond_;
ros::NodeHandle nh_;
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> as_;
actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> goal_handle_;
bool has_goal_;
control_msgs::FollowJointTrajectoryFeedback feedback_;
control_msgs::FollowJointTrajectoryResult result_;
ros::Subscriber speed_sub_;
ros::Subscriber urscript_sub_;
ros::ServiceServer io_srv_;
ros::ServiceServer payload_srv_;
std::thread* rt_publish_thread_;
std::thread* mb_publish_thread_;
double io_flag_delay_;
double max_velocity_;
std::vector<double> joint_offsets_;
std::string base_frame_;
std::string tool_frame_;
bool use_ros_control_;
std::thread* ros_control_thread_;
boost::shared_ptr<ros_control_ur::UrHardwareInterface> hardware_interface_;
boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
public:
RosWrapper(std::string host, int reverse_port) :
as_(nh_, "follow_joint_trajectory",
boost::bind(&RosWrapper::goalCB, this, _1),
boost::bind(&RosWrapper::cancelCB, this, _1), false), robot_(
rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300), io_flag_delay_(0.05), joint_offsets_(
6, 0.0) {
std::string joint_prefix = "";
std::vector<std::string> joint_names;
char buf[256];
if (ros::param::get("~prefix", joint_prefix)) {
if (joint_prefix.length() > 0) {
sprintf(buf, "Setting prefix to %s", joint_prefix.c_str());
print_info(buf);
}
}
joint_names.push_back(joint_prefix + "shoulder_pan_joint");
joint_names.push_back(joint_prefix + "shoulder_lift_joint");
joint_names.push_back(joint_prefix + "elbow_joint");
joint_names.push_back(joint_prefix + "wrist_1_joint");
joint_names.push_back(joint_prefix + "wrist_2_joint");
joint_names.push_back(joint_prefix + "wrist_3_joint");
robot_.setJointNames(joint_names);
use_ros_control_ = false;
ros::param::get("~use_ros_control", use_ros_control_);
if (use_ros_control_) {
hardware_interface_.reset(
new ros_control_ur::UrHardwareInterface(nh_, &robot_));
controller_manager_.reset(
new controller_manager::ControllerManager(
hardware_interface_.get(), nh_));
double max_vel_change = 0.12; // equivalent of an acceleration of 15 rad/sec^2
if (ros::param::get("~max_acceleration", max_vel_change)) {
max_vel_change = max_vel_change / 125;
}
sprintf(buf, "Max acceleration set to: %f [rad/sec²]",
max_vel_change * 125);
print_debug(buf);
hardware_interface_->setMaxVelChange(max_vel_change);
}
//Using a very high value in order to not limit execution of trajectories being sent from MoveIt!
max_velocity_ = 10.;
if (ros::param::get("~max_velocity", max_velocity_)) {
sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]",
max_velocity_);
print_debug(buf);
}
//Bounds for SetPayload service
//Using a very conservative value as it should be set through the parameter server
double min_payload = 0.;
double max_payload = 1.;
if (ros::param::get("~min_payload", min_payload)) {
sprintf(buf, "Min payload set to: %f [kg]", min_payload);
print_debug(buf);
}
if (ros::param::get("~max_payload", max_payload)) {
sprintf(buf, "Max payload set to: %f [kg]", max_payload);
print_debug(buf);
}
robot_.setMinPayload(min_payload);
robot_.setMaxPayload(max_payload);
sprintf(buf, "Bounds for set_payload service calls: [%f, %f]",
min_payload, max_payload);
print_debug(buf);
double servoj_time = 0.008;
if (ros::param::get("~servoj_time", servoj_time)) {
sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time);
print_debug(buf);
}
robot_.setServojTime(servoj_time);
double servoj_lookahead_time = 0.03;
if (ros::param::get("~servoj_lookahead_time", servoj_lookahead_time)) {
sprintf(buf, "Servoj_lookahead_time set to: %f [sec]", servoj_lookahead_time);
print_debug(buf);
}
robot_.setServojLookahead(servoj_lookahead_time);
double servoj_gain = 300.;
if (ros::param::get("~servoj_gain", servoj_gain)) {
sprintf(buf, "Servoj_gain set to: %f [sec]", servoj_gain);
print_debug(buf);
}
robot_.setServojGain(servoj_gain);
//Base and tool frames
base_frame_ = joint_prefix + "base_link";
tool_frame_ = joint_prefix + "tool0_controller";
if (ros::param::get("~base_frame", base_frame_)) {
sprintf(buf, "Base frame set to: %s", base_frame_.c_str());
print_debug(buf);
}
if (ros::param::get("~tool_frame", tool_frame_)) {
sprintf(buf, "Tool frame set to: %s", tool_frame_.c_str());
print_debug(buf);
}
if (robot_.start()) {
if (use_ros_control_) {
ros_control_thread_ = new std::thread(
boost::bind(&RosWrapper::rosControlLoop, this));
print_debug(
"The control thread for this driver has been started");
} else {
//start actionserver
has_goal_ = false;
as_.start();
//subscribe to the data topic of interest
rt_publish_thread_ = new std::thread(
boost::bind(&RosWrapper::publishRTMsg, this));
print_debug(
"The action server for this driver has been started");
}
mb_publish_thread_ = new std::thread(
boost::bind(&RosWrapper::publishMbMsg, this));
speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1,
&RosWrapper::speedInterface, this);
urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1,
&RosWrapper::urscriptInterface, this);
io_srv_ = nh_.advertiseService("ur_driver/set_io",
&RosWrapper::setIO, this);
payload_srv_ = nh_.advertiseService("ur_driver/set_payload",
&RosWrapper::setPayload, this);
}
}
void halt() {
robot_.halt();
rt_publish_thread_->join();
}
private:
void trajThread(std::vector<double> timestamps,
std::vector<std::vector<double> > positions,
std::vector<std::vector<double> > velocities) {
robot_.doTraj(timestamps, positions, velocities);
if (has_goal_) {
result_.error_code = result_.SUCCESSFUL;
goal_handle_.setSucceeded(result_);
has_goal_ = false;
}
}
void goalCB(
actionlib::ServerGoalHandle<
control_msgs::FollowJointTrajectoryAction> gh) {
std::string buf;
print_info("on_goal");
if (!robot_.sec_interface_->robot_state_->isReady()) {
result_.error_code = -100; //nothing is defined for this...?
if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) {
result_.error_string =
"Cannot accept new trajectories: Robot arm is not powered on";
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) {
result_.error_string =
"Cannot accept new trajectories: Robot is not enabled";
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
result_.error_string =
"Cannot accept new trajectories. (Debug: Robot mode is "
+ std::to_string(
robot_.sec_interface_->robot_state_->getRobotMode())
+ ")";
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
if (robot_.sec_interface_->robot_state_->isEmergencyStopped()) {
result_.error_code = -100; //nothing is defined for this...?
result_.error_string =
"Cannot accept new trajectories: Robot is emergency stopped";
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
if (robot_.sec_interface_->robot_state_->isProtectiveStopped()) {
result_.error_code = -100; //nothing is defined for this...?
result_.error_string =
"Cannot accept new trajectories: Robot is protective stopped";
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
*gh.getGoal(); //make a copy that we can modify
if (has_goal_) {
print_warning(
"Received new goal while still executing previous trajectory. Canceling previous trajectory");
has_goal_ = false;
robot_.stopTraj();
result_.error_code = -100; //nothing is defined for this...?
result_.error_string = "Received another trajectory";
goal_handle_.setAborted(result_, result_.error_string);
std::this_thread::sleep_for(std::chrono::milliseconds(250));
}
goal_handle_ = gh;
if (!validateJointNames()) {
std::string outp_joint_names = "";
for (unsigned int i = 0; i < goal.trajectory.joint_names.size();
i++) {
outp_joint_names += goal.trajectory.joint_names[i] + " ";
}
result_.error_code = result_.INVALID_JOINTS;
result_.error_string =
"Received a goal with incorrect joint names: "
+ outp_joint_names;
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
if (!has_positions()) {
result_.error_code = result_.INVALID_GOAL;
result_.error_string = "Received a goal without positions";
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
if (!has_velocities()) {
result_.error_code = result_.INVALID_GOAL;
result_.error_string = "Received a goal without velocities";
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
if (!traj_is_finite()) {
result_.error_string = "Received a goal with infinities or NaNs";
result_.error_code = result_.INVALID_GOAL;
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
if (!has_limited_velocities()) {
result_.error_code = result_.INVALID_GOAL;
result_.error_string =
"Received a goal with velocities that are higher than "
+ std::to_string(max_velocity_);
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
reorder_traj_joints(goal.trajectory);
if (!start_positions_match(goal.trajectory, 0.01)) {
result_.error_code = result_.INVALID_GOAL;
result_.error_string = "Goal start doesn't match current pose";
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
std::vector<double> timestamps;
std::vector<std::vector<double> > positions, velocities;
if (goal.trajectory.points[0].time_from_start.toSec() != 0.) {
print_warning(
"Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory");
timestamps.push_back(0.0);
positions.push_back(
robot_.rt_interface_->robot_state_->getQActual());
velocities.push_back(
robot_.rt_interface_->robot_state_->getQdActual());
}
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
timestamps.push_back(
goal.trajectory.points[i].time_from_start.toSec());
positions.push_back(goal.trajectory.points[i].positions);
velocities.push_back(goal.trajectory.points[i].velocities);
}
goal_handle_.setAccepted();
has_goal_ = true;
std::thread(&RosWrapper::trajThread, this, timestamps, positions,
velocities).detach();
}
void cancelCB(
actionlib::ServerGoalHandle<
control_msgs::FollowJointTrajectoryAction> gh) {
// set the action state to preempted
print_info("on_cancel");
if (has_goal_) {
if (gh == goal_handle_) {
robot_.stopTraj();
has_goal_ = false;
}
}
result_.error_code = -100; //nothing is defined for this...?
result_.error_string = "Goal cancelled by client";
gh.setCanceled(result_);
}
bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp) {
resp.success = true;
//if (req.fun == ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT) {
if (req.fun == 1) {
robot_.setDigitalOut(req.pin, req.state > 0.0 ? true : false);
} else if (req.fun == 2) {
//} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_FLAG) {
robot_.setFlag(req.pin, req.state > 0.0 ? true : false);
//According to urdriver.py, set_flag will fail if called to rapidly in succession
ros::Duration(io_flag_delay_).sleep();
} else if (req.fun == 3) {
//} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT) {
robot_.setAnalogOut(req.pin, req.state);
} else if (req.fun == 4) {
//} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE) {
robot_.setToolVoltage((int) req.state);
} else {
resp.success = false;
}
return resp.success;
}
bool setPayload(ur_msgs::SetPayloadRequest& req,
ur_msgs::SetPayloadResponse& resp) {
if (robot_.setPayload(req.payload))
resp.success = true;
else
resp.success = true;
return resp.success;
}
bool validateJointNames() {
std::vector<std::string> actual_joint_names = robot_.getJointNames();
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
*goal_handle_.getGoal();
if (goal.trajectory.joint_names.size() != actual_joint_names.size())
return false;
for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) {
unsigned int j;
for (j = 0; j < actual_joint_names.size(); j++) {
if (goal.trajectory.joint_names[i] == actual_joint_names[j])
break;
}
if (goal.trajectory.joint_names[i] == actual_joint_names[j]) {
actual_joint_names.erase(actual_joint_names.begin() + j);
} else {
return false;
}
}
return true;
}
void reorder_traj_joints(trajectory_msgs::JointTrajectory& traj) {
/* Reorders trajectory - destructive */
std::vector<std::string> actual_joint_names = robot_.getJointNames();
std::vector<unsigned int> mapping;
mapping.resize(actual_joint_names.size(), actual_joint_names.size());
for (unsigned int i = 0; i < traj.joint_names.size(); i++) {
for (unsigned int j = 0; j < actual_joint_names.size(); j++) {
if (traj.joint_names[i] == actual_joint_names[j])
mapping[j] = i;
}
}
traj.joint_names = actual_joint_names;
std::vector<trajectory_msgs::JointTrajectoryPoint> new_traj;
for (unsigned int i = 0; i < traj.points.size(); i++) {
trajectory_msgs::JointTrajectoryPoint new_point;
for (unsigned int j = 0; j < traj.points[i].positions.size(); j++) {
new_point.positions.push_back(
traj.points[i].positions[mapping[j]]);
new_point.velocities.push_back(
traj.points[i].velocities[mapping[j]]);
if (traj.points[i].accelerations.size() != 0)
new_point.accelerations.push_back(
traj.points[i].accelerations[mapping[j]]);
}
new_point.time_from_start = traj.points[i].time_from_start;
new_traj.push_back(new_point);
}
traj.points = new_traj;
}
bool has_velocities() {
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
*goal_handle_.getGoal();
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
if (goal.trajectory.points[i].positions.size()
!= goal.trajectory.points[i].velocities.size())
return false;
}
return true;
}
bool has_positions() {
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
*goal_handle_.getGoal();
if (goal.trajectory.points.size() == 0)
return false;
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
if (goal.trajectory.points[i].positions.size()
!= goal.trajectory.joint_names.size())
return false;
}
return true;
}
bool start_positions_match(const trajectory_msgs::JointTrajectory &traj, double eps)
{
for (unsigned int i = 0; i < traj.points[0].positions.size(); i++)
{
std::vector<double> qActual = robot_.rt_interface_->robot_state_->getQActual();
if( fabs(traj.points[0].positions[i] - qActual[i]) > eps )
{
return false;
}
}
return true;
}
bool has_limited_velocities() {
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
*goal_handle_.getGoal();
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
for (unsigned int j = 0;
j < goal.trajectory.points[i].velocities.size(); j++) {
if (fabs(goal.trajectory.points[i].velocities[j])
> max_velocity_)
return false;
}
}
return true;
}
bool traj_is_finite() {
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
*goal_handle_.getGoal();
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
for (unsigned int j = 0;
j < goal.trajectory.points[i].velocities.size(); j++) {
if (!std::isfinite(goal.trajectory.points[i].positions[j]))
return false;
if (!std::isfinite(goal.trajectory.points[i].velocities[j]))
return false;
}
}
return true;
}
void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg) {
if (msg->points[0].velocities.size() == 6) {
double acc = 100;
if (msg->points[0].accelerations.size() > 0)
acc = *std::max_element(msg->points[0].accelerations.begin(),
msg->points[0].accelerations.end());
robot_.setSpeed(msg->points[0].velocities[0],
msg->points[0].velocities[1], msg->points[0].velocities[2],
msg->points[0].velocities[3], msg->points[0].velocities[4],
msg->points[0].velocities[5], acc);
}
}
void urscriptInterface(const std_msgs::String::ConstPtr& msg) {
robot_.rt_interface_->addCommandToQueue(msg->data);
}
void rosControlLoop() {
ros::Duration elapsed_time;
struct timespec last_time, current_time;
static const double BILLION = 1000000000.0;
realtime_tools::RealtimePublisher<tf::tfMessage> tf_pub( nh_, "/tf", 1 );
geometry_msgs::TransformStamped tool_transform;
tool_transform.header.frame_id = base_frame_;
tool_transform.child_frame_id = tool_frame_;
tf_pub.msg_.transforms.push_back( tool_transform );
realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped> tool_vel_pub( nh_, "tool_velocity", 1 );
tool_vel_pub.msg_.header.frame_id = base_frame_;
clock_gettime(CLOCK_MONOTONIC, &last_time);
while (ros::ok()) {
std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex
std::unique_lock<std::mutex> locker(msg_lock);
while (!robot_.rt_interface_->robot_state_->getControllerUpdated()) {
rt_msg_cond_.wait(locker);
}
// Input
hardware_interface_->read();
robot_.rt_interface_->robot_state_->setControllerUpdated();
// Control
clock_gettime(CLOCK_MONOTONIC, &current_time);
elapsed_time = ros::Duration(current_time.tv_sec - last_time.tv_sec + (current_time.tv_nsec - last_time.tv_nsec)/ BILLION);
ros::Time ros_time = ros::Time::now();
controller_manager_->update(ros_time, elapsed_time);
last_time = current_time;
// Output
hardware_interface_->write();
// Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
std::vector<double> tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual();
// Compute rotation angle
double rx = tool_vector_actual[3];
double ry = tool_vector_actual[4];
double rz = tool_vector_actual[5];
double angle = std::sqrt(std::pow(rx,2) + std::pow(ry,2) + std::pow(rz,2));
// Broadcast transform
if( tf_pub.trylock() )
{
tf_pub.msg_.transforms[0].header.stamp = ros_time;
if (angle < 1e-16) {
tf_pub.msg_.transforms[0].transform.rotation.x = 0;
tf_pub.msg_.transforms[0].transform.rotation.y = 0;
tf_pub.msg_.transforms[0].transform.rotation.z = 0;
tf_pub.msg_.transforms[0].transform.rotation.w = 1;
} else {
tf_pub.msg_.transforms[0].transform.rotation.x = (rx/angle) * std::sin(angle*0.5);
tf_pub.msg_.transforms[0].transform.rotation.y = (ry/angle) * std::sin(angle*0.5);
tf_pub.msg_.transforms[0].transform.rotation.z = (rz/angle) * std::sin(angle*0.5);
tf_pub.msg_.transforms[0].transform.rotation.w = std::cos(angle*0.5);
}
tf_pub.msg_.transforms[0].transform.translation.x = tool_vector_actual[0];
tf_pub.msg_.transforms[0].transform.translation.y = tool_vector_actual[1];
tf_pub.msg_.transforms[0].transform.translation.z = tool_vector_actual[2];
tf_pub.unlockAndPublish();
}
//Publish tool velocity
std::vector<double> tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual();
if( tool_vel_pub.trylock() )
{
tool_vel_pub.msg_.header.stamp = ros_time;
tool_vel_pub.msg_.twist.linear.x = tcp_speed[0];
tool_vel_pub.msg_.twist.linear.y = tcp_speed[1];
tool_vel_pub.msg_.twist.linear.z = tcp_speed[2];
tool_vel_pub.msg_.twist.angular.x = tcp_speed[3];
tool_vel_pub.msg_.twist.angular.y = tcp_speed[4];
tool_vel_pub.msg_.twist.angular.z = tcp_speed[5];
tool_vel_pub.unlockAndPublish();
}
}
}
void publishRTMsg() {
ros::Publisher joint_pub = nh_.advertise<sensor_msgs::JointState>(
"joint_states", 1);
ros::Publisher wrench_pub = nh_.advertise<geometry_msgs::WrenchStamped>(
"wrench", 1);
ros::Publisher tool_vel_pub = nh_.advertise<geometry_msgs::TwistStamped>("tool_velocity", 1);
static tf::TransformBroadcaster br;
while (ros::ok()) {
sensor_msgs::JointState joint_msg;
joint_msg.name = robot_.getJointNames();
geometry_msgs::WrenchStamped wrench_msg;
geometry_msgs::PoseStamped tool_pose_msg;
std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex
std::unique_lock<std::mutex> locker(msg_lock);
while (!robot_.rt_interface_->robot_state_->getDataPublished()) {
rt_msg_cond_.wait(locker);
}
joint_msg.header.stamp = ros::Time::now();
joint_msg.position =
robot_.rt_interface_->robot_state_->getQActual();
for (unsigned int i = 0; i < joint_msg.position.size(); i++) {
joint_msg.position[i] += joint_offsets_[i];
}
joint_msg.velocity =
robot_.rt_interface_->robot_state_->getQdActual();
joint_msg.effort = robot_.rt_interface_->robot_state_->getIActual();
joint_pub.publish(joint_msg);
std::vector<double> tcp_force =
robot_.rt_interface_->robot_state_->getTcpForce();
wrench_msg.header.stamp = joint_msg.header.stamp;
wrench_msg.wrench.force.x = tcp_force[0];
wrench_msg.wrench.force.y = tcp_force[1];
wrench_msg.wrench.force.z = tcp_force[2];
wrench_msg.wrench.torque.x = tcp_force[3];
wrench_msg.wrench.torque.y = tcp_force[4];
wrench_msg.wrench.torque.z = tcp_force[5];
wrench_pub.publish(wrench_msg);
// Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
std::vector<double> tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual();
//Create quaternion
tf::Quaternion quat;
double rx = tool_vector_actual[3];
double ry = tool_vector_actual[4];
double rz = tool_vector_actual[5];
double angle = std::sqrt(std::pow(rx,2) + std::pow(ry,2) + std::pow(rz,2));
if (angle < 1e-16) {
quat.setValue(0, 0, 0, 1);
} else {
quat.setRotation(tf::Vector3(rx/angle, ry/angle, rz/angle), angle);
}
//Create and broadcast transform
tf::Transform transform;
transform.setOrigin(tf::Vector3(tool_vector_actual[0], tool_vector_actual[1], tool_vector_actual[2]));
transform.setRotation(quat);
br.sendTransform(tf::StampedTransform(transform, joint_msg.header.stamp, base_frame_, tool_frame_));
//Publish tool velocity
std::vector<double> tcp_speed =
robot_.rt_interface_->robot_state_->getTcpSpeedActual();
geometry_msgs::TwistStamped tool_twist;
tool_twist.header.frame_id = base_frame_;
tool_twist.header.stamp = joint_msg.header.stamp;
tool_twist.twist.linear.x = tcp_speed[0];
tool_twist.twist.linear.y = tcp_speed[1];
tool_twist.twist.linear.z = tcp_speed[2];
tool_twist.twist.angular.x = tcp_speed[3];
tool_twist.twist.angular.y = tcp_speed[4];
tool_twist.twist.angular.z = tcp_speed[5];
tool_vel_pub.publish(tool_twist);
robot_.rt_interface_->robot_state_->setDataPublished();
}
}
void publishMbMsg() {
bool warned = false;
ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>(
"ur_driver/io_states", 1);
while (ros::ok()) {
ur_msgs::IOStates io_msg;
std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex
std::unique_lock<std::mutex> locker(msg_lock);
while (!robot_.sec_interface_->robot_state_->getNewDataAvailable()) {
msg_cond_.wait(locker);
}
int i_max = 10;
if (robot_.sec_interface_->robot_state_->getVersion() > 3.0)
i_max = 18; // From version 3.0, there are up to 18 inputs and outputs
for (unsigned int i = 0; i < i_max; i++) {
ur_msgs::Digital digi;
digi.pin = i;
digi.state =
((robot_.sec_interface_->robot_state_->getDigitalInputBits()
& (1 << i)) >> i);
io_msg.digital_in_states.push_back(digi);
digi.state =
((robot_.sec_interface_->robot_state_->getDigitalOutputBits()
& (1 << i)) >> i);
io_msg.digital_out_states.push_back(digi);
}
ur_msgs::Analog ana;
ana.pin = 0;
ana.state = robot_.sec_interface_->robot_state_->getAnalogInput0();
io_msg.analog_in_states.push_back(ana);
ana.pin = 1;
ana.state = robot_.sec_interface_->robot_state_->getAnalogInput1();
io_msg.analog_in_states.push_back(ana);
ana.pin = 0;
ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput0();
io_msg.analog_out_states.push_back(ana);
ana.pin = 1;
ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput1();
io_msg.analog_out_states.push_back(ana);
io_pub.publish(io_msg);
if (robot_.sec_interface_->robot_state_->isEmergencyStopped()
or robot_.sec_interface_->robot_state_->isProtectiveStopped()) {
if (robot_.sec_interface_->robot_state_->isEmergencyStopped()
and !warned) {
print_error("Emergency stop pressed!");
} else if (robot_.sec_interface_->robot_state_->isProtectiveStopped()
and !warned) {
print_error("Robot is protective stopped!");
}
if (has_goal_) {
print_error("Aborting trajectory");
robot_.stopTraj();
result_.error_code = result_.SUCCESSFUL;
result_.error_string = "Robot was halted";
goal_handle_.setAborted(result_, result_.error_string);
has_goal_ = false;
}
warned = true;
} else
warned = false;
robot_.sec_interface_->robot_state_->finishedReading();
}
}
};
int main(int argc, char **argv) {
bool use_sim_time = false;
std::string host;
int reverse_port = 50001;
ros::init(argc, argv, "ur_driver");
ros::NodeHandle nh;
if (ros::param::get("use_sim_time", use_sim_time)) {
print_warning("use_sim_time is set!!");
}
if (!(ros::param::get("~robot_ip_address", host))) {
if (argc > 1) {
print_warning(
"Please set the parameter robot_ip_address instead of giving it as a command line argument. This method is DEPRECATED");
host = argv[1];
} else {
print_fatal(
"Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip");
exit(1);
}
}
if ((ros::param::get("~reverse_port", reverse_port))) {
if((reverse_port <= 0) or (reverse_port >= 65535)) {
print_warning("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 50001");
reverse_port = 50001;
}
} else
reverse_port = 50001;
RosWrapper interface(host, reverse_port);
ros::AsyncSpinner spinner(3);
spinner.start();
ros::waitForShutdown();
interface.halt();
exit(0);
}

7
tests/main.cpp Normal file
View File

@@ -0,0 +1,7 @@
#include "gtest/gtest.h"
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

136
tests/ur/master_board.cpp Normal file
View File

@@ -0,0 +1,136 @@
#include "ur_modern_driver/ur/master_board.h"
#include <gtest/gtest.h>
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/test/random_data.h"
#include "ur_modern_driver/test/utils.h"
#include "ur_modern_driver/types.h"
TEST(MasterBoardData_V1_X, testRandomDataParsing)
{
RandomDataTest rdt(71);
rdt.set<uint8_t>(1, 58); //sets euromap67_interface_installed to true
BinParser bp = rdt.getParser();
MasterBoardData_V1_X state;
ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false";
ASSERT_EQ((rdt.getNext<uint16_t, 10>()), state.digital_input_bits);
ASSERT_EQ((rdt.getNext<uint16_t, 10>()), state.digital_output_bits);
ASSERT_EQ(rdt.getNext<int8_t>(), state.analog_input_range0);
ASSERT_EQ(rdt.getNext<int8_t>(), state.analog_input_range1);
ASSERT_EQ(rdt.getNext<double>(), state.analog_input0);
ASSERT_EQ(rdt.getNext<double>(), state.analog_input1);
ASSERT_EQ(rdt.getNext<int8_t>(), state.analog_output_domain0);
ASSERT_EQ(rdt.getNext<int8_t>(), state.analog_output_domain1);
ASSERT_EQ(rdt.getNext<double>(), state.analog_output0);
ASSERT_EQ(rdt.getNext<double>(), state.analog_output1);
ASSERT_EQ(rdt.getNext<float>(), state.master_board_temperature);
ASSERT_EQ(rdt.getNext<float>(), state.robot_voltage_48V);
ASSERT_EQ(rdt.getNext<float>(), state.robot_current);
ASSERT_EQ(rdt.getNext<float>(), state.master_IO_current);
ASSERT_EQ(rdt.getNext<uint8_t>(), state.master_safety_state);
ASSERT_EQ(rdt.getNext<bool>(), state.master_on_off_state);
ASSERT_EQ(rdt.getNext<bool>(), state.euromap67_interface_installed);
ASSERT_EQ(rdt.getNext<int32_t>(), state.euromap_input_bits);
ASSERT_EQ(rdt.getNext<int32_t>(), state.euromap_output_bits);
ASSERT_EQ(rdt.getNext<int16_t>(), state.euromap_voltage);
ASSERT_EQ(rdt.getNext<int16_t>(), state.euromap_current);
ASSERT_TRUE(bp.empty()) << "Did not consume all data";
}
TEST(MasterBoardData_V3_0__1, testRandomDataParsing)
{
RandomDataTest rdt(83);
rdt.set<uint8_t>(1, 62); //sets euromap67_interface_installed to true
BinParser bp = rdt.getParser();
MasterBoardData_V3_0__1 state;
ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false";
ASSERT_EQ((rdt.getNext<uint32_t, 18>()), state.digital_input_bits);
ASSERT_EQ((rdt.getNext<uint32_t, 18>()), state.digital_output_bits);
ASSERT_EQ(rdt.getNext<int8_t>(), state.analog_input_range0);
ASSERT_EQ(rdt.getNext<int8_t>(), state.analog_input_range1);
ASSERT_EQ(rdt.getNext<double>(), state.analog_input0);
ASSERT_EQ(rdt.getNext<double>(), state.analog_input1);
ASSERT_EQ(rdt.getNext<int8_t>(), state.analog_output_domain0);
ASSERT_EQ(rdt.getNext<int8_t>(), state.analog_output_domain1);
ASSERT_EQ(rdt.getNext<double>(), state.analog_output0);
ASSERT_EQ(rdt.getNext<double>(), state.analog_output1);
ASSERT_EQ(rdt.getNext<float>(), state.master_board_temperature);
ASSERT_EQ(rdt.getNext<float>(), state.robot_voltage_48V);
ASSERT_EQ(rdt.getNext<float>(), state.robot_current);
ASSERT_EQ(rdt.getNext<float>(), state.master_IO_current);
ASSERT_EQ(rdt.getNext<uint8_t>(), state.safety_mode);
ASSERT_EQ(rdt.getNext<bool>(), state.in_reduced_mode);
ASSERT_EQ(rdt.getNext<bool>(), state.euromap67_interface_installed);
ASSERT_EQ(rdt.getNext<int32_t>(), state.euromap_input_bits);
ASSERT_EQ(rdt.getNext<int32_t>(), state.euromap_output_bits);
ASSERT_EQ(rdt.getNext<float>(), state.euromap_voltage);
ASSERT_EQ(rdt.getNext<float>(), state.euromap_current);
rdt.skip(sizeof(uint32_t));
ASSERT_TRUE(bp.empty()) << "Did not consume all data";
}
TEST(MasterBoardData_V3_2, testRandomDataParsing)
{
RandomDataTest rdt(85);
rdt.set<uint8_t>(1, 62); //sets euromap67_interface_installed to true
BinParser bp = rdt.getParser();
MasterBoardData_V3_2 state;
ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false";
ASSERT_EQ((rdt.getNext<uint32_t, 18>()), state.digital_input_bits);
ASSERT_EQ((rdt.getNext<uint32_t, 18>()), state.digital_output_bits);
ASSERT_EQ(rdt.getNext<int8_t>(), state.analog_input_range0);
ASSERT_EQ(rdt.getNext<int8_t>(), state.analog_input_range1);
ASSERT_EQ(rdt.getNext<double>(), state.analog_input0);
ASSERT_EQ(rdt.getNext<double>(), state.analog_input1);
ASSERT_EQ(rdt.getNext<int8_t>(), state.analog_output_domain0);
ASSERT_EQ(rdt.getNext<int8_t>(), state.analog_output_domain1);
ASSERT_EQ(rdt.getNext<double>(), state.analog_output0);
ASSERT_EQ(rdt.getNext<double>(), state.analog_output1);
ASSERT_EQ(rdt.getNext<float>(), state.master_board_temperature);
ASSERT_EQ(rdt.getNext<float>(), state.robot_voltage_48V);
ASSERT_EQ(rdt.getNext<float>(), state.robot_current);
ASSERT_EQ(rdt.getNext<float>(), state.master_IO_current);
ASSERT_EQ(rdt.getNext<uint8_t>(), state.safety_mode);
ASSERT_EQ(rdt.getNext<bool>(), state.in_reduced_mode);
ASSERT_EQ(rdt.getNext<bool>(), state.euromap67_interface_installed);
ASSERT_EQ(rdt.getNext<int32_t>(), state.euromap_input_bits);
ASSERT_EQ(rdt.getNext<int32_t>(), state.euromap_output_bits);
ASSERT_EQ(rdt.getNext<float>(), state.euromap_voltage);
ASSERT_EQ(rdt.getNext<float>(), state.euromap_current);
rdt.skip(sizeof(uint32_t));
ASSERT_EQ(rdt.getNext<uint8_t>(), state.operational_mode_selector_input);
ASSERT_EQ(rdt.getNext<uint8_t>(), state.three_position_enabling_device_input);
ASSERT_TRUE(bp.empty()) << "Did not consume all data";
}
TEST(MasterBoardData_V1_X, testTooSmallBuffer)
{
RandomDataTest rdt(10);
BinParser bp = rdt.getParser();
MasterBoardData_V1_X state;
EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough";
}
TEST(MasterBoardData_V3_0__1, testTooSmallBuffer)
{
RandomDataTest rdt(10);
BinParser bp = rdt.getParser();
MasterBoardData_V3_0__1 state;
EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough";
}
TEST(MasterBoardData_V3_2, testTooSmallBuffer)
{
RandomDataTest rdt(10);
BinParser bp = rdt.getParser();
MasterBoardData_V3_2 state;
EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough";
}

99
tests/ur/robot_mode.cpp Normal file
View File

@@ -0,0 +1,99 @@
#include "ur_modern_driver/ur/robot_mode.h"
#include <gtest/gtest.h>
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/test/random_data.h"
#include "ur_modern_driver/test/utils.h"
#include "ur_modern_driver/types.h"
TEST(RobotModeData_V1_X, testRandomDataParsing)
{
RandomDataTest rdt(24);
BinParser bp = rdt.getParser();
RobotModeData_V1_X state;
ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false";
ASSERT_EQ(rdt.getNext<uint64_t>(), state.timestamp);
ASSERT_EQ(rdt.getNext<bool>(), state.physical_robot_connected);
ASSERT_EQ(rdt.getNext<bool>(), state.real_robot_enabled);
ASSERT_EQ(rdt.getNext<bool>(), state.robot_power_on);
ASSERT_EQ(rdt.getNext<bool>(), state.emergency_stopped);
ASSERT_EQ(rdt.getNext<bool>(), state.protective_stopped);
ASSERT_EQ(rdt.getNext<bool>(), state.program_running);
ASSERT_EQ(rdt.getNext<bool>(), state.program_paused);
ASSERT_EQ(rdt.getNext<robot_mode_V1_X>(), state.robot_mode);
ASSERT_EQ(rdt.getNext<double>(), state.speed_fraction);
ASSERT_TRUE(bp.empty()) << "Did not consume all data";
}
TEST(RobotModeData_V3_0__1, testRandomDataParsing)
{
RandomDataTest rdt(33);
BinParser bp = rdt.getParser();
RobotModeData_V3_0__1 state;
ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false";
ASSERT_EQ(rdt.getNext<uint64_t>(), state.timestamp);
ASSERT_EQ(rdt.getNext<bool>(), state.physical_robot_connected);
ASSERT_EQ(rdt.getNext<bool>(), state.real_robot_enabled);
ASSERT_EQ(rdt.getNext<bool>(), state.robot_power_on);
ASSERT_EQ(rdt.getNext<bool>(), state.emergency_stopped);
ASSERT_EQ(rdt.getNext<bool>(), state.protective_stopped);
ASSERT_EQ(rdt.getNext<bool>(), state.program_running);
ASSERT_EQ(rdt.getNext<bool>(), state.program_paused);
ASSERT_EQ(rdt.getNext<robot_mode_V3_X>(), state.robot_mode);
ASSERT_EQ(rdt.getNext<robot_control_mode_V3_X>(), state.control_mode);
ASSERT_EQ(rdt.getNext<double>(), state.target_speed_fraction);
ASSERT_EQ(rdt.getNext<double>(), state.speed_scaling);
ASSERT_TRUE(bp.empty()) << "Did not consume all data";
}
TEST(RobotModeData_V3_2, testRandomDataParsing)
{
RandomDataTest rdt(41);
BinParser bp = rdt.getParser();
RobotModeData_V3_2 state;
ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false";
ASSERT_EQ(rdt.getNext<uint64_t>(), state.timestamp);
ASSERT_EQ(rdt.getNext<bool>(), state.physical_robot_connected);
ASSERT_EQ(rdt.getNext<bool>(), state.real_robot_enabled);
ASSERT_EQ(rdt.getNext<bool>(), state.robot_power_on);
ASSERT_EQ(rdt.getNext<bool>(), state.emergency_stopped);
ASSERT_EQ(rdt.getNext<bool>(), state.protective_stopped);
ASSERT_EQ(rdt.getNext<bool>(), state.program_running);
ASSERT_EQ(rdt.getNext<bool>(), state.program_paused);
ASSERT_EQ(rdt.getNext<robot_mode_V3_X>(), state.robot_mode);
ASSERT_EQ(rdt.getNext<robot_control_mode_V3_X>(), state.control_mode);
ASSERT_EQ(rdt.getNext<double>(), state.target_speed_fraction);
ASSERT_EQ(rdt.getNext<double>(), state.speed_scaling);
ASSERT_EQ(rdt.getNext<double>(), state.target_speed_fraction_limit);
ASSERT_TRUE(bp.empty()) << "Did not consume all data";
}
TEST(RobotModeData_V1_X, testTooSmallBuffer)
{
RandomDataTest rdt(10);
BinParser bp = rdt.getParser();
RobotModeData_V1_X state;
EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough";
}
TEST(RobotModeData_V3_0__1, testTooSmallBuffer)
{
RandomDataTest rdt(10);
BinParser bp = rdt.getParser();
RobotModeData_V3_0__1 state;
EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough";
}
TEST(RobotModeData_V3_2, testTooSmallBuffer)
{
RandomDataTest rdt(10);
BinParser bp = rdt.getParser();
RobotModeData_V3_2 state;
EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough";
}

192
tests/ur/rt_state.cpp Normal file
View File

@@ -0,0 +1,192 @@
#include "ur_modern_driver/ur/rt_state.h"
#include <gtest/gtest.h>
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/test/random_data.h"
#include "ur_modern_driver/test/utils.h"
#include "ur_modern_driver/types.h"
TEST(RTState_V1_6__7, testRandomDataParsing)
{
RandomDataTest rdt(764);
BinParser bp = rdt.getParser(true);
RTState_V1_6__7 state;
EXPECT_TRUE(state.parseWith(bp)) << "parse() returned false";
ASSERT_EQ(rdt.getNext<double>(), state.time);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.q_target);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qd_target);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qdd_target);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_target);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.m_target);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.q_actual);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qd_actual);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_actual);
ASSERT_EQ(rdt.getNext<double3_t>(), state.tool_accelerometer_values);
rdt.skip(sizeof(double) * 15);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.tcp_force);
ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tool_vector_actual);
ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tcp_speed_actual);
ASSERT_EQ(rdt.getNext<uint64_t>(), state.digital_inputs);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.motor_temperatures);
ASSERT_EQ(rdt.getNext<double>(), state.controller_time);
rdt.skip(sizeof(double)); // skip unused value
ASSERT_EQ(rdt.getNext<double>(), state.robot_mode);
EXPECT_TRUE(bp.empty()) << "Did not consume all data";
}
TEST(RTState_V1_8, testRandomDataParsing)
{
RandomDataTest rdt(812);
BinParser bp = rdt.getParser(true);
RTState_V1_8 state;
EXPECT_TRUE(state.parseWith(bp)) << "parse() returned false";
ASSERT_EQ(rdt.getNext<double>(), state.time);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.q_target);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qd_target);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qdd_target);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_target);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.m_target);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.q_actual);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qd_actual);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_actual);
ASSERT_EQ(rdt.getNext<double3_t>(), state.tool_accelerometer_values);
rdt.skip(sizeof(double) * 15);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.tcp_force);
ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tool_vector_actual);
ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tcp_speed_actual);
ASSERT_EQ(rdt.getNext<uint64_t>(), state.digital_inputs);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.motor_temperatures);
ASSERT_EQ(rdt.getNext<double>(), state.controller_time);
rdt.skip(sizeof(double)); // skip unused value
ASSERT_EQ(rdt.getNext<double>(), state.robot_mode);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.joint_modes);
EXPECT_TRUE(bp.empty()) << "Did not consume all data";
}
TEST(RTState_V3_0__1, testRandomDataParsing)
{
RandomDataTest rdt(1044);
BinParser bp = rdt.getParser(true);
RTState_V3_0__1 state;
EXPECT_TRUE(state.parseWith(bp)) << "parse() returned false";
ASSERT_EQ(rdt.getNext<double>(), state.time);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.q_target);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qd_target);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qdd_target);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_target);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.m_target);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.q_actual);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qd_actual);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_actual);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_control);
ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tool_vector_actual);
ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tcp_speed_actual);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.tcp_force);
ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tool_vector_target);
ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tcp_speed_target);
ASSERT_EQ(rdt.getNext<uint64_t>(), state.digital_inputs);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.motor_temperatures);
ASSERT_EQ(rdt.getNext<double>(), state.controller_time);
rdt.skip(sizeof(double)); // skip unused value
ASSERT_EQ(rdt.getNext<double>(), state.robot_mode);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.joint_modes);
ASSERT_EQ(rdt.getNext<double>(), state.safety_mode);
rdt.skip(sizeof(double) * 6);
ASSERT_EQ(rdt.getNext<double3_t>(), state.tool_accelerometer_values);
rdt.skip(sizeof(double) * 6);
ASSERT_EQ(rdt.getNext<double>(), state.speed_scaling);
ASSERT_EQ(rdt.getNext<double>(), state.linear_momentum_norm);
rdt.skip(sizeof(double) * 2);
ASSERT_EQ(rdt.getNext<double>(), state.v_main);
ASSERT_EQ(rdt.getNext<double>(), state.v_robot);
ASSERT_EQ(rdt.getNext<double>(), state.i_robot);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.v_actual);
EXPECT_TRUE(bp.empty()) << "Did not consume all data";
}
TEST(RTState_V3_2__3, testRandomDataParsing)
{
RandomDataTest rdt(1060);
BinParser bp = rdt.getParser(true);
RTState_V3_2__3 state;
EXPECT_TRUE(state.parseWith(bp)) << "parse() returned false";
ASSERT_EQ(rdt.getNext<double>(), state.time);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.q_target);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qd_target);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qdd_target);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_target);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.m_target);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.q_actual);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qd_actual);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_actual);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_control);
ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tool_vector_actual);
ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tcp_speed_actual);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.tcp_force);
ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tool_vector_target);
ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tcp_speed_target);
ASSERT_EQ(rdt.getNext<uint64_t>(), state.digital_inputs);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.motor_temperatures);
ASSERT_EQ(rdt.getNext<double>(), state.controller_time);
rdt.skip(sizeof(double)); // skip unused value
ASSERT_EQ(rdt.getNext<double>(), state.robot_mode);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.joint_modes);
ASSERT_EQ(rdt.getNext<double>(), state.safety_mode);
rdt.skip(sizeof(double) * 6);
ASSERT_EQ(rdt.getNext<double3_t>(), state.tool_accelerometer_values);
rdt.skip(sizeof(double) * 6);
ASSERT_EQ(rdt.getNext<double>(), state.speed_scaling);
ASSERT_EQ(rdt.getNext<double>(), state.linear_momentum_norm);
rdt.skip(sizeof(double) * 2);
ASSERT_EQ(rdt.getNext<double>(), state.v_main);
ASSERT_EQ(rdt.getNext<double>(), state.v_robot);
ASSERT_EQ(rdt.getNext<double>(), state.i_robot);
ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.v_actual);
ASSERT_EQ(rdt.getNext<uint64_t>(), state.digital_outputs);
ASSERT_EQ(rdt.getNext<double>(), state.program_state);
EXPECT_TRUE(bp.empty()) << "did not consume all data";
}
TEST(RTState_V1_6__7, testTooSmallBuffer)
{
RandomDataTest rdt(10);
BinParser bp = rdt.getParser(true);
RTState_V1_6__7 state;
EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough";
}
TEST(RTState_V1_8, testTooSmallBuffer)
{
RandomDataTest rdt(10);
BinParser bp = rdt.getParser(true);
RTState_V1_8 state;
EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough";
}
TEST(RTState_V3_0__1, testTooSmallBuffer)
{
RandomDataTest rdt(10);
BinParser bp = rdt.getParser(true);
RTState_V3_0__1 state;
EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough";
}
TEST(RTState_V3_2__3, testTooSmallBuffer)
{
RandomDataTest rdt(10);
BinParser bp = rdt.getParser(true);
RTState_V3_2__3 state;
EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough";
}