Merge branch 'ros-control'

CMakeLists.txt

@@ -5,6 +5,8 @@ add_definitions( -DROS_BUILD )
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
+  hardware_interface
+  controller_manager
   actionlib
   control_msgs
   geometry_msgs
@@ -107,8 +109,8 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package(
   INCLUDE_DIRS include
 #  LIBRARIES ur_modern_driver
-#  CATKIN_DEPENDS actionlib control_msgs geometry_msgs roscpp sensor_msgs trajectory_msgs ur_msgs
+  CATKIN_DEPENDS hardware_interface controller_manager actionlib control_msgs geometry_msgs roscpp sensor_msgs trajectory_msgs ur_msgs
-#  DEPENDS system_lib
+  DEPENDS ur_hardware_interface
 )
 
 ###########
@@ -135,9 +137,12 @@ include_directories(include
 )
 
 ## Declare a C++ library
-# add_library(ur_modern_driver
+
-#   src/${PROJECT_NAME}/ur_modern_driver.cpp
+# Hardware Interface
-# )
+add_library(ur_hardware_interface src/ur_hardware_interface.cpp)
+target_link_libraries(ur_hardware_interface
+  ${catkin_LIBRARIES}
+)
 
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
@@ -160,7 +165,8 @@ add_executable(ur_driver ${${PROJECT_NAME}_SOURCES})
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(ur_driver
-   ${catkin_LIBRARIES}
+  ur_hardware_interface
+  ${catkin_LIBRARIES}
  )
 
 #############

README.md

@@ -41,8 +41,6 @@ Besides this, the driver subscribes to two new topics:
 */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.
 
 
-This driver is written in c++, which should make it possible to integrate it with ros_control. If you feel like undertaking this task, please dive right in. I don't have the posibility to do this.
-
 No script is sent to the robot. This means that the teach pendant can be used to move the robot around while the driver is running.
 
 ---

config/ur10_controllers.yaml

@@ -0,0 +1,60 @@
+# Settings for ros_control control loop
+hardware_control_loop:
+   loop_hz: 125
+
+# Settings for ros_control hardware interface
+hardware_interface:
+   joints:
+     - shoulder_pan_joint
+     - shoulder_lift_joint
+     - elbow_joint
+     - wrist_1_joint
+     - wrist_2_joint
+     - wrist_3_joint
+
+# Publish all joint states ----------------------------------
+joint_state_controller:
+   type:         joint_state_controller/JointStateController
+   publish_rate: 125
+
+# Publish wrench ----------------------------------
+force_torque_sensor_controller:
+   type:         force_torque_sensor_controller/ForceTorqueSensorController
+   publish_rate: 125
+
+# Joint Trajectory Controller -------------------------------
+# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
+position_trajectory_controller:
+   type: velocity_controllers/JointTrajectoryController
+   joints:
+     - shoulder_pan_joint
+     - shoulder_lift_joint
+     - elbow_joint
+     - wrist_1_joint
+     - wrist_2_joint
+     - wrist_3_joint
+   constraints:
+      goal_time: 0.6
+      stopped_velocity_tolerance: 0.05
+      shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
+      shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
+      elbow_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_1_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_2_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_3_joint: {trajectory: 0.1, goal: 0.1}
+   stop_trajectory_duration: 0.5
+   state_publish_rate:  125
+   action_monitor_rate: 10
+   gains:
+      #!!These values have not been optimized!!
+      shoulder_pan_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      shoulder_lift_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      elbow_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      wrist_1_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      wrist_2_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      wrist_3_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+
+   # state_publish_rate:  50 # Defaults to 50
+   # action_monitor_rate: 20 # Defaults to 20
+   #hold_trajectory_duration: 0 # Defaults to 0.5
+

config/ur5_controllers.yaml

