Sanity check of actionserver goal input

include/ur_modern_driver/ur_driver.h

@@ -21,7 +21,6 @@
 class UrDriver {
 private:
 	double maximum_time_step_;
-	double maximum_velocity_;
 	double minimum_payload_;
 	double maximum_payload_;
 	std::vector<std::string> joint_names_;
@@ -29,7 +28,7 @@ public:
 	UrRealtimeCommunication* rt_interface_;
 
 	UrDriver(std::condition_variable& msg_cond, std::string host,
-			unsigned int safety_count_max = 12, double max_time_step = 0.08, double max_vel = 10., double min_payload= 0., double max_payload=1.);
+			unsigned int safety_count_max = 12, double max_time_step = 0.08, double min_payload= 0., double max_payload=1.);
 	void start();
 	void halt();
 
@@ -52,7 +51,6 @@ public:
 	void setAnalogOut(unsigned int n, double f);
 	bool setPayload(double m);
 
-	void setMaxVel(double vel);
 	void setMinPayload(double m);
 	void setMaxPayload(double m);
 

src/ur_driver.cpp

@@ -12,10 +12,10 @@
 #include "ur_modern_driver/ur_driver.h"
 
 UrDriver::UrDriver(std::condition_variable& msg_cond, std::string host,
-		unsigned int safety_count_max, double max_time_step, double max_vel,
+		unsigned int safety_count_max, double max_time_step, double min_payload,
-		double min_payload, double max_payload) :
+		double max_payload) :
-		maximum_time_step_(max_time_step), maximum_velocity_(max_vel), minimum_payload_(
+		maximum_time_step_(max_time_step), minimum_payload_(min_payload), maximum_payload_(
-				min_payload), maximum_payload_(max_payload) {
+				max_payload) {
 	rt_interface_ = new UrRealtimeCommunication(msg_cond, host,
 			safety_count_max);
 
@@ -155,9 +155,6 @@ bool UrDriver::setPayload(double m) {
 		return false;
 }
 
-void UrDriver::setMaxVel(double vel) {
-	maximum_velocity_ = vel;
-}
 void UrDriver::setMinPayload(double m) {
 	minimum_payload_ = m;
 }

src/ur_ros_wrapper.cpp

@@ -9,8 +9,6 @@
  * ----------------------------------------------------------------------------
  */
 
-#include "ros/ros.h"
-#include <ros/console.h>
 #include "ur_modern_driver/ur_driver.h"
 #include <string.h>
 #include <vector>
@@ -18,7 +16,10 @@
 #include <condition_variable>
 #include <thread>
 #include <algorithm>
+#include <cmath>
 
+#include "ros/ros.h"
+#include <ros/console.h>
 #include "sensor_msgs/JointState.h"
 #include "geometry_msgs/WrenchStamped.h"
 #include "control_msgs/FollowJointTrajectoryAction.h"
@@ -45,6 +46,7 @@ protected:
 	ros::ServiceServer payloadSrv_;
 	std::thread* rt_publish_thread_;
 	double io_flag_delay_;
+	double max_velocity_;
 
 public:
 	RosWrapper(std::string host) :
@@ -65,13 +67,11 @@ public:
 		joint_names.push_back(joint_prefix + "wrist_3_joint");
 		robot_.setJointNames(joint_names);
 
-		{ //Using a very high value in order to not limit execution of trajectories being sent from MoveIt!
+		//Using a very high value in order to not limit execution of trajectories being sent from MoveIt!
-			double max_velocity = 10.;
+		max_velocity_ = 10.;
-			if (ros::param::get("~max_velocity", max_velocity)) {
+		if (ros::param::get("~max_velocity", max_velocity_)) {
-				ROS_DEBUG(
+			ROS_DEBUG("Max velocity accepted by ur_driver: %f [rad/s]",
-						"Max velocity accepted by ur_driver: %f [rad/s]", max_velocity);
+					max_velocity_);
-				robot_.setMaxVel(max_velocity);
-			}
 		}
 
 		{ //Bounds for SetPayload service
@@ -79,16 +79,17 @@ public:
 			double min_payload = 0.;
 			double max_payload = 1.;
 			if (ros::param::get("~min_payload", min_payload)) {
-				ROS_DEBUG(
+				ROS_DEBUG("Min payload accepted by ur_driver: %f [kg]",
-						"Min payload accepted by ur_driver: %f [kg]", min_payload);
+						min_payload);
 			}
 			if (ros::param::get("~max_payload", max_payload)) {
-				ROS_DEBUG(
+				ROS_DEBUG("Max payload accepted by ur_driver: %f [kg]",
-						"Max payload accepted by ur_driver: %f [kg]", max_payload);
+						max_payload);
 			}
 			robot_.setMinPayload(min_payload);
 			robot_.setMaxPayload(max_payload);
-			ROS_INFO("Bounds for set_payload service calls: [%f, %f]", min_payload, max_payload);
+			ROS_INFO("Bounds for set_payload service calls: [%f, %f]",
+					min_payload, max_payload);
 		}
 		robot_.start();
 
@@ -131,13 +132,34 @@ private:
 					i++) {
 				outp_joint_names += goal_.trajectory.joint_names[i] + " ";
 			}
-			ROS_ERROR("Received a goal with incorrect joint names: %s",
-					outp_joint_names.c_str());
 			result_.error_code = result_.INVALID_JOINTS;
-			/*result_.error_string =
+			result_.error_string =
-			 "Received a goal with incorrect joint names: %s", outp_joint_names.c_str(); */
+					"Received a goal with incorrect joint names: "
-			as_.setAborted(result_,
+							+ outp_joint_names;
-					("Received a goal with incorrect joint names: %s", outp_joint_names.c_str()));
+			as_.setAborted(result_, result_.error_string);
+			ROS_ERROR(result_.error_string.c_str());
+		}
+
+		if (!has_velocities()) {
+			result_.error_code = result_.INVALID_GOAL;
+			result_.error_string = "Received a goal without velocities";
+			as_.setAborted(result_, result_.error_string);
+			ROS_ERROR(result_.error_string.c_str());
+		}
+
+		if (!traj_is_finite()) {
+			result_.error_string = "Received a goal with infinites or NaNs";
+			result_.error_code = result_.INVALID_GOAL;
+			as_.setAborted(result_, result_.error_string);
+			ROS_ERROR(result_.error_string.c_str());
+		}
+
+		if (!has_limited_velocities()) {
+			result_.error_code = result_.INVALID_GOAL;
+			result_.error_string =
+					"Received a goal with velocities that are higher than %f", max_velocity_;
+			as_.setAborted(result_, result_.error_string);
+			ROS_ERROR(result_.error_string.c_str());
 		}
 
 		reorder_traj_joints(goal_.trajectory);
@@ -246,6 +268,40 @@ private:
 		traj.points = new_traj;
 	}
 
+	bool has_velocities() {
+		for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) {
+			if (goal_.trajectory.points[i].positions.size()
+					!= goal_.trajectory.points[i].velocities.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;
+					j < goal_.trajectory.points[i].velocities.size(); j++) {
+				if (fabs(goal_.trajectory.points[i].velocities[j])
+						> max_velocity_)
+					return false;
+			}
+		}
+		return true;
+	}
+
+	bool traj_is_finite() {
+		for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) {
+			for (unsigned int j = 0;
+					j < goal_.trajectory.points[i].velocities.size(); j++) {
+				if (!std::isfinite(goal_.trajectory.points[i].positions[j]))
+					return false;
+				if (!std::isfinite(goal_.trajectory.points[i].velocities[j]))
+					return false;
+			}
+		}
+		return true;
+	}
+
 	void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg) {
 		reorder_traj_joints(*msg);
 		double acc = *std::max_element(msg->points[0].accelerations.begin(),