From 0e6fe245b9a7c97206250654945bca3470c37f74 Mon Sep 17 00:00:00 2001
From: Thomas Timm Andersen <thomas.timm.dk@gmail.com>
Date: Thu, 10 Sep 2015 14:17:15 +0200
Subject: [PATCH] Added boundary check for setting payload

---
 include/ur_modern_driver/ur_driver.h |  2 +-
 src/ur_driver.cpp                    | 21 +++++++++++++--------
 src/ur_ros_wrapper.cpp               | 14 ++++++++------
 3 files changed, 22 insertions(+), 15 deletions(-)

diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h
index aefb34f..aa3c196 100644
--- a/include/ur_modern_driver/ur_driver.h
+++ b/include/ur_modern_driver/ur_driver.h
@@ -50,7 +50,7 @@ public:
 	void setFlag(unsigned int n, bool b);
 	void setDigitalOut(unsigned int n, bool b);
 	void setAnalogOut(unsigned int n, double f);
-	void setPayload(double m);
+	bool setPayload(double m);
 
 	void setMaxVel(double vel);
 	void setMinPayload(double m);
diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp
index fdb4aa0..b2056ea 100644
--- a/src/ur_driver.cpp
+++ b/src/ur_driver.cpp
@@ -12,8 +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, double min_payload, double max_payload ) :
-		maximum_time_step_(max_time_step), maximum_velocity_(max_vel), minimum_payload_(min_payload), maximum_payload_(max_payload) {
+		unsigned int safety_count_max, double max_time_step, double max_vel,
+		double min_payload, double max_payload) :
+		maximum_time_step_(max_time_step), maximum_velocity_(max_vel), minimum_payload_(
+				min_payload), maximum_payload_(max_payload) {
 	rt_interface_ = new UrRealtimeCommunication(msg_cond, host,
 			safety_count_max);
 
@@ -142,12 +144,15 @@ void UrDriver::setAnalogOut(unsigned int n, double f) {
 	rt_interface_->addCommandToQueue(buf);
 }
 
-void UrDriver::setPayload(double m) {
-	char buf[256];
-	sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m);
-	printf("%s", buf);
-	rt_interface_->addCommandToQueue(buf);
-
+bool UrDriver::setPayload(double m) {
+	if ((m < maximum_payload_) && (m > minimum_payload_)) {
+		char buf[256];
+		sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m);
+		printf("%s", buf);
+		rt_interface_->addCommandToQueue(buf);
+		return true;
+	} else
+		return false;
 }
 
 void UrDriver::setMaxVel(double vel) {
diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp
index fff16cc..905edef 100644
--- a/src/ur_ros_wrapper.cpp
+++ b/src/ur_ros_wrapper.cpp
@@ -68,7 +68,7 @@ public:
 		{ //Using a very high value in order to not limit execution of trajectories being sent from MoveIt!
 			double max_velocity = 10.;
 			if (ros::param::get("~max_velocity", max_velocity)) {
-				ROS_INFO(
+				ROS_DEBUG(
 						"Max velocity accepted by ur_driver: %f [rad/s]", max_velocity);
 				robot_.setMaxVel(max_velocity);
 			}
@@ -79,11 +79,11 @@ public:
 			double min_payload = 0.;
 			double max_payload = 1.;
 			if (ros::param::get("~min_payload", min_payload)) {
-				ROS_INFO(
+				ROS_DEBUG(
 						"Min payload accepted by ur_driver: %f [kg]", min_payload);
 			}
 			if (ros::param::get("~max_payload", max_payload)) {
-				ROS_INFO(
+				ROS_DEBUG(
 						"Max payload accepted by ur_driver: %f [kg]", max_payload);
 			}
 			robot_.setMinPayload(min_payload);
@@ -189,9 +189,11 @@ private:
 
 	bool setPayload(ur_msgs::SetPayloadRequest& req,
 			ur_msgs::SetPayloadResponse& resp) {
-		robot_.setPayload(req.payload);
-		resp.success = true;
-		return true;
+		if (robot_.setPayload(req.payload))
+			resp.success = true;
+		else
+			resp.success = true;
+		return resp.success;
 	}
 
 	bool validateJointNames() {