From 1d54bcca1d4d6fcc5881c1a747b014c38abc041a Mon Sep 17 00:00:00 2001
From: Thomas Timm Andersen <thomas.timm.dk@gmail.com>
Date: Thu, 8 Oct 2015 15:47:39 +0200
Subject: [PATCH] Stop sending data to servoj when stopTraj is called

---
 include/ur_modern_driver/ur_driver.h | 1 +
 src/ur_driver.cpp                    | 7 +++++--
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h
index de74a82..e26f869 100644
--- a/include/ur_modern_driver/ur_driver.h
+++ b/include/ur_modern_driver/ur_driver.h
@@ -40,6 +40,7 @@ private:
 	int incoming_sockfd_;
 	int new_sockfd_;
 	double servoj_time_;
+	bool executing_traj_;
 public:
 	UrRealtimeCommunication* rt_interface_;
 	UrCommunication* sec_interface_;
diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp
index c141ec3..6b72008 100644
--- a/src/ur_driver.cpp
+++ b/src/ur_driver.cpp
@@ -126,13 +126,14 @@ void UrDriver::doTraj(std::vector<double> inp_timestamps,
 	std::vector<double> positions;
 	unsigned int j;
 
+	executing_traj_ = true;
 	UrDriver::uploadProg();
 
 	t0 = std::chrono::high_resolution_clock::now();
 	t = t0;
 	j = 0;
-	while (inp_timestamps[inp_timestamps.size() - 1]
-			>= std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count()) {
+	while ((inp_timestamps[inp_timestamps.size() - 1]
+			>= std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count()) and executing_traj_) {
 		while (inp_timestamps[j]
 				<= std::chrono::duration_cast<std::chrono::duration<double>>(
 						t - t0).count() && j < inp_timestamps.size() - 1) {
@@ -151,6 +152,7 @@ void UrDriver::doTraj(std::vector<double> inp_timestamps,
 		t = std::chrono::high_resolution_clock::now();
 	}
 	//Signal robot to stop driverProg()
+	executing_traj_ = false;
 	UrDriver::closeServo(positions);
 }
 
@@ -183,6 +185,7 @@ void UrDriver::servoj(std::vector<double> positions,
 }
 
 void UrDriver::stopTraj() {
+	executing_traj_ = false;
 	rt_interface_->addCommandToQueue("stopj(10)\n");
 }