@@ -0,0 +1,60 @@
+# Settings for ros_control control loop
+hardware_control_loop:
+   loop_hz: 125
+
+# Settings for ros_control hardware interface
+hardware_interface:
+   joints:
+     - shoulder_pan_joint
+     - shoulder_lift_joint
+     - elbow_joint
+     - wrist_1_joint
+     - wrist_2_joint
+     - wrist_3_joint
+
+# Publish all joint states ----------------------------------
+joint_state_controller:
+   type:         joint_state_controller/JointStateController
+   publish_rate: 125
+
+# Publish wrench ----------------------------------
+force_torque_sensor_controller:
+   type:         force_torque_sensor_controller/ForceTorqueSensorController
+   publish_rate: 125
+
+# Joint Trajectory Controller -------------------------------
+# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
+position_trajectory_controller:
+   type: velocity_controllers/JointTrajectoryController
+   joints:
+     - shoulder_pan_joint
+     - shoulder_lift_joint
+     - elbow_joint
+     - wrist_1_joint
+     - wrist_2_joint
+     - wrist_3_joint
+   constraints:
+      goal_time: 0.6
+      stopped_velocity_tolerance: 0.05
+      shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
+      shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
+      elbow_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_1_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_2_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_3_joint: {trajectory: 0.1, goal: 0.1}
+   stop_trajectory_duration: 0.5
+   state_publish_rate:  125
+   action_monitor_rate: 10
+   gains:
+      #!!These values have not been optimized!!
+      shoulder_pan_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      shoulder_lift_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      elbow_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      wrist_1_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      wrist_2_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      wrist_3_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+
+   # state_publish_rate:  50 # Defaults to 50
+   # action_monitor_rate: 20 # Defaults to 20
+   #hold_trajectory_duration: 0 # Defaults to 0.5
+

include/ur_modern_driver/ur_communication.h

@@ -29,6 +29,8 @@
 #include <iostream>
 #include <unistd.h>
 #include <chrono>
+#include <fcntl.h>
+#include <sys/types.h>
 
 #ifdef ROS_BUILD
 #include <ros/ros.h>
@@ -49,7 +51,7 @@ public:
 	RobotState* robot_state_;
 
 	UrCommunication(std::condition_variable& msg_cond, std::string host);
-	void start();
+	bool start();
 	void halt();
 
 };

include/ur_modern_driver/ur_hardware_interface.h

@@ -0,0 +1,119 @@
+/*
+ * ur_hardware_control_loop.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
+ * ----------------------------------------------------------------------------
+ */
+
+/* Based on original source from University of Colorado, Boulder. License copied below. Feel free to offer Dave a beer as well if you meet him. */
+
+/*********************************************************************
+ * 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
+ Desc:   Example ros_control hardware interface that performs a perfect control loop for simulation
+ */
+
+#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 "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 doSwitch(const std::list<hardware_interface::ControllerInfo>&, const std::list<hardware_interface::ControllerInfo>&);
+
+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::VelocityJointInterface velocity_joint_interface_;
+	bool velocity_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_velocity_command_;
+	std::size_t num_joints_;
+	double robot_force_[3] = {0.,0.,0.};
+	double robot_torque_[3] = {0.,0.,0.};
+
+	// Robot API
+	UrDriver* robot_;
+
+};
+// class
+
+}// namespace
+
+#endif

launch/ur10_ros_control.launch

@@ -0,0 +1,40 @@
+<launch>
+
+  <!-- GDB functionality -->
+  <arg name="debug" default="false" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
+  <arg name="robot_ip"/>
+  <arg name="limited" default="false"/>
+  <arg name="min_payload"  default="0.0"/>
+  <arg name="max_payload"  default="10.0"/>
+
+  <!-- robot model -->
+  <include file="$(find ur_description)/launch/ur10_upload.launch">
+    <arg name="limited" value="$(arg limited)"/>
+  </include>
+
+  <group ns="universal_robot">
+
+    <!-- Load hardware interface -->
+    <node name="ur_hardware_interface" pkg="ur_modern_driver" type="ur_driver" output="screen" launch-prefix="$(arg launch_prefix)">
+	    <param name="robot_ip_address" type="str" value="$(arg robot_ip)"/>
+	    <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)"/>
+    </node>
+
+    <!-- Load controller settings -->
+    <rosparam file="$(find ur_modern_driver)/config/ur10_controllers.yaml" command="load"/>
+
+    <!-- Load controller manager -->
+    <node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
+	  output="screen" args="spawn joint_state_controller force_torque_sensor_controller position_trajectory_controller" />
+
+    <!-- Convert joint states to /tf tranforms -->
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
+
+  </group>
+
+</launch>
+

