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,
-		double max_time_step, double min_payload, double max_payload) :
+		unsigned int reverse_port, double servoj_time,
+		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,8 +65,8 @@ 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_velocities) {
  // DEPRECATED
@@ -116,24 +118,23 @@ void UrDriver::addTraj(std::vector<double> inp_timestamps,
  //printf("%s", command_string.c_str());
  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,
-		int keepalive, double time) {
+void UrDriver::servoj(std::vector<double> positions, int keepalive,
+		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);
+		if (goal_handle_ != NULL) {
 			result_.error_code = result_.SUCCESSFUL;
-		gh.setSucceeded(result_);
+			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
+		if (goal_handle_ != NULL) {
+			print_info("Stopping previous traj");
 			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++) {