Cleaned up a bit and removed a few global vars

CMakeLists.txt

@@ -0,0 +1,209 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ur_modern_driver)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  actionlib
+  control_msgs
+  geometry_msgs
+  roscpp
+  sensor_msgs
+  trajectory_msgs
+  ur_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   control_msgs#   geometry_msgs#   sensor_msgs#   trajectory_msgs#   ur_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+#  LIBRARIES ur_modern_driver
+#  CATKIN_DEPENDS actionlib control_msgs geometry_msgs roscpp sensor_msgs trajectory_msgs ur_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+# check c++11 / c++0x
+include(CheckCXXCompilerFlag)
+CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+if(COMPILER_SUPPORTS_CXX11)
+    set(CMAKE_CXX_FLAGS "-std=c++11")
+elseif(COMPILER_SUPPORTS_CXX0X)
+    set(CMAKE_CXX_FLAGS "-std=c++0x")
+else()
+    message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+endif()
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(include 
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(ur_modern_driver
+#   src/${PROJECT_NAME}/ur_modern_driver.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(ur_modern_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+set(${PROJECT_NAME}_SOURCES
+    src/ur_ros_wrapper.cpp
+    src/ur_realtime_communication.cpp
+    src/ur_driver.cpp
+    src/robot_state_RT.cpp)
+add_executable(ur_driver ${${PROJECT_NAME}_SOURCES})
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+ 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
+   ${catkin_LIBRARIES}
+ )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ur_modern_driver ur_driver
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## 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)

include/ur_modern_driver/robot_state_RT.h

@@ -0,0 +1,101 @@
+/*
+ * robotStateRT.h
+ *
+ * ----------------------------------------------------------------------------
+ * "THE BEER-WARE LICENSE" (Revision 42):
+ * <thomas.timm.dk@gmail.com> wrote this file.  As long as you retain this notice you
+ * can do whatever you want with this stuff. If we meet some day, and you think
+ * this stuff is worth it, you can buy me a beer in return.   Thomas Timm Andersen
+ * ----------------------------------------------------------------------------
+ */
+
+#ifndef ROBOTSTATERT_H_
+#define ROBOTSTATERT_H_
+
+#include <inttypes.h>
+#include <vector>
+#include <stdlib.h>
+#include <string.h>
+#include <mutex>
+#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 new_data_available_; //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);
+	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();
+	bool getNewDataAvailable();
+	void finishedReading();
+	std::vector<double> getVActual();
+	void unpack(uint8_t * buf);
+};
+
+#endif /* ROBOTSTATERT_H_ */

include/ur_modern_driver/ur_driver.h

@@ -0,0 +1,32 @@
+/*
+ * ur_driver
+ *
+ * ----------------------------------------------------------------------------
+ * "THE BEER-WARE LICENSE" (Revision 42):
+ * <thomas.timm.dk@gmail.com> wrote this file.  As long as you retain this notice you
+ * can do whatever you want with this stuff. If we meet some day, and you think
+ * this stuff is worth it, you can buy me a beer in return.   Thomas Timm Andersen
+ * ----------------------------------------------------------------------------
+ */
+
+#ifndef UR_DRIVER_H_
+#define UR_DRIVER_H_
+
+#include <mutex>
+#include <condition_variable>
+#include "ur_realtime_communication.h"
+
+class UrDriver {
+public:
+	UrRealtimeCommunication* rt_interface_;
+
+	UrDriver(std::condition_variable& msg_cond, std::string host,
+			uint safety_count_max = 12);
+	void start();
+	void halt();
+	void setSpeed(double q0, double q1, double q2, double q3, double q4,
+			double q5, double acc = 100.);
+
+};
+
+#endif /* UR_DRIVER_H_ */

include/ur_modern_driver/ur_realtime_communication.h

@@ -0,0 +1,62 @@
+/*
+ * ur_realtime_communication.h
+ *
+ * ----------------------------------------------------------------------------
+ * "THE BEER-WARE LICENSE" (Revision 42):
+ * <thomas.timm.dk@gmail.com> wrote this file.  As long as you retain this notice you
+ * can do whatever you want with this stuff. If we meet some day, and you think
+ * this stuff is worth it, you can buy me a beer in return.   Thomas Timm Andersen
+ * ----------------------------------------------------------------------------
+ */
+
+#ifndef UR_REALTIME_COMMUNICATION_H_
+#define UR_REALTIME_COMMUNICATION_H_
+
+#include "robot_state_RT.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>
+
+class UrRealtimeCommunication {
+private:
+	const double SAMPLETIME_;
+	uint safety_count_max_;
+	int sockfd_;
+	struct sockaddr_in serv_addr_;
+	struct hostent *server_;
+	bool keepalive_;
+	std::thread comThread_;
+	int flag_;
+	std::recursive_mutex command_string_lock_;
+	std::string command_;
+	uint safety_count_;
+	void run();
+
+public:
+	bool connected_;
+	RobotStateRT* robot_state_;
+
+	UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host,
+			uint safety_count_max = 12);
+	void 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);
+
+};
+
+#endif /* UR_REALTIME_COMMUNICATION_H_ */