launch/ur5_bringup.launch

@@ -12,7 +12,7 @@
   <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="max_payload"  default="5.0"/>
 
   <!-- robot model -->
   <include file="$(find ur_description)/launch/ur5_upload.launch">

launch/ur5_ros_control.launch

@@ -0,0 +1,40 @@
+<launch>
+
+  <!-- GDB functionality -->
+  <arg name="debug" default="false" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
+  <arg name="robot_ip"/>
+  <arg name="limited" default="false"/>
+  <arg name="min_payload"  default="0.0"/>
+  <arg name="max_payload"  default="5.0"/>
+  <arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
+  <!-- robot model -->
+  <include file="$(find ur_description)/launch/ur5_upload.launch">
+    <arg name="limited" value="$(arg limited)"/>
+  </include>
+
+  <group ns="universal_robot">
+
+    <!-- Load hardware interface -->
+    <node name="ur_hardware_interface" pkg="ur_modern_driver" type="ur_driver" output="screen" launch-prefix="$(arg launch_prefix)">
+	    <param name="robot_ip_address" type="str" value="$(arg robot_ip)"/>
+	    <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)"/>
+    </node>
+
+    <!-- Load controller settings -->
+    <rosparam file="$(find ur_modern_driver)/config/ur5_controllers.yaml" command="load"/>
+
+    <!-- Load controller manager -->
+    <!-- <node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
+	  output="screen" args="spawn joint_state_controller force_torque_sensor_controller position_trajectory_controller" /> -->
+
+    <!-- Convert joint states to /tf tranforms -->
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
+
+  </group>
+
+</launch>
+

package.xml

@@ -40,6 +40,8 @@
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>hardware_interface</build_depend>
+  <build_depend>controller_manager</build_depend>
   <build_depend>actionlib</build_depend>
   <build_depend>control_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
@@ -48,6 +50,9 @@
   <build_depend>std_msgs</build_depend>
   <build_depend>trajectory_msgs</build_depend>
   <build_depend>ur_msgs</build_depend>
+  <run_depend>hardware_interface</run_depend>
+  <run_depend>controller_manager</run_depend>
+  <run_depend>ros_controllers</run_depend>
   <run_depend>actionlib</run_depend>
   <run_depend>control_msgs</run_depend>
   <run_depend>geometry_msgs</run_depend>
@@ -56,6 +61,7 @@
   <run_depend>std_msgs</run_depend>
   <run_depend>trajectory_msgs</run_depend>
   <run_depend>ur_msgs</run_depend>
+  <run_depend>ur_description</run_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

src/ur_communication.cpp

@@ -61,11 +61,12 @@ UrCommunication::UrCommunication(std::condition_variable& msg_cond,
 			sizeof(int));
 	setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
 			sizeof(int));
+	fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
 	connected_ = false;
 	keepalive_ = false;
 }
 
