From f592d2189adf37a145ad957ca5a26edec4939920 Mon Sep 17 00:00:00 2001
From: Thomas Timm Andersen <thomas.timm.dk@gmail.com>
Date: Thu, 8 Oct 2015 13:44:24 +0200
Subject: [PATCH] Added a check to incoming goals to see if there are any
 points in the trajectory

---
 src/ur_ros_wrapper.cpp | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp
index 2035161..c782bde 100644
--- a/src/ur_ros_wrapper.cpp
+++ b/src/ur_ros_wrapper.cpp
@@ -190,6 +190,13 @@ private:
 			print_error(result_.error_string);
 		}
 
+		if (!has_positions()) {
+			result_.error_code = result_.INVALID_GOAL;
+			result_.error_string = "Received a goal without positions";
+			as_.setAborted(result_, result_.error_string);
+			print_error(result_.error_string);
+		}
+
 		if (!has_velocities()) {
 			result_.error_code = result_.INVALID_GOAL;
 			result_.error_string = "Received a goal without velocities";
@@ -325,6 +332,17 @@ private:
 		return true;
 	}
 
+	bool has_positions() {
+		if (goal_.trajectory.points.size() == 0)
+			return false;
+		for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) {
+			if (goal_.trajectory.points[i].positions.size()
+					!= goal_.trajectory.joint_names.size())
+				return false;
+		}
+		return true;
+	}
+
 	bool has_limited_velocities() {
 		for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) {
 			for (unsigned int j = 0;