package.xml

@@ -0,0 +1,64 @@
+<?xml version="1.0"?>
+<package>
+  <name>ur_modern_driver</name>
+  <version>0.0.1</version>
+  <description>The new driver for the UR3/UR5/UR10 robot arms from universal robots</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="thomas.timm.dk@gmail.com">Thomas Timm Andersen</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>Beerware</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <url type="website">http://wiki.ros.org/ur_modern_driver</url>
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <author email="thomas.timm.dk@gmail.com">Thomas Timm Andersen</author>
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>actionlib</build_depend>
+  <build_depend>control_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>trajectory_msgs</build_depend>
+  <build_depend>ur_msgs</build_depend>
+  <run_depend>actionlib</run_depend>
+  <run_depend>control_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>trajectory_msgs</run_depend>
+  <run_depend>ur_msgs</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/robot_state_RT.cpp

@@ -0,0 +1,392 @@
+/*
+ * robotStateRT.cpp
+ *
+ * ----------------------------------------------------------------------------
+ * "THE BEER-WARE LICENSE" (Revision 42):
+ * <thomas.timm.dk@gmail.com> wrote this file.  As long as you retain this notice you
+ * can do whatever you want with this stuff. If we meet some day, and you think
+ * this stuff is worth it, you can buy me a beer in return.   Thomas Timm Andersen
+ * ----------------------------------------------------------------------------
+ */
+
+#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);
+	new_data_available_ = false;
+	pMsg_cond_ = &msg_cond;
+}
+
+bool RobotStateRT::getNewDataAvailable() {
+	return new_data_available_;
+}
+
+void RobotStateRT::finishedReading() {
+	new_data_available_ = false;
+}
+
+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;
+}
+
+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 = q_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();
+	if (version_ == 0.0) {
+		uint32_t len;
+		memcpy(&len, &buf[offset], sizeof(len));
+		if (len <= 756) {
+			version_ = 1.6;
+		} else if (len <= 764) {
+			version_ = 1.7;
+		} else if (len <= 812) {
+			version_ = 1.8;
+		} else if (len <= 1044) {
+			version_ = 3.0;
+		}
+	}
+	offset += 4;
+	memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+	time_ = 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.8) {
+		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();
+	new_data_available_ = true;
+	pMsg_cond_->notify_all();
+
+}
+

src/ur_driver.cpp

@@ -0,0 +1,33 @@
+/*
+ * ur_driver.cpp
+ *
+ * ----------------------------------------------------------------------------
+ * "THE BEER-WARE LICENSE" (Revision 42):
+ * <thomas.timm.dk@gmail.com> wrote this file.  As long as you retain this notice you
+ * can do whatever you want with this stuff. If we meet some day, and you think
+ * this stuff is worth it, you can buy me a beer in return.   Thomas Timm Andersen
+ * ----------------------------------------------------------------------------
+ */
+
+#include "ur_modern_driver/ur_driver.h"
+
+UrDriver::UrDriver(std::condition_variable& msg_cond, std::string host,
+		uint safety_count_max) {
+	rt_interface_ = new UrRealtimeCommunication(msg_cond, host,
+			safety_count_max);
+
+}
+
+void UrDriver::start() {
+	rt_interface_->start();
+}
+
+void UrDriver::halt() {
+	rt_interface_->halt();
+}
+
+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);
+}
+

src/ur_realtime_communication.cpp

