Included a position-based controller. Also prettied up printing

2026-04-10 10:00:48 +02:00 · 2015-09-25 09:57:33 +02:00
parent a15fcc695c
commit 5c785af6c4
14 changed files with 299 additions and 242 deletions
--- a/src/do_output.cpp
+++ b/src/do_output.cpp
@@ -0,0 +1,50 @@
+/*
+ * do_output.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/du_output.h"
+
+void print_debug(std::string inp) {
+#ifdef ROS_BUILD
+	ROS_DEBUG(inp);
+#else
+	printf("DEBUG: %s\n", inp.c_str());
+#endif
+}
+void print_info(std::string inp) {
+#ifdef ROS_BUILD
+	ROS_INFO(inp);
+#else
+	printf("INFO: %s\n", inp.c_str());
+#endif
+}
+void print_warning(std::string inp) {
+#ifdef ROS_BUILD
+	ROS_WARN(inp);
+#else
+	printf("WARNING: %s\n", inp.c_str());
+#endif
+}
+void print_error(std::string inp) {
+#ifdef ROS_BUILD
+	ROS_ERROR(inp);
+#else
+	printf("ERROR: %s\n", inp.c_str());
+#endif
+}
+void print_fatal(std::string inp) {
+#ifdef ROS_BUILD
+	ROS_FATAL(inp);
+	ros::shutdown();
+#else
+	printf("FATAL: %s\n", inp.c_str());
+	exit(1);
+#endif
+}
--- a/src/ur_communication.cpp
+++ b/src/ur_communication.cpp
@@ -18,33 +18,15 @@ UrCommunication::UrCommunication(std::condition_variable& msg_cond,
 	bzero((char *) &sec_serv_addr_, sizeof(sec_serv_addr_));
 	pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
 	if (pri_sockfd_ < 0) {
-#ifdef ROS_BUILD
-		ROS_FATAL("ERROR opening socket");
-		ros::shutdown();
-#else
-		printf("ERROR opening socket");
-		exit(1);
-#endif
+		print_fatal("ERROR opening socket pri_sockfd");
 	}
 	sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
 	if (sec_sockfd_ < 0) {
-#ifdef ROS_BUILD
-		ROS_FATAL("ERROR opening socket");
-		ros::shutdown();
-#else
-		printf("ERROR opening socket");
-		exit(1);
-#endif
+		print_fatal("ERROR opening socket sec_sockfd");
 	}
 	server_ = gethostbyname(host.c_str());
 	if (server_ == NULL) {
-#ifdef ROS_BUILD
-		ROS_FATAL("ERROR, no such host");
-		ros::shutdown();
-#else
-		printf("ERROR, no such host\n");
-		exit(1);
-#endif
+		print_fatal("ERROR, unknown host");
 	}
 	pri_serv_addr_.sin_family = AF_INET;
 	sec_serv_addr_.sin_family = AF_INET;
@@ -72,46 +54,26 @@ bool UrCommunication::start() {
 	unsigned int bytes_read;
 	std::string cmd;
 	bzero(buf, 512);
-#ifdef ROS_BUILD
-	ROS_DEBUG("Acquire firmware version: Connecting...");
-#else
-	printf("Acquire firmware version: Connecting...\n");
-#endif
+	print_debug("Acquire firmware version: Connecting...");
 	if (connect(pri_sockfd_, (struct sockaddr *) &pri_serv_addr_,
 			sizeof(pri_serv_addr_)) < 0) {
-#ifdef ROS_BUILD
-		ROS_FATAL("Error connecting to get firmware version");
-		ros::shutdown();
+		print_fatal("Error connecting to get firmware version");
 		return false;
-#else
-		printf("Error connecting to get firmware version\n");
-		return false;
-#endif
 	}
-#ifdef ROS_BUILD
-	ROS_DEBUG("Acquire firmware version: Got connection");
-#else
-	printf("Acquire firmware version: Got connection\n");
-#endif
+	print_debug("Acquire firmware version: Got connection");
 	bytes_read = read(pri_sockfd_, buf, 512);
 	setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (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));
-#ifdef ROS_BUILD
-	ROS_DEBUG("Firmware version detected: %1.7f", robot_state_->getVersion());
-#else
-	printf("Firmware version detected: %f\n", robot_state_->getVersion());
-#endif
+	char tmp[64];
+	sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion());
+	print_debug(tmp);
 	close(pri_sockfd_);

-#ifdef ROS_BUILD
-	ROS_DEBUG("Switching to secondary interface for masterboard data: Connecting...");
-#else
-	printf(
-			"Switching to secondary interface for masterboard data: Connecting...\n"); // which generates less network traffic
-#endif
+	print_debug(
+			"Switching to secondary interface for masterboard data: Connecting...");

 	fd_set writefds;
 	struct timeval timeout;
@@ -126,20 +88,10 @@ bool UrCommunication::start() {
 	unsigned int flag_len;
 	getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
 	if (flag_ < 0) {
-#ifdef ROS_BUILD
-		ROS_FATAL("Error connecting to secondary interface");
-		ros::shutdown();
+		print_fatal("Error connecting to secondary interface");
 		return false;
-#else
-		printf("Error connecting to secondary interface\n");
-		return false;
-#endif
 	}
-#ifdef ROS_BUILD
-	ROS_DEBUG("Secondary interface: Got connection");
-#else
-	printf("Secondary interface: Got connection\n");
-#endif
+	print_debug("Secondary interface: Got connection");
 	comThread_ = std::thread(&UrCommunication::run, this);
 	return true;
 }
@@ -170,26 +122,15 @@ void UrCommunication::run() {
 				robot_state_->unpack(buf, bytes_read);
 			} else {
 				connected_ = false;
-				close (sec_sockfd_);
+				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
+			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) {
-#ifdef ROS_BUILD
-				ROS_FATAL("ERROR opening secondary socket");
-				ros::shutdown();
-#else
-				printf("ERROR opening secondary socket");
-				exit(1);
-#endif
+				print_fatal("ERROR opening secondary socket");
 			}
 			flag_ = 1;
 			setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
@@ -211,12 +152,7 @@ void UrCommunication::run() {
 				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
+					print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds...");
 				} else {
 					connected_ = true;
 				}
--- a/src/ur_driver.cpp
+++ b/src/ur_driver.cpp
@@ -24,15 +24,12 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond,

 	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) {
-#ifdef ROS_BUILD
-		ROS_FATAL("ERROR opening socket for reverse communication");
-#else
-		printf("ERROR opening socket for reverse communication");
-#endif
+		print_fatal("ERROR opening socket for reverse communication");
 	}
 	bzero((char *) &serv_addr, sizeof(serv_addr));

@@ -45,11 +42,7 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
 	setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int));
 	if (bind(incoming_sockfd_, (struct sockaddr *) &serv_addr,
 			sizeof(serv_addr)) < 0) {
-#ifdef ROS_BUILD
-		ROS_FATAL("ERROR on binding socket for reverse communication");
-#else
-		printf("ERROR on binding socket for reverse communication");
-#endif
+		print_fatal("ERROR on binding socket for reverse communication");
 	}
 	listen(incoming_sockfd_, 5);
 }
@@ -158,9 +151,7 @@ void UrDriver::doTraj(std::vector<double> inp_timestamps,
 		t = std::chrono::high_resolution_clock::now();
 	}
 	//Signal robot to stop driverProg()
-	UrDriver::servoj(positions, 0.008, 0);
-	UrDriver::closeServo();
-
+	UrDriver::closeServo(positions);
 }

 void UrDriver::servoj(std::vector<double> positions, double time,
@@ -277,14 +268,14 @@ void UrDriver::openServo() {
 	new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr *) &cli_addr,
 			&clilen);
 	if (new_sockfd_ < 0) {
-#ifdef ROS_BUILD
-		ROS_FATAL("ERROR on accepting reverse communication");
-#else
-		printf("ERROR on accepting reverse communication");
-#endif
+		print_fatal("ERROR on accepting reverse communication");
 	}
 }
-void UrDriver::closeServo() {
+void UrDriver::closeServo(std::vector<double> positions) {
+	if (positions.size() != 6)
+		UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0.008, 0);
+	else
+		UrDriver::servoj(positions, 0.008, 0);
 	close(new_sockfd_);
 }

@@ -296,11 +287,9 @@ bool UrDriver::start() {
 	if (!rt_interface_->start())
 		return false;
 	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_);
-#else
-	printf("Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_);
-#endif
+	char buf[256];
+	sprintf(buf, "Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_);
+	print_debug(buf);
 	return true;

 }
@@ -327,62 +316,37 @@ void UrDriver::setJointNames(std::vector<std::string> jn) {
 void UrDriver::setToolVoltage(unsigned int v) {
 	char buf[256];
 	sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v);
-#ifdef ROS_BUILD
-	ROS_DEBUG("%s", buf);
-#else
-	printf("%s", buf);
-#endif
-
 	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");
-#ifdef ROS_BUILD
-	ROS_DEBUG("%s", buf);
-#else
-	printf("%s", buf);
-#endif
-
 	rt_interface_->addCommandToQueue(buf);
-
+	print_debug(buf);
 }
 void UrDriver::setDigitalOut(unsigned int n, bool b) {
 	char buf[256];
 	sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n,
 			b ? "True" : "False");
-#ifdef ROS_BUILD
-	ROS_DEBUG("%s", buf);
-#else
-	printf("%s", buf);
-#endif
-
 	rt_interface_->addCommandToQueue(buf);
+	print_debug(buf);

 }
 void UrDriver::setAnalogOut(unsigned int n, double f) {
 	char buf[256];
 	sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f);
-#ifdef ROS_BUILD
-	ROS_DEBUG("%s", buf);
-#else
-	printf("%s", buf);
-#endif
-
 	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);
-#ifdef ROS_BUILD
-		ROS_DEBUG("%s", buf);
-#else
-		printf("%s", buf);
-#endif
 		rt_interface_->addCommandToQueue(buf);
+		print_debug(buf);
 		return true;
 	} else
 		return false;
--- a/src/ur_hardware_interface.cpp
+++ b/src/ur_hardware_interface.cpp
@@ -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)
@@ -45,7 +45,7 @@
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************

-  Author: Dave Coleman
+ Author: Dave Coleman
 */

 #include <ur_modern_driver/ur_hardware_interface.h>