-void UrCommunication::start() {
+bool UrCommunication::start() {
 	keepalive_ = true;
 	uint8_t buf[512];
 	unsigned int bytes_read;
@@ -81,9 +82,10 @@ void UrCommunication::start() {
 #ifdef ROS_BUILD
 		ROS_FATAL("Error connecting to get firmware version");
 		ros::shutdown();
+		return false;
 #else
 		printf("Error connecting to get firmware version\n");
-		exit(1);
+		return false;
 #endif
 	}
 #ifdef ROS_BUILD
@@ -110,14 +112,27 @@ void UrCommunication::start() {
 	printf(
 			"Switching to secondary interface for masterboard data: Connecting...\n"); // which generates less network traffic
 #endif
-	if (connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_,
+
-			sizeof(sec_serv_addr_)) < 0) {
+	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) {
 #ifdef ROS_BUILD
-		ROS_FATAL("Error connecting");
+		ROS_FATAL("Error connecting to secondary interface");
 		ros::shutdown();
+		return false;
 #else
-		printf("Error connecting\n");
+		printf("Error connecting to secondary interface\n");
-		exit(1);
+		return false;
 #endif
 	}
 #ifdef ROS_BUILD
@@ -126,6 +141,7 @@ void UrCommunication::start() {
 	printf("Secondary interface: Got connection\n");
 #endif
 	comThread_ = std::thread(&UrCommunication::run, this);
+	return true;
 }
 
 void UrCommunication::halt() {
@@ -135,15 +151,79 @@ void UrCommunication::halt() {
 
 void UrCommunication::run() {
 	uint8_t buf[2048];
-	unsigned int bytes_read;
+	int bytes_read;
 	bzero(buf, 2048);
+	struct timeval timeout;
+	fd_set readfds;
+	FD_ZERO(&readfds);
+	FD_SET(sec_sockfd_, &readfds);
 	connected_ = true;
 	while (keepalive_) {
-		bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes
+		while (connected_ && keepalive_) {
-		setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
+			timeout.tv_sec = 0; //do this each loop as selects modifies timeout
-				sizeof(int));
+			timeout.tv_usec = 500000; // timeout of 0.5 sec
-		robot_state_->unpack(buf, bytes_read);
+			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_NODELAY,
+						(char *) &flag_, sizeof(int));
+				robot_state_->unpack(buf, bytes_read);
+			} else {
+				connected_ = false;
+				close (sec_sockfd_);
+			}
+		}
+		if (keepalive_) {
+			//reconnect
+#ifdef ROS_BUILD
+			ROS_WARN("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
+#else
+			printf(
+					"Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...\n");
+#endif
+			sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+			if (sec_sockfd_ < 0) {
+#ifdef ROS_BUILD
+				ROS_FATAL("ERROR opening secondary socket");
+				ros::shutdown();
+#else
+				printf("ERROR opening secondary socket");
+				exit(1);
+#endif
+			}
+			flag_ = 1;
+			setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (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;
+				keepalive_ = true;
+
+				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) {
+#ifdef ROS_BUILD
+					ROS_ERROR("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds...");
+#else
+					printf(
+							"Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds...");
+#endif
+				} else {
+					connected_ = true;
+				}
+			}
+		}
 	}
+
 	//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_);

src/ur_driver.cpp

@@ -91,9 +91,9 @@ void UrDriver::addTraj(std::vector<double> inp_timestamps,
 	}
 	//make sure we come to a smooth stop
 	/*while (timestamps.back() < inp_timestamps.back()) {
-		timestamps.push_back(timestamps.back() + 0.008);
+	 timestamps.push_back(timestamps.back() + 0.008);
-	}
+	 }
-	timestamps.pop_back();*/
+	 timestamps.pop_back();*/
 
 	unsigned int j = 0;
 	for (unsigned int i = 0; i < timestamps.size(); i++) {
@@ -162,7 +162,7 @@ void UrDriver::doTraj(std::vector<double> inp_timestamps,
 		}
 		positions = UrDriver::interp_cubic(
 				std::chrono::duration_cast<std::chrono::duration<double>>(
-						t - t0).count()- inp_timestamps[j - 1],
+						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]);
 		for (int i = 0; i < 6; i++) {
@@ -297,16 +297,17 @@ void UrDriver::uploadProg() {
 }
 
 void UrDriver::start() {
-	sec_interface_->start();
+	if (sec_interface_->start()) {
-	rt_interface_->robot_state_->setVersion(
+		rt_interface_->robot_state_->setVersion(
-			sec_interface_->robot_state_->getVersion());
+				sec_interface_->robot_state_->getVersion());
-	rt_interface_->start();
+		rt_interface_->start();
-	ip_addr_ = rt_interface_->getLocalIp(); //inet_ntoa(serv_addr.sin_addr);
+		ip_addr_ = rt_interface_->getLocalIp(); //inet_ntoa(serv_addr.sin_addr);
 #ifdef ROS_BUILD
-			ROS_DEBUG("Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_);
+		ROS_DEBUG("Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_);
 #else
-	printf("Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_);
+		printf("Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_);
 #endif
+	}
 }
 
 void UrDriver::halt() {

src/ur_hardware_interface.cpp

@@ -0,0 +1,178 @@
+/*
+ * ur_hardware_control_loop.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
+ * ----------------------------------------------------------------------------
+ */
+
+/* Based on original source from University of Colorado, Boulder. License copied below. Feel free to offer Dave a beer as well if you meet him. */
+
+/*********************************************************************
+ * 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
+ Desc:   Example ros_control hardware interface that performs a perfect control loop for simulation
+ */
+
+#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
+
+	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_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 velocity joint interface
+		velocity_joint_interface_.registerHandle(
+				hardware_interface::JointHandle(
+						joint_state_interface_.getHandle(joint_names_[i]),
+						&joint_velocity_command_[i]));
+	}
+
+	// Create joint state interface
+	force_torque_interface_.registerHandle(
+			hardware_interface::ForceTorqueSensorHandle("wrench", "",
+					robot_force_, robot_torque_));
+
+	registerInterface(&joint_state_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;
+}
+
+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::write() {
+	if (velocity_interface_running_) {
+		robot_->setSpeed(joint_velocity_command_[0], joint_velocity_command_[1],
+				joint_velocity_command_[2], joint_velocity_command_[3],
+				joint_velocity_command_[4], joint_velocity_command_[5], 100);
+	}
+}
+
+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 =
+			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");
+		}
+	}
+	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");
+		}
+	}
+	/*std::string outp;
+	 outp = "doSwitch called - Start list:";
+	 for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
+	 start_list.begin(); controller_it != start_list.end();
+	 ++controller_it) {
+	 outp += " " + controller_it->name + "(" + controller_it->hardware_interface + ")";
+	 }
+	 outp += " - Stop list:";
+	 for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
+	 stop_list.begin(); controller_it != stop_list.end();
+	 ++controller_it) {
+	 outp += " " + controller_it->name + "(" + controller_it->hardware_interface + ")";
+
+	 }
+	 ROS_INFO(outp.c_str()); */
+
+}
+
+} // namespace
+

src/ur_ros_wrapper.cpp

@@ -10,6 +10,7 @@
  */
 
 #include "ur_modern_driver/ur_driver.h"
+#include "ur_modern_driver/ur_hardware_interface.h"
 #include <string.h>
 #include <vector>
 #include <mutex>
@@ -17,6 +18,7 @@
 #include <thread>
 #include <algorithm>
 #include <cmath>
+#include <time.h>
 
 #include "ros/ros.h"
 #include <ros/console.h>
@@ -35,6 +37,7 @@
 #include "ur_msgs/Digital.h"
 #include "ur_msgs/Analog.h"
 #include "std_msgs/String.h"
+#include <controller_manager/controller_manager.h>
 
 class RosWrapper {
 protected:
@@ -52,9 +55,12 @@ protected:
 	ros::ServiceServer payload_srv_;
 	std::thread* rt_publish_thread_;
 	std::thread* mb_publish_thread_;
+	std::thread* ros_control_thread_;
 	double io_flag_delay_;
 	double max_velocity_;
 	std::vector<double> joint_offsets_;
+	boost::shared_ptr<ros_control_ur::UrHardwareInterface> hardware_interface_;
+	boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
 
 public:
 	RosWrapper(std::string host) :
@@ -76,6 +82,11 @@ public:
 		joint_names.push_back(joint_prefix + "wrist_3_joint");
 		robot_.setJointNames(joint_names);
 
+		hardware_interface_.reset(new ros_control_ur::UrHardwareInterface(nh_, &robot_));
+		controller_manager_.reset(
+				new controller_manager::ControllerManager(
+						hardware_interface_.get(), nh_));
+
 		//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_)) {
@@ -102,13 +113,13 @@ public:
 		robot_.start();
 
 		//register the goal and feedback callbacks
-		as_.registerGoalCallback(boost::bind(&RosWrapper::goalCB, this));
+		//as_.registerGoalCallback(boost::bind(&RosWrapper::goalCB, this));
-		as_.registerPreemptCallback(boost::bind(&RosWrapper::preemptCB, this));
+		//as_.registerPreemptCallback(boost::bind(&RosWrapper::preemptCB, this));
 
-		as_.start();
+		//as_.start();
 
 		//subscribe to the data topic of interest
-		speed_sub_ = nh_.subscribe("joint_speed", 1,
+		speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1,
 				&RosWrapper::speedInterface, this);
 		urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1,
 				&RosWrapper::urscriptInterface, this);
@@ -118,8 +129,10 @@ public:
 		payload_srv_ = nh_.advertiseService("ur_driver/set_payload",
 				&RosWrapper::setPayload, this);
 
-		rt_publish_thread_ = new std::thread(
+		ros_control_thread_ = new std::thread(
-				boost::bind(&RosWrapper::publishRTMsg, this));
+				boost::bind(&RosWrapper::rosControlLoop, this));
+		/*rt_publish_thread_ = new std::thread(
+				boost::bind(&RosWrapper::publishRTMsg, this)); */
 		mb_publish_thread_ = new std::thread(
 				boost::bind(&RosWrapper::publishMbMsg, this));
 		ROS_DEBUG("The action server for this driver has been started");
@@ -269,7 +282,7 @@ private:
 						traj.points[i].velocities[mapping[j]]);
 				if (traj.points[i].accelerations.size() != 0)
 					new_point.accelerations.push_back(
-						traj.points[i].accelerations[mapping[j]]);
+							traj.points[i].accelerations[mapping[j]]);
 			}
 			new_point.time_from_start = traj.points[i].time_from_start;
 			new_traj.push_back(new_point);
