Added trajectory and actionlib interface

include/ur_modern_driver/ur_driver.h

@@ -15,20 +15,35 @@
 #include <mutex>
 #include <condition_variable>
 #include "ur_realtime_communication.h"
+#include <vector>
+#include <math.h>
 
 class UrDriver {
 private:
 	double maximum_time_step_;
+	std::vector<std::string> joint_names_;
 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();
+
 	void setSpeed(double q0, double q1, double q2, double q3, double q4,
 			double q5, double acc = 100.);
+	void addTraj(std::vector<double> inp_timestamps,
+			std::vector<std::vector<double> > positions,
+			std::vector<std::vector<double> > velocities);
+	void stopTraj();
 
+	std::vector<double> interp_cubic(double t, double T,
+			std::vector<double> p0_pos, std::vector<double> p1_pos,
+			std::vector<double> p0_vel, std::vector<double> p1_vel);
+
+	std::vector<std::string> getJointNames();
+	void setJointNames(std::vector<std::string> jn);
 };
 
 #endif /* UR_DRIVER_H_ */

src/ur_driver.cpp

@@ -12,12 +12,86 @@
 #include "ur_modern_driver/ur_driver.h"
 
 UrDriver::UrDriver(std::condition_variable& msg_cond, std::string host,
-		unsigned int safety_count_max) {
+		std::vector<std::string> joint_names, unsigned int safety_count_max) {
 	rt_interface_ = new UrRealtimeCommunication(msg_cond, host,
 			safety_count_max);
+	maximum_time_step_ = 2.;
+	joint_names_ = joint_names;
 
 }
 
+std::vector<double> UrDriver::interp_cubic(double t, double T,
+		std::vector<double> p0_pos, std::vector<double> p1_pos,
+		std::vector<double> p0_vel, std::vector<double> p1_vel) {
+	/*Returns positions of the joints at time 't' */
+	std::vector<double> positions;
+	for (unsigned int i = 0; i < p0_pos.size(); i++) {
+		double a = p0_pos[i];
+		double b = p0_vel[i];
+		double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i]
+				- T * p1_vel[i]) / pow(T, 2);
+		double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i]
+				+ T * p1_vel[i]) / pow(T, 3);
+		positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3));
+	}
+	return positions;
+}
+
+void UrDriver::addTraj(std::vector<double> inp_timestamps,
+		std::vector<std::vector<double> > inp_positions,
+		std::vector<std::vector<double> > inp_velocities) {
+
+	std::vector<double> timestamps;
+	std::vector<std::vector<double> > positions;
+	std::string command_string = "def traj():\n";
+
+	for (unsigned int i = 1; i < inp_timestamps.size(); i++) {
+		timestamps.push_back(inp_timestamps[i - 1]);
+		double dt = inp_timestamps[i] - inp_timestamps[i - 1];
+		unsigned int steps = (unsigned int) ceil(dt / maximum_time_step_);
+		double step_size = dt / steps;
+		for (unsigned int j = 1; j < steps; j++) {
+			timestamps.push_back(inp_timestamps[i - 1] + step_size * j);
+		}
+	}
+	//make sure we come to a smooth stop
+	/*while (timestamps.back() < inp_timestamps.back()) {
+	 timestamps.push_back(timestamps.back() + 0.008);
+	 }
+	 timestamps.pop_back();
+	 */
+	unsigned int j = 0;
+	for (unsigned int i = 0; i < timestamps.size(); i++) {
+		while (inp_timestamps[j] <= timestamps[i]) {
+			j += 1;
+		}
+		positions.push_back(
+				UrDriver::interp_cubic(timestamps[i], inp_timestamps[j],
+						inp_positions[j - 1], inp_positions[j],
+						inp_velocities[j - 1], inp_velocities[j]));
+	}
+
+	timestamps.push_back(inp_timestamps[inp_timestamps.size() - 1]);
+	positions.push_back(inp_positions[inp_positions.size() - 1]);
+	for (unsigned int i = 1; i < timestamps.size(); i++) {
+		char buf[128];
+		sprintf(buf,
+				"\tservoj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], t=%1.5f)\n",
+				positions[i][0], positions[i][1], positions[i][2],
+				positions[i][3], positions[i][4], positions[i][5],
+				timestamps[i] - timestamps[i - 1]);
+		command_string += buf;
+	}
+	command_string += "end\ntraj()\n";
+	//printf("%s", command_string.c_str());
+	rt_interface_->addCommandToQueue(command_string);
+
+}
+
+void UrDriver::stopTraj() {
+	rt_interface_->addCommandToQueue("stopj(10)\n");
+}
+
 void UrDriver::start() {
 	rt_interface_->start();
 }
