Fixed an issue where canceling a trajectory wouldn't stop the robot.

src/ur_driver.cpp

@@ -13,15 +13,17 @@
 
 UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
 		std::condition_variable& msg_cond, std::string host,
-		unsigned int reverse_port, double servoj_time, unsigned int safety_count_max,
+		unsigned int reverse_port, double servoj_time,
-		double max_time_step, double min_payload, double max_payload) :
+		unsigned int safety_count_max, double max_time_step, double min_payload,
+		double max_payload) :
 		REVERSE_PORT_(reverse_port), maximum_time_step_(max_time_step), minimum_payload_(
-				min_payload), maximum_payload_(max_payload), servoj_time_(servoj_time) {
+				min_payload), maximum_payload_(max_payload), servoj_time_(
+				servoj_time) {
 	char buffer[256];
 	struct sockaddr_in serv_addr;
 	int n, flag;
 	//char *ip_addr;
-
+	executing_traj_ = false;
 	rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host,
 			safety_count_max);
 	new_sockfd_ = -1;
@@ -63,77 +65,76 @@ std::vector<double> UrDriver::interp_cubic(double t, double T,
 	}
 	return positions;
 }
- /*
+/*
-void UrDriver::addTraj(std::vector<double> inp_timestamps,
+ void UrDriver::addTraj(std::vector<double> inp_timestamps,
-		std::vector<std::vector<double> > inp_positions,
+ std::vector<std::vector<double> > inp_positions,
-		std::vector<std::vector<double> > inp_velocities) {
+ std::vector<std::vector<double> > inp_velocities) {
-	// DEPRECATED
+ // DEPRECATED
-	printf("!! addTraj is deprecated !!\n");
+ printf("!! addTraj is deprecated !!\n");
-	std::vector<double> timestamps;
+ std::vector<double> timestamps;
-	std::vector<std::vector<double> > positions;
+ std::vector<std::vector<double> > positions;
-	std::string command_string = "def traj():\n";
+ std::string command_string = "def traj():\n";
 
-	for (unsigned int i = 1; i < inp_timestamps.size(); i++) {
+ for (unsigned int i = 1; i < inp_timestamps.size(); i++) {
-		timestamps.push_back(inp_timestamps[i - 1]);
+ timestamps.push_back(inp_timestamps[i - 1]);
-		double dt = inp_timestamps[i] - inp_timestamps[i - 1];
+ double dt = inp_timestamps[i] - inp_timestamps[i - 1];
-		unsigned int steps = (unsigned int) ceil(dt / maximum_time_step_);
+ unsigned int steps = (unsigned int) ceil(dt / maximum_time_step_);
-		double step_size = dt / steps;
+ double step_size = dt / steps;
-		for (unsigned int j = 1; j < steps; j++) {
+ for (unsigned int j = 1; j < steps; j++) {
-			timestamps.push_back(inp_timestamps[i - 1] + step_size * j);
+ timestamps.push_back(inp_timestamps[i - 1] + step_size * j);
-		}
+ }
-	}
+ }
-	// //make sure we come to a smooth stop
+ // //make sure we come to a smooth stop
-	// while (timestamps.back() < inp_timestamps.back()) {
+ // 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;
+ unsigned int j = 0;
-	for (unsigned int i = 0; i < timestamps.size(); i++) {
+ for (unsigned int i = 0; i < timestamps.size(); i++) {
-		while (inp_timestamps[j] <= timestamps[i]) {
+ while (inp_timestamps[j] <= timestamps[i]) {
-			j += 1;
+ j += 1;
-		}
+ }
-		positions.push_back(
+ positions.push_back(
-				UrDriver::interp_cubic(timestamps[i] - inp_timestamps[j - 1],
+ UrDriver::interp_cubic(timestamps[i] - inp_timestamps[j - 1],
-						inp_timestamps[j] - inp_timestamps[j - 1],
+ inp_timestamps[j] - inp_timestamps[j - 1],
-						inp_positions[j - 1], inp_positions[j],
+ inp_positions[j - 1], inp_positions[j],
-						inp_velocities[j - 1], inp_velocities[j]));
+ inp_velocities[j - 1], inp_velocities[j]));
-	}
+ }
 
-	timestamps.push_back(inp_timestamps[inp_timestamps.size() - 1]);
+ timestamps.push_back(inp_timestamps[inp_timestamps.size() - 1]);
-	positions.push_back(inp_positions[inp_positions.size() - 1]);
+ positions.push_back(inp_positions[inp_positions.size() - 1]);
-	/// This is actually faster than using a stringstream :-o
+ /// This is actually faster than using a stringstream :-o
-	for (unsigned int i = 1; i < timestamps.size(); i++) {
+ for (unsigned int i = 1; i < timestamps.size(); i++) {
-		char buf[128];
+ char buf[128];
-		sprintf(buf,
+ sprintf(buf,
-				"\tservoj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], t=%1.5f)\n",
+ "\tservoj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], t=%1.5f)\n",
-				positions[i][0], positions[i][1], positions[i][2],
+ positions[i][0], positions[i][1], positions[i][2],
-				positions[i][3], positions[i][4], positions[i][5],
+ positions[i][3], positions[i][4], positions[i][5],
-				timestamps[i] - timestamps[i - 1]);
+ timestamps[i] - timestamps[i - 1]);
-		command_string += buf;
+ command_string += buf;
-	}
+ }
-	command_string += "end\n";
+ command_string += "end\n";
-	//printf("%s", command_string.c_str());
+ //printf("%s", command_string.c_str());
-	rt_interface_->addCommandToQueue(command_string);
+ rt_interface_->addCommandToQueue(command_string);
 
-}
+ }
-*/
+ */
 void UrDriver::doTraj(std::vector<double> inp_timestamps,
 		std::vector<std::vector<double> > inp_positions,
 		std::vector<std::vector<double> > inp_velocities) {
-
 	std::chrono::high_resolution_clock::time_point t0, t;
 	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()) and executing_traj_) {
+			>= 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) {
@@ -144,19 +145,19 @@ void UrDriver::doTraj(std::vector<double> inp_timestamps,
 						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]);