@@ -330,11 +343,40 @@ private:
 
 	}
 
+	void rosControlLoop() {
+		ros::Duration elapsed_time;
+		struct timespec last_time, current_time;
+		static const double BILLION = 1000000000.0;
+
+		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);
+			}
+			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);
+			last_time = current_time;
+			// Input
+			hardware_interface_->read();
+			robot_.rt_interface_->robot_state_->setControllerUpdated();
+			// Control
+			controller_manager_->update(ros::Time(current_time.tv_sec, current_time.tv_nsec), elapsed_time);
+
+			// Output
+			hardware_interface_->write();
+		}
+	}
+
 	void publishRTMsg() {
 		ros::Publisher joint_pub = nh_.advertise<sensor_msgs::JointState>(
-				"/joint_states", 1);
+				"joint_states", 1);
 		ros::Publisher wrench_pub = nh_.advertise<geometry_msgs::WrenchStamped>(
-				"/wrench", 1);
+				"wrench", 1);
 		while (ros::ok()) {
 			sensor_msgs::JointState joint_msg;
 			joint_msg.name = robot_.getJointNames();
@@ -371,7 +413,7 @@ private:
 	}
 
 	void publishMbMsg() {
-		ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>("/io_states",
+		ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>("ur_driver/io_states",
 				1);
 
 		while (ros::ok()) {
@@ -443,7 +485,11 @@ int main(int argc, char **argv) {
 
 	RosWrapper interface(host);
 
-	ros::spin();
+	ros::AsyncSpinner spinner(3);
+	spinner.start();
+
+	ros::waitForShutdown();
+
 	interface.halt();
 
 	exit(0);