@@ -31,3 +105,10 @@ void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4,
 	rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc);
 }
 
+std::vector<std::string> UrDriver::getJointNames() {
+	return joint_names_;
+}
+
+void UrDriver::setJointNames(std::vector<std::string> jn) {
+	joint_names_ = jn;
+}

src/ur_realtime_communication.cpp

@@ -35,7 +35,7 @@ UrRealtimeCommunication::UrRealtimeCommunication(
 	setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int));
 	connected_ = false;
 	keepalive_ = false;
-	safety_count_ = 0;
+	safety_count_ = safety_count_max+1;
 	safety_count_max_ = safety_count_max;
 }
 

src/ur_ros_wrapper.cpp

@@ -16,19 +16,146 @@
 #include <vector>
 #include <mutex>
 #include <condition_variable>
+#include <thread>
+
 #include "sensor_msgs/JointState.h"
 #include "geometry_msgs/WrenchStamped.h"
-#include <thread>
+#include "control_msgs/FollowJointTrajectoryAction.h"
+#include "actionlib/server/simple_action_server.h"
+#include "trajectory_msgs/JointTrajectoryPoint.h"
 
 std::condition_variable g_msg_cond;
 
-void publishRTMsg(UrDriver robot, std::vector<std::string> joint_names) {
-	ros::NodeHandle n_rt;
-	ros::Publisher joint_pub = n_rt.advertise<sensor_msgs::JointState>("/joint_states", 1);
-	ros::Publisher wrench_pub = n_rt.advertise<geometry_msgs::WrenchStamped>("/wrench", 1);
+class URTrajectoryFollower {
+protected:
+	ros::NodeHandle nh_;
+	actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_;
+	std::string action_name_;
+	actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal_;
+	control_msgs::FollowJointTrajectoryFeedback feedback_;
+	control_msgs::FollowJointTrajectoryResult result_;
+	ros::Subscriber sub_;
+	UrDriver* robot_;
+public:
+	URTrajectoryFollower(UrDriver* robot, std::string name) :
+			as_(nh_, "follow_joint_trajectory", false), action_name_(name) {
+		robot_ = robot;
+		//register the goal and feeback callbacks
+		as_.registerGoalCallback(
+				boost::bind(&URTrajectoryFollower::goalCB, this));
+		as_.registerPreemptCallback(
+				boost::bind(&URTrajectoryFollower::preemptCB, this));
+
+		//subscribe to the data topic of interest
+		as_.start();
+		ROS_INFO("The action server for this driver has been started");
+	}
+	void goalCB() {
+		ROS_INFO("on_goal");
+
+		actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction>::GoalConstPtr goal =
+				as_.acceptNewGoal();
+		goal_ = *goal; //make a copy that we can modify
+		if (!validateJointNames()) {
+			std::string outp_joint_names = "";
+			for (unsigned int i = 0; i < goal_.trajectory.joint_names.size();
+					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 =
+					"Received a goal with incorrect joint names: %s", outp_joint_names.c_str(); */
+			as_.setAborted(result_, ("Received a goal with incorrect joint names: %s", outp_joint_names.c_str()));
+		}
+
+		reorder_traj_joints();
+		std::vector<double> timestamps;
+		std::vector<std::vector<double> > positions, velocities;
+		for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) {
+			timestamps.push_back(goal_.trajectory.points[i].time_from_start.toSec());
+			positions.push_back(goal_.trajectory.points[i].positions);
+			velocities.push_back(goal_.trajectory.points[i].velocities);
+
+		}
+		robot_->addTraj(timestamps, positions, velocities);
+
+		//sleep?
+		result_.error_code = result_.SUCCESSFUL;
+		as_.setSucceeded(result_);
+
+	}
+
+	void preemptCB() {
+		ROS_INFO("%s: Preempted", action_name_.c_str());
+		// set the action state to preempted
+		robot_->stopTraj();
+		as_.setPreempted();
+	}
+private:
+	bool validateJointNames() {
+		std::vector<std::string> actual_joint_names = robot_->getJointNames();
+		if (goal_.trajectory.joint_names.size() != actual_joint_names.size())
+			return false;
+
+		for (unsigned int i = 0; i < goal_.trajectory.joint_names.size(); i++) {
+			unsigned int j;
+			for (j = 0; j < actual_joint_names.size(); j++) {
+				if (goal_.trajectory.joint_names[i] == actual_joint_names[j])
+					break;
+			}
+			if (goal_.trajectory.joint_names[i] == actual_joint_names[j]) {
+				actual_joint_names.erase(actual_joint_names.begin() + j);
+			} else {
+				return false;
+			}
+		}
+
+		return true;
+	}
+
+	void reorder_traj_joints() {
+		std::vector<std::string> actual_joint_names = robot_->getJointNames();
+		std::vector<unsigned int> mapping;
+		mapping.resize(actual_joint_names.size(), actual_joint_names.size());
+		for (unsigned int i = 0; i < goal_.trajectory.joint_names.size(); i++) {
+			for (unsigned int j = 0; j < actual_joint_names.size(); j++) {
+				if (goal_.trajectory.joint_names[i] == actual_joint_names[j])
+					mapping[j] = i;
+			}
+		}
+		goal_.trajectory.joint_names = actual_joint_names;
+		std::vector<trajectory_msgs::JointTrajectoryPoint> new_traj;
+		for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) {
+			trajectory_msgs::JointTrajectoryPoint new_point;
+			for (unsigned int j = 0;
+					j < goal_.trajectory.points[i].positions.size(); j++) {
+				new_point.positions.push_back(
+						goal_.trajectory.points[i].positions[mapping[j]]);
+				new_point.velocities.push_back(
+						goal_.trajectory.points[i].velocities[mapping[j]]);
+				new_point.accelerations.push_back(
+						goal_.trajectory.points[i].accelerations[mapping[j]]);
+			}
+			new_point.time_from_start =
+					goal_.trajectory.points[i].time_from_start;
+			new_traj.push_back(new_point);
+		}
+		goal_.trajectory.points = new_traj;
+	}
+
+};
+
+void publishRTMsg(UrDriver robot) {
+	ros::NodeHandle nh_rt;
+	ros::Publisher joint_pub = nh_rt.advertise<sensor_msgs::JointState>(
+			"/joint_states", 1);
+	ros::Publisher wrench_pub = nh_rt.advertise<geometry_msgs::WrenchStamped>(
+			"/wrench", 1);
 	while (ros::ok()) {
 		sensor_msgs::JointState joint_msg;
-		joint_msg.name = joint_names;
+		joint_msg.name = robot.getJointNames();
 		geometry_msgs::WrenchStamped wrench_msg;
 		std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex
 		std::unique_lock<std::mutex> locker(msg_lock);
@@ -40,7 +167,8 @@ void publishRTMsg(UrDriver robot, std::vector<std::string> joint_names) {
 		joint_msg.velocity = robot.rt_interface_->robot_state_->getQdActual();
 		joint_msg.effort = robot.rt_interface_->robot_state_->getIActual();
 		joint_pub.publish(joint_msg);
-		std::vector<double> tcp_force = robot.rt_interface_->robot_state_->getTcpForce();
+		std::vector<double> tcp_force =
+				robot.rt_interface_->robot_state_->getTcpForce();
 		wrench_msg.header.stamp = joint_msg.header.stamp;
 		wrench_msg.wrench.force.x = tcp_force[0];
 		wrench_msg.wrench.force.y = tcp_force[1];
@@ -57,14 +185,12 @@ void publishRTMsg(UrDriver robot, std::vector<std::string> joint_names) {
 
 int main(int argc, char **argv) {
 	bool use_sim_time = false;
-	std::string joint_prefix ="";
+	std::string joint_prefix = "";
 	std::string host;
 	std::vector<std::string> joint_names;
-	UrDriver robot(g_msg_cond, host);
-
 
 	ros::init(argc, argv, "ur_driver");
-	ros::NodeHandle n;
+	ros::NodeHandle nh;
 	if (ros::param::get("use_sim_time", use_sim_time)) {
 		ROS_WARN("use_sim_time is set!!");
 	}
@@ -72,7 +198,7 @@ int main(int argc, char **argv) {
 		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 + "should_lift_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");
@@ -81,12 +207,17 @@ int main(int argc, char **argv) {
 	if (argc > 1) {
 		host = argv[1];
 	} else if (!(ros::param::get("~robot_ip", host))) {
-		ROS_FATAL("Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip");
+		ROS_FATAL(
+				"Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip");
 		exit(1);
 	}
+	UrDriver robot(g_msg_cond, host, joint_names);
+
 	robot.start();
 
-	std::thread rt_publish_thread(publishRTMsg, robot, joint_names);
+	std::thread rt_publish_thread(publishRTMsg, robot);
+	URTrajectoryFollower action_server(&robot, ros::this_node::getName());
+
 	ros::spin();
 	robot.halt();
 	exit(0);