@@ -0,0 +1,107 @@
+/*
+ * ur_realtime_communication.cpp
+ *
+ * ----------------------------------------------------------------------------
+ * "THE BEER-WARE LICENSE" (Revision 42):
+ * <thomas.timm.dk@gmail.com> wrote this file.  As long as you retain this notice you
+ * can do whatever you want with this stuff. If we meet some day, and you think
+ * this stuff is worth it, you can buy me a beer in return.   Thomas Timm Andersen
+ * ----------------------------------------------------------------------------
+ */
+
+#include "ur_modern_driver/ur_realtime_communication.h"
+
+UrRealtimeCommunication::UrRealtimeCommunication(
+		std::condition_variable& msg_cond, std::string host,
+		uint safety_count_max) :
+		SAMPLETIME_(0.008) {
+	robot_state_ = new RobotStateRT(msg_cond);
+	bzero((char *) &serv_addr_, sizeof(serv_addr_));
+	sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+	if (sockfd_ < 0) {
+		printf("ERROR opening socket");
+		exit(0);
+	}
+	server_ = gethostbyname(host.c_str());
+	if (server_ == NULL) {
+		printf("ERROR, no such host\n");
+		exit(0);
+	}
+	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_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int));
+	connected_ = false;
+	keepalive_ = false;
+	safety_count_ = 0;
+	safety_count_max_ = safety_count_max;
+}
+
+void UrRealtimeCommunication::start() {
+	keepalive_ = true;
+	if (connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_))
+			< 0)
+		printf("Error connecting");
+	printf("connecting...\n");
+	comThread_ = std::thread(&UrRealtimeCommunication::run, this);
+}
+
+void UrRealtimeCommunication::halt() {
+	keepalive_ = false;
+	comThread_.join();
+}
+
+void UrRealtimeCommunication::addCommandToQueue(std::string inp) {
+	if (inp.back() != '\n') {
+		inp.append("\n");
+	}
+	command_string_lock_.lock();
+	command_ += inp;
+	command_string_lock_.unlock();
+}
+
+void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
+		double q3, double q4, double q5, double acc) {
+	char cmd[1024];
+	sprintf(cmd,
+			"speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %1.5f,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];
+	bzero(buf, 2048);
+	printf("Got connection\n");
+	connected_ = true;
+	while (keepalive_) {
+		read(sockfd_, buf, 2048);
+		setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
+				sizeof(int));
+		robot_state_->unpack(buf);
+		command_string_lock_.lock();
+		if (command_.length() != 0) {
+			write(sockfd_, command_.c_str(), command_.length());
+			command_ = "";
+		}
+		if (safety_count_ == safety_count_max_) {
+			setSpeed(0., 0., 0., 0., 0., 0.);
+			write(sockfd_, command_.c_str(), command_.length());
+			command_ = "";
+		}
+		safety_count_ += 1;
+		command_string_lock_.unlock();
+
+	}
+}
+
+void UrRealtimeCommunication::setSafetyCountMax(uint inp) {
+	safety_count_max_ = inp;
+}
+

src/ur_ros_wrapper.cpp

@@ -0,0 +1,93 @@
+/*
+ * ur_driver.cpp
+ *
+ * ----------------------------------------------------------------------------
+ * "THE BEER-WARE LICENSE" (Revision 42):
+ * <thomas.timm.dk@gmail.com> wrote this file.  As long as you retain this notice you
+ * can do whatever you want with this stuff. If we meet some day, and you think
+ * this stuff is worth it, you can buy me a beer in return.   Thomas Timm Andersen
+ * ----------------------------------------------------------------------------
+ */
+
+#include "ros/ros.h"
+#include <ros/console.h>
+#include "ur_modern_driver/ur_driver.h"
+#include <string.h>
+#include <vector>
+#include <mutex>
+#include <condition_variable>
+#include "sensor_msgs/JointState.h"
+#include "geometry_msgs/WrenchStamped.h"
+#include <thread>
+
+std::condition_variable g_msg_cond;
+
+void publishRTMsg(UrDriver robot, std::vector<std::string> joint_names) {
+	ros::NodeHandle n_rt;
+	ros::Publisher joint_pub = n_rt.advertise<sensor_msgs::JointState>("/joint_states", 1);
+	ros::Publisher wrench_pub = n_rt.advertise<geometry_msgs::WrenchStamped>("/wrench", 1);
+	while (ros::ok()) {
+		sensor_msgs::JointState joint_msg;
+		joint_msg.name = joint_names;
+		geometry_msgs::WrenchStamped wrench_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_->getNewDataAvailable()) {
+			g_msg_cond.wait(locker);
+		}
+		joint_msg.header.stamp = ros::Time::now();
+		joint_msg.position = robot.rt_interface_->robot_state_->getQActual();
+		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);
+
+		robot.rt_interface_->robot_state_->finishedReading();
+
+	}
+}
+
+int main(int argc, char **argv) {
+	bool use_sim_time = false;
+	std::string joint_prefix ="";
+	std::string host;
+	std::vector<std::string> joint_names;
+	UrDriver robot(g_msg_cond, host);
+
+
+	ros::init(argc, argv, "ur_driver");
+	ros::NodeHandle n;
+	if (ros::param::get("use_sim_time", use_sim_time)) {
+		ROS_WARN("use_sim_time is set!!");
+	}
+	if (ros::param::get("~prefix", joint_prefix)) {
+		ROS_INFO("Setting prefix to %s", joint_prefix.c_str());
+	}
+	joint_names.push_back(joint_prefix + "shoulder_pan_joint");
+	joint_names.push_back(joint_prefix + "should_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");
+
+	if (argc > 1) {
+		host = argv[1];
+	} else if (!(ros::param::get("~robot_ip", host))) {
+		ROS_FATAL("Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip");
+		exit(1);
+	}
+	robot.start();
+
+	std::thread rt_publish_thread(publishRTMsg, robot, joint_names);
+	ros::spin();
+	robot.halt();
+	exit(0);
+}