diff --git a/include/ur_modern_driver/robot_state.h b/include/ur_modern_driver/robot_state.h
index e85cf23..2a2b077 100644
--- a/include/ur_modern_driver/robot_state.h
+++ b/include/ur_modern_driver/robot_state.h
@@ -133,7 +133,7 @@ public:
 	bool getNewDataAvailable();
 	void finishedReading();
 	std::vector<double> getVActual();
-	int unpack(uint8_t * buf, unsigned int buf_length);
+	void unpack(uint8_t * buf, unsigned int buf_length);
 	void unpackRobotMessage(uint8_t * buf, unsigned int offset, uint32_t len);
 	void unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
 			uint32_t len);
diff --git a/include/ur_modern_driver/ur_communication.h b/include/ur_modern_driver/ur_communication.h
index 44d9b54..c9bc361 100644
--- a/include/ur_modern_driver/ur_communication.h
+++ b/include/ur_modern_driver/ur_communication.h
@@ -42,8 +42,6 @@ private:
 	bool keepalive_;
 	std::thread comThread_;
 	int flag_;
-	std::recursive_mutex command_string_lock_;
-	std::string command_;
 	void run();
 
 public:
@@ -53,7 +51,6 @@ public:
 	UrCommunication(std::condition_variable& msg_cond, std::string host);
 	void start();
 	void halt();
-	void addCommandToQueue(std::string inp);
 
 };
 
diff --git a/src/robot_state.cpp b/src/robot_state.cpp
index a5677e8..d56f7ca 100644
--- a/src/robot_state.cpp
+++ b/src/robot_state.cpp
@@ -19,7 +19,7 @@ double RobotState::ntohd(uint64_t nf) {
 	memcpy(&x, &nf, sizeof(x));
 	return x;
 }
-int RobotState::unpack(uint8_t* buf, unsigned int buf_length) {
+void RobotState::unpack(uint8_t* buf, unsigned int buf_length) {
 	/* Returns missing bytes to unpack a message, or 0 if all data was parsed */
 	unsigned int offset = 0;
 	while (buf_length > offset) {
@@ -28,7 +28,7 @@ int RobotState::unpack(uint8_t* buf, unsigned int buf_length) {
 		memcpy(&len, &buf[offset], sizeof(len));
 		len = ntohl(len);
 		if (len + offset > buf_length) {
-			return (len + offset - buf_length);
+			return ;
 		}
 		memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type));
 		switch (message_type) {
@@ -46,7 +46,7 @@ int RobotState::unpack(uint8_t* buf, unsigned int buf_length) {
 		offset += len;
 
 	}
-	return 0;
+	return;
 }
 
 void RobotState::unpackRobotMessage(uint8_t * buf, unsigned int offset,
diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp
index a9ba0fb..2ceacd0 100644
--- a/src/ur_communication.cpp
+++ b/src/ur_communication.cpp
@@ -133,15 +133,6 @@ void UrCommunication::halt() {
 	comThread_.join();
 }
 
-void UrCommunication::addCommandToQueue(std::string inp) {
-	if (inp.back() != '\n') {
-		inp.append("\n");
-	}
-	command_string_lock_.lock();
-	command_ += inp;
-	command_string_lock_.unlock();
-}
-
 void UrCommunication::run() {
 	uint8_t buf[2048];
 	unsigned int bytes_read;
@@ -152,13 +143,6 @@ void UrCommunication::run() {
 		setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
 				sizeof(int));
 		robot_state_->unpack(buf, bytes_read);
-		command_string_lock_.lock();
-		if (command_.length() != 0) {
-			write(sec_sockfd_, command_.c_str(), command_.length());
-			command_ = "";
-		}
-		command_string_lock_.unlock();
-
 	}
 	//wait for some traffic so the UR socket doesn't die in version 3.1.
 	std::this_thread::sleep_for(std::chrono::milliseconds(500));
diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp
index ec01ada..3f9665d 100644
--- a/src/ur_realtime_communication.cpp
+++ b/src/ur_realtime_communication.cpp
@@ -71,14 +71,11 @@ void UrRealtimeCommunication::halt() {
 }
 
 void UrRealtimeCommunication::addCommandToQueue(std::string inp) {
+	int bytes_written;
 	if (inp.back() != '\n') {
 		inp.append("\n");
 	}
-	write(sockfd_, inp.c_str(), inp.length());
-	/*command_string_lock_.lock();
-	command_ += inp;
-	command_string_lock_.unlock();*/
-
+	bytes_written = write(sockfd_, inp.c_str(), inp.length());
 }
 
 void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
@@ -96,6 +93,7 @@ void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
 
 void UrRealtimeCommunication::run() {
 	uint8_t buf[2048];
+	int bytes_read;
 	bzero(buf, 2048);
 #ifdef ROS_BUILD
 	ROS_DEBUG("Realtime port: Got connection");
@@ -104,26 +102,16 @@ void UrRealtimeCommunication::run() {
 #endif
 	connected_ = true;
 	while (keepalive_) {
-		read(sockfd_, buf, 2048);
+		bytes_read = 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();
-
 	}
 	setSpeed(0., 0., 0., 0., 0., 0.);
-	write(sockfd_, command_.c_str(), command_.length());
 	close(sockfd_);
 }