Included a position-based controller. Also prettied up printing

include/ur_modern_driver/do_output.h

@@ -0,0 +1,27 @@
+/*
+ * do_output.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_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_ */

include/ur_modern_driver/ur_communication.h

@@ -13,6 +13,7 @@
 #define UR_COMMUNICATION_H_
 
 #include "robot_state.h"
+#include "do_output.h"
 #include <vector>
 #include <stdlib.h>
 #include <stdio.h>
@@ -32,9 +33,6 @@
 #include <fcntl.h>
 #include <sys/types.h>
 
-#ifdef ROS_BUILD
-#include <ros/ros.h>
-#endif
 
 class UrCommunication {
 private:

include/ur_modern_driver/ur_driver.h

@@ -16,6 +16,7 @@
 #include <condition_variable>
 #include "ur_realtime_communication.h"
 #include "ur_communication.h"
+#include "do_output.h"
 #include <vector>
 #include <math.h>
 #include <string>
@@ -65,7 +66,7 @@ public:
 
 	void uploadProg();
 	void openServo();
-	void closeServo();
+	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,

include/ur_modern_driver/ur_hardware_interface.h

@@ -9,7 +9,7 @@
  * ----------------------------------------------------------------------------
  */
 
-/* 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. */
+/* Based on original source from University of Colorado, Boulder. License copied below. Please offer Dave a beer as well if you meet him. */
 
 /*********************************************************************
  * Software License Agreement (BSD License)
@@ -58,6 +58,7 @@
 #include <controller_manager/controller_manager.h>
 #include <boost/scoped_ptr.hpp>
 #include <ros/ros.h>
+#include "do_output.h"
 #include "ur_driver.h"
 
 namespace ros_control_ur {
@@ -85,7 +86,11 @@ public:
 	/// \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>&);
+	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:
 
@@ -95,17 +100,20 @@ protected:
 	// 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::size_t num_joints_;
-	double robot_force_[3] = {0.,0.,0.};
-	double robot_torque_[3] = {0.,0.,0.};
+	double robot_force_[3] = { 0., 0., 0. };
+	double robot_torque_[3] = { 0., 0., 0. };
 
 	// Robot API
 	UrDriver* robot_;

include/ur_modern_driver/ur_realtime_communication.h

@@ -13,6 +13,7 @@
 #define UR_REALTIME_COMMUNICATION_H_
 
 #include "robot_state_RT.h"
+#include "do_output.h"
 #include <vector>
 #include <stdlib.h>
 #include <stdio.h>
@@ -33,10 +34,6 @@
 #include <fcntl.h>
 #include <sys/types.h>
 
-#ifdef ROS_BUILD
-#include <ros/ros.h>
-#endif
-
 class UrRealtimeCommunication {
 private:
 	unsigned int safety_count_max_;