Added IO and payload service

include/ur_modern_driver/ur_driver.h

@@ -26,7 +26,6 @@ public:
 	UrRealtimeCommunication* rt_interface_;
 
 	UrDriver(std::condition_variable& msg_cond, std::string host,
-			std::vector<std::string> joint_names,
 			unsigned int safety_count_max = 12);
 	void start();
 	void halt();
@@ -44,6 +43,12 @@ public:
 
 	std::vector<std::string> getJointNames();
 	void setJointNames(std::vector<std::string> jn);
+	void setToolVoltage(unsigned int v);
+	void setFlag(unsigned int n, bool b);
+	void setDigitalOut(unsigned int n, bool b);
+	void setAnalogOut(unsigned int n, double f);
+	void setPayloaf(double m);
+
 };
 
 #endif /* UR_DRIVER_H_ */

src/ur_driver.cpp

@@ -12,11 +12,10 @@
 #include "ur_modern_driver/ur_driver.h"
 
 UrDriver::UrDriver(std::condition_variable& msg_cond, std::string host,
-		std::vector<std::string> joint_names, unsigned int safety_count_max) {
+		unsigned int safety_count_max) {
 	rt_interface_ = new UrRealtimeCommunication(msg_cond, host,
 			safety_count_max);
 	maximum_time_step_ = 0.08;
-	joint_names_ = joint_names;
 
 }
 
@@ -66,7 +65,8 @@ void UrDriver::addTraj(std::vector<double> inp_timestamps,
 			j += 1;
 		}
 		positions.push_back(
-				UrDriver::interp_cubic(timestamps[i] - inp_timestamps[j-1], inp_timestamps[j] - inp_timestamps[j-1],
+				UrDriver::interp_cubic(timestamps[i] - 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]));
 	}
@@ -112,3 +112,40 @@ std::vector<std::string> UrDriver::getJointNames() {
 void UrDriver::setJointNames(std::vector<std::string> jn) {
 	joint_names_ = jn;
 }
+
+void UrDriver::setToolVoltage(unsigned int v) {
+	char buf[256];
+	sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v);
+	printf("%s", buf);
+	rt_interface_->addCommandToQueue(buf);
+}
+void UrDriver::setFlag(unsigned int n, bool b) {
+	char buf[256];
+	sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n,
+			b ? "True" : "False");
+	printf("%s", buf);
+	rt_interface_->addCommandToQueue(buf);
+
+}
+void UrDriver::setDigitalOut(unsigned int n, bool b) {
+	char buf[256];
+	sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n,
+			b ? "True" : "False");
+	printf("%s", buf);
+	rt_interface_->addCommandToQueue(buf);
+
+}
+void UrDriver::setAnalogOut(unsigned int n, double f) {
+	char buf[256];
+	sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d, %1.4f)\nend\n", n, f);
+	printf("%s", buf);
+	rt_interface_->addCommandToQueue(buf);
+}
+
+void UrDriver::setPayloaf(double m){
+	char buf[256];
+	sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m);
+	printf("%s", buf);
+	rt_interface_->addCommandToQueue(buf);
+
+}

src/ur_ros_wrapper.cpp

@@ -24,6 +24,11 @@
 #include "control_msgs/FollowJointTrajectoryAction.h"
 #include "actionlib/server/simple_action_server.h"
 #include "trajectory_msgs/JointTrajectoryPoint.h"
+#include "ur_msgs/SetIO.h"
+#include "ur_msgs/SetPayloadRequest.h"
+#include "ur_msgs/SetPayloadResponse.h"
+#include "ur_msgs/SetIORequest.h"
+#include "ur_msgs/SetIOResponse.h"
 
 class RosWrapper {
 protected:
@@ -37,15 +42,29 @@ protected:
 	std::thread* rt_publish_thread_;
 	std::condition_variable msg_cond_;
 public:
-	RosWrapper(std::string host, std::vector<std::string> joint_names) :
+	RosWrapper(std::string host) :
-			as_(nh_, "follow_joint_trajectory", false), robot_(msg_cond_, host,
+			as_(nh_, "follow_joint_trajectory", false), robot_(msg_cond_, host) {
-					joint_names) {
+
+		std::string joint_prefix = "";
+		std::vector<std::string> joint_names;
+
+		if (ros::param::get("~prefix", joint_prefix)) {
+			ROS_INFO("Setting prefix to %s", joint_prefix.c_str());
+		}
+		joint_names.push_back(joint_prefix + "shoulder_pan_joint");
+		joint_names.push_back(joint_prefix + "shoulder_lift_joint");
+		joint_names.push_back(joint_prefix + "elbow_joint");
+		joint_names.push_back(joint_prefix + "wrist_1_joint");
+		joint_names.push_back(joint_prefix + "wrist_2_joint");
+		joint_names.push_back(joint_prefix + "wrist_3_joint");
+		robot_.setJointNames(joint_names);
+
+		robot_.start();
 
 		//register the goal and feedback callbacks
 		as_.registerGoalCallback(boost::bind(&RosWrapper::goalCB, this));
 		as_.registerPreemptCallback(boost::bind(&RosWrapper::preemptCB, this));
 
-		robot_.start();
 		as_.start();
 
 		//subscribe to the data topic of interest
@@ -58,6 +77,12 @@ public:
 
 	}
 
+	void halt() {
+		robot_.halt();
+		rt_publish_thread_->join();
+
+	}
+private:
 	void goalCB() {
 		ROS_INFO("on_goal");
 
@@ -104,12 +129,29 @@ public:
 		as_.setPreempted();
 	}
 
-	void halt() {
+	bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp) {
-		robot_.halt();
+		resp.success = true;
-		rt_publish_thread_->join();
+		if (req.fun == ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT) {
-
+			robot_.setDigitalOut(req.pin, req.state > 0.0 ? true : false);
+		} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_FLAG) {
+			robot_.setFlag(req.pin, req.state > 0.0 ? true : false);
+		} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT) {
+			robot_.setAnalogOut(req.pin, req.state);
+		} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE) {
+			robot_.setToolVoltage((int) req.state);
+		} else {
+			resp.success = false;
 		}
-private:
+		return resp.success;
+	}
+
+	bool setPayloaf(ur_msgs::SetPayloadRequest& req,
+			ur_msgs::SetPayloadResponse& resp) {
+		robot_.setPayloaf(req.payload);
+		resp.success = true;
+		return true;
+	}
+
 	bool validateJointNames() {
 		std::vector<std::string> actual_joint_names = robot_.getJointNames();
 		if (goal_.trajectory.joint_names.size() != actual_joint_names.size())
@@ -226,20 +268,8 @@ int main(int argc, char **argv) {
 				"Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip");
 		exit(1);
 	}
-	std::string joint_prefix = "";
-	std::vector<std::string> joint_names;
 
-	if (ros::param::get("~prefix", joint_prefix)) {
+	RosWrapper interface(host);
-		ROS_INFO("Setting prefix to %s", joint_prefix.c_str());
-	}
-	joint_names.push_back(joint_prefix + "shoulder_pan_joint");
-	joint_names.push_back(joint_prefix + "shoulder_lift_joint");
-	joint_names.push_back(joint_prefix + "elbow_joint");
-	joint_names.push_back(joint_prefix + "wrist_1_joint");
-	joint_names.push_back(joint_prefix + "wrist_2_joint");
-	joint_names.push_back(joint_prefix + "wrist_3_joint");
-
-	RosWrapper interface(host, joint_names);
 
 	ros::spin();
 	interface.halt();