Service server for unlocking the protective stop

include/ur_modern_driver/ur_driver.h

@@ -52,6 +52,7 @@ class UrDriver {
 	double firmware_version_;
 	double servoj_lookahead_time_;
 	double servoj_gain_;
+	std::string host_;
 public:
 	UrRealtimeCommunication* rt_interface_;
 	UrCommunication* sec_interface_;
@@ -96,6 +97,7 @@ class UrDriver {
 	void setServojLookahead(double t);
 	void setServojGain(double g);
 
+	bool unlockProtectiveStop();
 };
 
 #endif /* UR_DRIVER_H_ */

src/ur_driver.cpp

@@ -25,7 +25,8 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
 		double max_payload, double servoj_lookahead_time, double servoj_gain) :
 		REVERSE_PORT_(reverse_port), maximum_time_step_(max_time_step), minimum_payload_(
 				min_payload), maximum_payload_(max_payload), servoj_time_(
-				servoj_time), servoj_lookahead_time_(servoj_lookahead_time), servoj_gain_(servoj_gain) {
+				servoj_time), servoj_lookahead_time_(servoj_lookahead_time), servoj_gain_(servoj_gain),
+		host_(host) {
 	char buffer[256];
 	struct sockaddr_in serv_addr;
 	int n, flag;
@@ -380,3 +381,49 @@ void UrDriver::setServojGain(double g){
 			servoj_gain_ = 100;
 		}
 }
+
+bool UrDriver::unlockProtectiveStop() {
+	addrinfo info;
+
+	memset(&info, 0, sizeof(info));
+	info.ai_family = AF_UNSPEC;
+	info.ai_socktype = SOCK_STREAM;
+
+	addrinfo* res = 0;
+
+	if(getaddrinfo(host_.c_str(), "29999", &info, &res) != 0 || !res)
+	{
+		perror("Could not get address for UR host");
+		return false;
+	}
+
+	sockaddr_storage addr;
+	socklen_t addrlen = res->ai_addrlen;
+	memcpy(&addr, res->ai_addr, res->ai_addrlen);
+
+	int fd = socket(res->ai_family, res->ai_socktype, res->ai_protocol);
+
+	freeaddrinfo(res);
+
+	if(fd < 0)
+	{
+		perror("socket");
+		return false;
+	}
+
+	if(connect(fd, (const sockaddr*)&addr, addrlen) != 0)
+	{
+		perror("Could not connect to UR dashboard");
+		return false;
+	}
+
+	const char* cmd = "unlock protective stop\n";
+	if(write(fd, cmd, strlen(cmd)) != strlen(cmd))
+	{
+		perror("Could not write to UR dashboard");
+		return false;
+	}
+
+	close(fd);
+	return true;
+}

src/ur_ros_wrapper.cpp

@@ -51,6 +51,8 @@
 #include <controller_manager/controller_manager.h>
 #include <realtime_tools/realtime_publisher.h>
 
+#include <std_srvs/Empty.h>
+
 /// TF
 #include <tf/tf.h>
 #include <tf/transform_broadcaster.h>
@@ -70,6 +72,7 @@ class RosWrapper {
 	ros::Subscriber urscript_sub_;
 	ros::ServiceServer io_srv_;
 	ros::ServiceServer payload_srv_;
+	ros::ServiceServer unlockProtStop_srv_;
 	std::thread* rt_publish_thread_;
 	std::thread* mb_publish_thread_;
 	double io_flag_delay_;
@@ -213,6 +216,8 @@ class RosWrapper {
 					&RosWrapper::setIO, this);
 			payload_srv_ = nh_.advertiseService("ur_driver/set_payload",
 					&RosWrapper::setPayload, this);
+			unlockProtStop_srv_ = nh_.advertiseService("ur_driver/unlock_protective_stop",
+					&RosWrapper::unlockProtectiveStop, this);
 		}
 	}
 
@@ -424,6 +429,11 @@ class RosWrapper {
 		return resp.success;
 	}
 
+	bool unlockProtectiveStop(std_srvs::EmptyRequest&, std_srvs::EmptyResponse&)
+	{
+		return robot_.unlockProtectiveStop();
+	}
+
 	bool validateJointNames() {
 		std::vector<std::string> actual_joint_names = robot_.getJointNames();
 		actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
@@ -781,7 +791,12 @@ class RosWrapper {
 					print_error("Aborting trajectory");
 					robot_.stopTraj();
 					result_.error_code = result_.SUCCESSFUL;
-					result_.error_string = "Robot was halted";
+
+					if(robot_.sec_interface_->robot_state_->isEmergencyStopped())
+						result_.error_string = "EMERGENCY STOP";
+					else
+						result_.error_string = "PROTECTIVE STOP";
+
 					goal_handle_.setAborted(result_, result_.error_string);
 					has_goal_ = false;
 				}