@@ -77,6 +77,7 @@ void UrHardwareInterface::init() {
 	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_);

 	// Initialize controller
@@ -90,6 +91,12 @@ void UrHardwareInterface::init() {
 						&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(
@@ -97,15 +104,17 @@ void UrHardwareInterface::init() {
 						&joint_velocity_command_[i]));
 	}

-	// Create joint state interface
+	// 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() {
@@ -131,9 +140,81 @@ void UrHardwareInterface::write() {
 		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);
+	} else if (position_interface_running_) {
+		robot_->servoj(joint_position_command_, 0.008, 1);
 	}
 }

+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) {
@@ -145,6 +226,12 @@ void UrHardwareInterface::doSwitch(
 			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");
+		}
 	}
 	for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
 			stop_list.begin(); controller_it != stop_list.end();
@@ -154,22 +241,14 @@ void UrHardwareInterface::doSwitch(
 			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");
+		}
 	}
-	/*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()); */

 }

--- a/src/ur_realtime_communication.cpp
+++ b/src/ur_realtime_communication.cpp
@@ -18,23 +18,11 @@ UrRealtimeCommunication::UrRealtimeCommunication(
 	bzero((char *) &serv_addr_, sizeof(serv_addr_));
 	sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
 	if (sockfd_ < 0) {
-#ifdef ROS_BUILD
-		ROS_FATAL("ERROR opening socket");
-		ros::shutdown();
-#else
-		printf("ERROR opening socket");
-		exit(1);
-#endif
+		print_fatal("ERROR opening socket");
 	}
 	server_ = gethostbyname(host.c_str());
 	if (server_ == NULL) {
-#ifdef ROS_BUILD
-		ROS_FATAL("ERROR, no such host");
-		ros::shutdown();
-#else
-		printf("ERROR, no such host\n");
-		exit(1);
-#endif
+		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);
@@ -54,11 +42,7 @@ bool UrRealtimeCommunication::start() {
 	struct timeval timeout;

 	keepalive_ = true;
-#ifdef ROS_BUILD
-	ROS_DEBUG("Realtime port: Connecting...");
-#else
-	printf("Realtime port: Connecting...\n");
-#endif
+	print_debug("Realtime port: Connecting...");

 	connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_));
 	FD_ZERO(&writefds);