-
 		UrDriver::servoj(positions);
 
 		// oversample with 4 * sample_time
-		std::this_thread::sleep_for(std::chrono::milliseconds((int) ((servoj_time_*1000)/4.)));
+		std::this_thread::sleep_for(
+				std::chrono::milliseconds((int) ((servoj_time_ * 1000) / 4.)));
 		t = std::chrono::high_resolution_clock::now();
 	}
 	//Signal robot to stop driverProg()
 	UrDriver::closeServo(positions);
 }
 
-void UrDriver::servoj(std::vector<double> positions,
+void UrDriver::servoj(std::vector<double> positions, int keepalive,
-		int keepalive, double time) {
+		double time) {
 	unsigned int bytes_written;
 	int tmp;
 	unsigned char buf[32];

src/ur_ros_wrapper.cpp

@@ -148,6 +148,7 @@ public:
 						"The control thread for this driver has been started");
 			} else {
 				//start actionserver
+				goal_handle_ = NULL;
 				as_.start();
 
 				//subscribe to the data topic of interest
@@ -180,8 +181,13 @@ private:
 			actionlib::ServerGoalHandle<
 					control_msgs::FollowJointTrajectoryAction> gh) {
 		print_info("on_goal");
+		actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
+				*gh.getGoal(); //make a copy that we can modify
+		if (goal_handle_ != NULL) {
+			RosWrapper::cancelCB(*goal_handle_);
+		}
 		goal_handle_ = &gh;
-		actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_->getGoal(); //make a copy that we can modify
+
 		if (!validateJointNames()) {
 			std::string outp_joint_names = "";
 			for (unsigned int i = 0; i < goal.trajectory.joint_names.size();
@@ -195,7 +201,6 @@ private:
 			gh.setRejected(result_, result_.error_string);
 			print_error(result_.error_string);
 		}
-
 		if (!has_positions()) {
 			result_.error_code = result_.INVALID_GOAL;
 			result_.error_string = "Received a goal without positions";
@@ -226,8 +231,15 @@ private:
 		}
 
 		reorder_traj_joints(goal.trajectory);
+
 		std::vector<double> timestamps;
 		std::vector<std::vector<double> > positions, velocities;
+		if (goal.trajectory.points[0].time_from_start.toSec() != 0.) {
+			print_warning("Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory");
+			timestamps.push_back(0.0);
+			positions.push_back(robot_.rt_interface_->robot_state_->getQActual());
+			velocities.push_back(robot_.rt_interface_->robot_state_->getQdActual());
+		}
 		for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
 			timestamps.push_back(
 					goal.trajectory.points[i].time_from_start.toSec());
@@ -235,10 +247,14 @@ private:
 			velocities.push_back(goal.trajectory.points[i].velocities);
 
 		}
-		gh.setAccepted();
+
+		goal_handle_->setAccepted();
 		robot_.doTraj(timestamps, positions, velocities);
-		result_.error_code = result_.SUCCESSFUL;
+		if (goal_handle_ != NULL) {
-		gh.setSucceeded(result_);
+			result_.error_code = result_.SUCCESSFUL;
+			goal_handle_->setSucceeded(result_);
+			goal_handle_ = NULL;
+		}
 	}
 
 	void cancelCB(
@@ -246,9 +262,13 @@ private:
 					control_msgs::FollowJointTrajectoryAction> gh) {
 		print_info("on_cancel");
 		// set the action state to preempted
-		robot_.stopTraj();
+		if (goal_handle_ != NULL) {
-		result_.error_code = result_.SUCCESSFUL;
+			print_info("Stopping previous traj");
-		result_.error_string = "Goal cancelled by client";
+			robot_.stopTraj();
+			result_.error_code = result_.SUCCESSFUL;
+			result_.error_string = "Goal cancelled by client";
+			goal_handle_ = NULL;
+		}
 		gh.setCanceled(result_);
 	}
 
@@ -285,7 +305,8 @@ private:
 
 	bool validateJointNames() {
 		std::vector<std::string> actual_joint_names = robot_.getJointNames();
-		actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_->getGoal();
+		actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
+				*goal_handle_->getGoal();
 		if (goal.trajectory.joint_names.size() != actual_joint_names.size())
 			return false;
 
@@ -336,7 +357,8 @@ private:
 	}
 
 	bool has_velocities() {
-		actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_->getGoal();
+		actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
+				*goal_handle_->getGoal();
 		for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
 			if (goal.trajectory.points[i].positions.size()
 					!= goal.trajectory.points[i].velocities.size())
@@ -346,7 +368,8 @@ private:
 	}
 
 	bool has_positions() {
-		actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_->getGoal();
+		actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
+				*goal_handle_->getGoal();
 		if (goal.trajectory.points.size() == 0)
 			return false;
 		for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
@@ -358,7 +381,8 @@ private:
 	}
 
 	bool has_limited_velocities() {
-		actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_->getGoal();
+		actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
+				*goal_handle_->getGoal();
 		for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
 			for (unsigned int j = 0;
 					j < goal.trajectory.points[i].velocities.size(); j++) {
@@ -371,7 +395,8 @@ private:
 	}
 
 	bool traj_is_finite() {
-		actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_->getGoal();
+		actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
+				*goal_handle_->getGoal();
 		for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
 			for (unsigned int j = 0;
 					j < goal.trajectory.points[i].velocities.size(); j++) {