@@ -69,25 +53,14 @@ bool UrRealtimeCommunication::start() {
 	unsigned int flag_len;
 	getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
 	if (flag_ < 0) {
-#ifdef ROS_BUILD
-		ROS_FATAL("Error connecting to RT port 30003 - errno: %d (%s)", flag_, strerror(flag_));
-
-#else
-		printf("Error connecting to RT port 30003 - errno: %d (%s)\n", flag_,
-				strerror(flag_));
-#endif
+		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) {
-#ifdef ROS_BUILD
-		ROS_FATAL("Could not get local IP - errno: %d (%s)", errno, strerror(errno));
-#else
-		printf("Could not get local IP - errno: %d (%s)", errno,
-				strerror(errno));
-#endif
+		print_fatal("Could not get local IP");
 		close(sockfd_);
 		return false;
 	}
@@ -132,11 +105,7 @@ void UrRealtimeCommunication::run() {
 	fd_set readfds;
 	FD_ZERO(&readfds);
 	FD_SET(sockfd_, &readfds);
-#ifdef ROS_BUILD
-	ROS_DEBUG("Realtime port: Got connection");
-#else
-	printf("Realtime port: Got connection\n");
-#endif
+	print_debug("Realtime port: Got connection");
 	connected_ = true;
 	while (keepalive_) {
 		while (connected_ && keepalive_) {
@@ -159,21 +128,10 @@ void UrRealtimeCommunication::run() {
 		}
 		if (keepalive_) {
 			//reconnect
-#ifdef ROS_BUILD
-			ROS_WARN("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
-#else
-			printf(
-					"Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...\n");
-#endif
+			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) {
-#ifdef ROS_BUILD
-				ROS_FATAL("ERROR opening socket");
-				ros::shutdown();
-#else
-				printf("ERROR opening socket");
-				exit(1);
-#endif
+				print_fatal("ERROR opening socket");
 			}
 			flag_ = 1;
 			setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
@@ -194,12 +152,7 @@ void UrRealtimeCommunication::run() {
 				unsigned int flag_len;
 				getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
 				if (flag_ < 0) {
-#ifdef ROS_BUILD
-					ROS_ERROR("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds...");
-#else
-					printf(
-							"Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds...");
-#endif
+					print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds...");
 				} else {
 					connected_ = true;
 				}
--- a/src/ur_ros_wrapper.cpp
+++ b/src/ur_ros_wrapper.cpp
@@ -11,6 +11,7 @@

 #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>
@@ -71,9 +72,11 @@ public:

 		std::string joint_prefix = "";
 		std::vector<std::string> joint_names;
+		char buf[256];

 		if (ros::param::get("~prefix", joint_prefix)) {
-			ROS_INFO("Setting prefix to %s", joint_prefix.c_str());
+			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");
@@ -96,8 +99,9 @@ public:
 		//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_)) {
-			ROS_DEBUG("Max velocity accepted by ur_driver: %f [rad/s]",
+			sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]",
 					max_velocity_);
+			print_debug(buf);
 		}

 		//Bounds for SetPayload service
@@ -105,21 +109,24 @@ public:
 		double min_payload = 0.;
 		double max_payload = 1.;
 		if (ros::param::get("~min_payload", min_payload)) {
-			ROS_DEBUG("Min payload set to: %f [kg]", min_payload);
+			sprintf(buf, "Min payload set to: %f [kg]", min_payload);
+			print_debug(buf);
 		}
 		if (ros::param::get("~max_payload", max_payload)) {
-			ROS_DEBUG("Max payload set to: %f [kg]", max_payload);
+			sprintf(buf, "Max payload set to: %f [kg]", max_payload);
+			print_debug(buf);
 		}
 		robot_.setMinPayload(min_payload);
 		robot_.setMaxPayload(max_payload);
-		ROS_DEBUG("Bounds for set_payload service calls: [%f, %f]", min_payload,
-				max_payload);
+		sprintf(buf, "Bounds for set_payload service calls: [%f, %f]",
+				min_payload, max_payload);
+		print_debug(buf);

 		if (robot_.start()) {
 			if (use_ros_control_) {
 				ros_control_thread_ = new std::thread(
 						boost::bind(&RosWrapper::rosControlLoop, this));
-				ROS_DEBUG(
+				print_debug(
 						"The control thread for this driver has been started");
 			} else {
 				//register the goal and feedback callbacks
@@ -133,7 +140,7 @@ public:
 				//subscribe to the data topic of interest
 				rt_publish_thread_ = new std::thread(
 						boost::bind(&RosWrapper::publishRTMsg, this));
-				ROS_DEBUG("The action server for this driver has been started");
+				print_debug("The action server for this driver has been started");
 			}
 			mb_publish_thread_ = new std::thread(
 					boost::bind(&RosWrapper::publishMbMsg, this));
@@ -156,7 +163,7 @@ public:
 	}
 private:
 	void goalCB() {
-		ROS_INFO("on_goal");
+		print_info("on_goal");

 		actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction>::GoalConstPtr goal =
 				as_.acceptNewGoal();
@@ -172,21 +179,21 @@ private:
 					"Received a goal with incorrect joint names: "
 							+ outp_joint_names;
 			as_.setAborted(result_, result_.error_string);
-			ROS_ERROR("%s", result_.error_string.c_str());
+			print_error(result_.error_string);
 		}

 		if (!has_velocities()) {
 			result_.error_code = result_.INVALID_GOAL;
 			result_.error_string = "Received a goal without velocities";
 			as_.setAborted(result_, result_.error_string);
-			ROS_ERROR("%s", result_.error_string.c_str());
+			print_error(result_.error_string);
 		}

 		if (!traj_is_finite()) {
 			result_.error_string = "Received a goal with infinites or NaNs";
 			result_.error_code = result_.INVALID_GOAL;
 			as_.setAborted(result_, result_.error_string);
-			ROS_ERROR("%s", result_.error_string.c_str());
+			print_error(result_.error_string);
 		}

 		if (!has_limited_velocities()) {
@@ -194,7 +201,7 @@ private:
 			result_.error_string =
 					"Received a goal with velocities that are higher than %f", max_velocity_;
 			as_.setAborted(result_, result_.error_string);
-			ROS_ERROR("%s", result_.error_string.c_str());
+			print_error(result_.error_string);
 		}

 		reorder_traj_joints(goal_.trajectory);
@@ -213,7 +220,7 @@ private:
 	}

 	void preemptCB() {
-		ROS_INFO("on_cancel");
+		print_info("on_cancel");
 		// set the action state to preempted
 		robot_.stopTraj();
 		as_.setPreempted();
@@ -481,15 +488,15 @@ int main(int argc, char **argv) {
 	ros::init(argc, argv, "ur_driver");
 	ros::NodeHandle nh;
 	if (ros::param::get("use_sim_time", use_sim_time)) {
-		ROS_WARN("use_sim_time is set!!");
+		print_warning("use_sim_time is set!!");
 	}
 	if (!(ros::param::get("~robot_ip_address", host))) {
 		if (argc > 1) {
-			ROS_WARN(
+			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 {
-			ROS_FATAL(
+			print_fatal(
 					"Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip");
 			exit(1);
 		}