Changed how trajectories are handled to try and speed things up

include/ur_modern_driver/ur_driver.h

@@ -19,6 +19,12 @@
 #include <vector>
 #include <math.h>
 #include <string>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+
+#include <chrono>
+
 
 class UrDriver {
 private:
@@ -26,23 +32,36 @@ private:
 	double minimum_payload_;
 	double maximum_payload_;
 	std::vector<std::string> joint_names_;
+	std::string ip_addr_;
+	const int MULT_JOINTSTATE_ = 1000000;
+	const int MULT_TIME_ = 1000000;
+	const unsigned int REVERSE_PORT_;
+	int incoming_sockfd_;
 public:
 	UrRealtimeCommunication* rt_interface_;
 	UrCommunication* sec_interface_;
 
-	UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host,
+	UrDriver(std::condition_variable& rt_msg_cond,
-			unsigned int safety_count_max = 12, double max_time_step = 0.08,
+			std::condition_variable& msg_cond, std::string host,
-			double min_payload = 0., double max_payload = 1.);
+			unsigned int reverse_port = 50007, unsigned int safety_count_max =
+					12, double max_time_step = 0.08, double min_payload = 0.,
+			double max_payload = 1.);
 	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,
+	void addTraj(
+			std::vector<double> inp_timestamps, //DEPRECATED
 			std::vector<std::vector<double> > positions,
 			std::vector<std::vector<double> > velocities);
+	void doTraj(std::vector<double> inp_timestamps,
+			std::vector<std::vector<double> > inp_positions,
+			std::vector<std::vector<double> > inp_velocities);
 	void stopTraj();
 
+	void uploadProg();
+
 	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);

include/ur_modern_driver/ur_realtime_communication.h

@@ -28,6 +28,8 @@
 #include <netdb.h>
 #include <iostream>
 #include <unistd.h>
+#include <arpa/inet.h>
+#include <errno.h>
 
 #ifdef ROS_BUILD
 #include <ros/ros.h>
@@ -39,6 +41,7 @@ private:
 	int sockfd_;
 	struct sockaddr_in serv_addr_;
 	struct hostent *server_;
+	std::string local_ip_;
 	bool keepalive_;
 	std::thread comThread_;
 	int flag_;
@@ -47,6 +50,7 @@ private:
 	unsigned int safety_count_;
 	void run();
 
+
 public:
 	bool connected_;
 	RobotStateRT* robot_state_;
@@ -59,6 +63,7 @@ public:
 			double q5, double acc = 100.);
 	void addCommandToQueue(std::string inp);
 	void setSafetyCountMax(uint inp);
+	std::string getLocalIp();
 
 };
 

src/ur_communication.cpp

@@ -98,7 +98,7 @@ void UrCommunication::start() {
 	//wait for some traffic so the UR socket doesn't die in version 3.1.
 	std::this_thread::sleep_for(std::chrono::milliseconds(500));
 #ifdef ROS_BUILD
-	ROS_INFO("Firmware version detected: %1.7f", robot_state_->getVersion());
+	ROS_DEBUG("Firmware version detected: %1.7f", robot_state_->getVersion());
 #else
 	printf("Firmware version detected: %f\n", robot_state_->getVersion());
 #endif

src/ur_driver.cpp

@@ -11,15 +11,47 @@
 
 #include "ur_modern_driver/ur_driver.h"
 
-UrDriver::UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host,
+UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
-		unsigned int safety_count_max, double max_time_step, double min_payload,
+		std::condition_variable& msg_cond, std::string host,
-		double max_payload) :
+		unsigned int reverse_port, unsigned int safety_count_max,
-		maximum_time_step_(max_time_step), minimum_payload_(min_payload), maximum_payload_(
+		double max_time_step, double min_payload, double max_payload) :
-				max_payload) {
+		REVERSE_PORT_(reverse_port), maximum_time_step_(max_time_step), minimum_payload_(
+				min_payload), maximum_payload_(max_payload) {
+	char buffer[256];
+	struct sockaddr_in serv_addr;
+	int n, flag;
+	//char *ip_addr;
+
 	rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host,
 			safety_count_max);
 	sec_interface_ = new UrCommunication(msg_cond, host);
 
+	incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+	if (incoming_sockfd_ < 0) {
+#ifdef ROS_BUILD
+		ROS_FATAL("ERROR opening socket for reverse communication");
+#else
+		printf("ERROR opening socket for reverse communication");
+#endif
+	}
+	bzero((char *) &serv_addr, sizeof(serv_addr));
+
+	serv_addr.sin_family = AF_INET;
+	serv_addr.sin_addr.s_addr = INADDR_ANY;
+	serv_addr.sin_port = htons(REVERSE_PORT_);
+	flag = 1;
+	setsockopt(incoming_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag,
+			sizeof(int));
+	setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int));
+	if (bind(incoming_sockfd_, (struct sockaddr *) &serv_addr,
+			sizeof(serv_addr)) < 0) {
+#ifdef ROS_BUILD
+		ROS_FATAL("ERROR on binding socket for reverse communication");
+#else
+		printf("ERROR on binding socket for reverse communication");
+#endif
+	}
+	listen(incoming_sockfd_, 5);
 }
 
 std::vector<double> UrDriver::interp_cubic(double t, double T,
@@ -42,7 +74,8 @@ std::vector<double> UrDriver::interp_cubic(double t, double T,
 void UrDriver::addTraj(std::vector<double> inp_timestamps,
 		std::vector<std::vector<double> > inp_positions,
 		std::vector<std::vector<double> > inp_velocities) {
-
+	/* DEPRECATED */
+	printf("!! addTraj is deprecated !!\n");
 	std::vector<double> timestamps;
 	std::vector<std::vector<double> > positions;
 	std::string command_string = "def traj():\n";
@@ -92,19 +125,194 @@ void UrDriver::addTraj(std::vector<double> inp_timestamps,
 
 }
 
+void UrDriver::doTraj(std::vector<double> inp_timestamps,
+		std::vector<std::vector<double> > inp_positions,
+		std::vector<std::vector<double> > inp_velocities) {
+	struct sockaddr_in cli_addr;
+	socklen_t clilen;
+	int new_sockfd;
+	std::chrono::high_resolution_clock::time_point t0, t;
+	std::vector<double> positions;
+	unsigned int j, bytes_written;
+	int tmp;
+	unsigned char buf[32];
+
+	UrDriver::uploadProg();
+
+	clilen = sizeof(cli_addr);
+	new_sockfd = accept(incoming_sockfd_, (struct sockaddr *) &cli_addr,
+			&clilen);
+	if (new_sockfd < 0) {
+#ifdef ROS_BUILD
+		ROS_FATAL("ERROR on accepting reverse communication");
+#else
+		printf("ERROR on accepting reverse communication");
+#endif
+	}
+
+	t0 = std::chrono::high_resolution_clock::now();
+	t = t0;
+	j = 0;
+	while (inp_timestamps[inp_timestamps.size() - 1]
+			>= std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count()) {
+		while (inp_timestamps[j]
+				<= std::chrono::duration_cast<std::chrono::duration<double>>(
+						t - t0).count() && j < inp_timestamps.size() - 1) {
+			j += 1;
+		}
+		positions = UrDriver::interp_cubic(
+				std::chrono::duration_cast<std::chrono::duration<double>>(
+						t - t0).count()- 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]);
+		for (int i = 0; i < 6; i++) {
+			tmp = htonl((int) (positions[i] * MULT_JOINTSTATE_));
+			buf[i * 4] = tmp & 0xff;
+			buf[i * 4 + 1] = (tmp >> 8) & 0xff;
+			buf[i * 4 + 2] = (tmp >> 16) & 0xff;
+			buf[i * 4 + 3] = (tmp >> 24) & 0xff;
+		}
+		tmp = htonl((int) (0.008 * MULT_TIME_));
+		buf[6 * 4] = tmp & 0xff;
+		buf[6 * 4 + 1] = (tmp >> 8) & 0xff;
+		buf[6 * 4 + 2] = (tmp >> 16) & 0xff;
+		buf[6 * 4 + 3] = (tmp >> 24) & 0xff;
+		tmp = htonl((int) 1);
+		buf[7 * 4] = tmp & 0xff;
+		buf[7 * 4 + 1] = (tmp >> 8) & 0xff;
+		buf[7 * 4 + 2] = (tmp >> 16) & 0xff;
+		buf[7 * 4 + 3] = (tmp >> 24) & 0xff;
+		bytes_written = write(new_sockfd, buf, 32);
+
+		// oversample with 2 * sample_time
+		std::this_thread::sleep_for(std::chrono::milliseconds(4));
+
+		t = std::chrono::high_resolution_clock::now();
+	}
+	//Signal robot to stop driverProg()
+	for (int i = 0; i < 6; i++) {
+		tmp = htonl(
+				(int) (inp_positions[inp_positions.size() - 1][i]
+						* MULT_JOINTSTATE_));
+		buf[i * 4] = tmp & 0xff;
+		buf[i * 4 + 1] = (tmp >> 8) & 0xff;
+		buf[i * 4 + 2] = (tmp >> 16) & 0xff;
+		buf[i * 4 + 3] = (tmp >> 24) & 0xff;
+	}
+	tmp = htonl((int) (0.008 * MULT_TIME_));
+	buf[6 * 4] = tmp & 0xff;
+	buf[6 * 4 + 1] = (tmp >> 8) & 0xff;
+	buf[6 * 4 + 2] = (tmp >> 16) & 0xff;
+	buf[6 * 4 + 3] = (tmp >> 24) & 0xff;
+	tmp = htonl((int) 0);
+	buf[7 * 4] = tmp & 0xff;
+	buf[7 * 4 + 1] = (tmp >> 8) & 0xff;
+	buf[7 * 4 + 2] = (tmp >> 16) & 0xff;
+	buf[7 * 4 + 3] = (tmp >> 24) & 0xff;
+	bytes_written = write(new_sockfd, buf, 32);
+
+	close(new_sockfd);
+
+}
+
 void UrDriver::stopTraj() {
 	rt_interface_->addCommandToQueue("stopj(10)\n");
 }
 
+void UrDriver::uploadProg() {
+	std::string cmd_str;
+	char buf[128];
+	cmd_str = "def driverProg():\n";
+
+	sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_);
+	cmd_str += buf;
+
+	sprintf(buf, "\tMULT_time = %i\n", MULT_TIME_);
+	cmd_str += buf;
+
+	cmd_str += "\tSERVO_IDLE = 0\n";
+	cmd_str += "\tSERVO_RUNNING = 1\n";
+	cmd_str += "\tcmd_servo_state = SERVO_IDLE\n";
+	cmd_str += "\tcmd_servo_id = 0  # 0 = idle, -1 = stop\n";
+	cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n";
+	cmd_str += "\tcmd_servo_dt = 0.0\n";
+	cmd_str += "\tdef set_servo_setpoint(q, dt):\n";
+	cmd_str += "\t\tenter_critical\n";
+	cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n";
+	cmd_str += "\t\tcmd_servo_q = q\n";
+	cmd_str += "\t\tcmd_servo_dt = dt\n";
+	cmd_str += "\t\texit_critical\n";
+	cmd_str += "\tend\n";
+	cmd_str += "\tthread servoThread():\n";
+	cmd_str += "\t\tstate = SERVO_IDLE\n";
+	cmd_str += "\t\twhile True:\n";
+	cmd_str += "\t\t\tenter_critical\n";
+	cmd_str += "\t\t\tq = cmd_servo_q\n";
+	cmd_str += "\t\t\tdt = cmd_servo_dt\n";
+	cmd_str += "\t\t\tdo_brake = False\n";
+	cmd_str += "\t\t\tif (state == SERVO_RUNNING) and ";
+	cmd_str += "(cmd_servo_state == SERVO_IDLE):\n";
+	cmd_str += "\t\t\t\tdo_brake = True\n";
+	cmd_str += "\t\t\tend\n";
+	cmd_str += "\t\t\tstate = cmd_servo_state\n";
+	cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n";
+	cmd_str += "\t\t\texit_critical\n";
+	cmd_str += "\t\t\tif do_brake:\n";
+	cmd_str += "\t\t\t\tstopj(1.0)\n";
+	cmd_str += "\t\t\t\tsync()\n";
+	cmd_str += "\t\t\telif state == SERVO_RUNNING:\n";
+	cmd_str += "\t\t\t\tservoj(q, t=dt)\n";
+	cmd_str += "\t\t\telse:\n";
+	cmd_str += "\t\t\t\tsync()\n";
+	cmd_str += "\t\t\tend\n";
+	cmd_str += "\t\tend\n";
+	cmd_str += "\tend\n";
+
+	sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(),
+			REVERSE_PORT_);
+	cmd_str += buf;
+
+	cmd_str += "\tthread_servo = run servoThread()\n";
+	cmd_str += "\tkeepalive = 1\n";
+	cmd_str += "\twhile keepalive > 0:\n";
+	cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1+1)\n";
+	cmd_str += "\t\tif params_mult[0] > 0:\n";
+	cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, ";
+	cmd_str += "params_mult[2] / MULT_jointstate, ";
+	cmd_str += "params_mult[3] / MULT_jointstate, ";
+	cmd_str += "params_mult[4] / MULT_jointstate, ";
+	cmd_str += "params_mult[5] / MULT_jointstate, ";
+	cmd_str += "params_mult[6] / MULT_jointstate]\n";
+	cmd_str += "\t\t\tt = params_mult[7] / MULT_time\n";
+	cmd_str += "\t\t\tkeepalive = params_mult[8]\n";
+	cmd_str += "\t\t\tset_servo_setpoint(q, t)\n";
+	cmd_str += "\t\tend\n";
+	cmd_str += "\tend\n";
+	cmd_str += "\tsleep(.1)\n";
+	cmd_str += "\tsocket_close()\n";
+	cmd_str += "end\n";
+
+	rt_interface_->addCommandToQueue(cmd_str);
+
+}
+
 void UrDriver::start() {
 	sec_interface_->start();
-	rt_interface_->robot_state_->setVersion(sec_interface_->robot_state_->getVersion());
+	rt_interface_->robot_state_->setVersion(
+			sec_interface_->robot_state_->getVersion());
 	rt_interface_->start();
+	ip_addr_ = rt_interface_->getLocalIp(); //inet_ntoa(serv_addr.sin_addr);
+#ifdef ROS_BUILD
+			ROS_DEBUG("Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_);
+#else
+	printf("Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_);
+#endif
 }
 
 void UrDriver::halt() {
 	sec_interface_->halt();
 	rt_interface_->halt();
+	close(incoming_sockfd_);
 }
 
 void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4,

src/ur_realtime_communication.cpp

@@ -56,12 +56,26 @@ void UrRealtimeCommunication::start() {
 	printf("Realtime port: Connecting...\n");
 #endif
 	if (connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_))
-			< 0)
+			< 0) {
 #ifdef ROS_BUILD
 		ROS_FATAL("Error connecting to RT port 30003");
 #else
 		printf("Error connecting to RT port 30003\n");
 #endif
+	}
+	sockaddr_in name;
+	socklen_t namelen = sizeof(name);
+	int err = getsockname(sockfd_, (sockaddr*) &name, &namelen);
+	if (err < 0) {
+#ifdef ROS_BUILD
+		ROS_FATAL("Could not get local IP - errno: %d (%s)", errno, strerror(errno));
+#else
+		printf("Could not get local IP - errno: %d (%s)", errno, strerror(errno));
+#endif
+	}
+	char str[18];
+	inet_ntop(AF_INET, &name.sin_addr, str, 18);
+	local_ip_ = str;
 	comThread_ = std::thread(&UrRealtimeCommunication::run, this);
 }
 
@@ -119,3 +133,6 @@ void UrRealtimeCommunication::setSafetyCountMax(uint inp) {
 	safety_count_max_ = inp;
 }
 
+std::string UrRealtimeCommunication::getLocalIp() {
+	return local_ip_;
+}

src/ur_ros_wrapper.cpp

@@ -95,7 +95,7 @@ public:
 			}
 			robot_.setMinPayload(min_payload);
 			robot_.setMaxPayload(max_payload);
-			ROS_INFO("Bounds for set_payload service calls: [%f, %f]",
+			ROS_DEBUG("Bounds for set_payload service calls: [%f, %f]",
 					min_payload, max_payload);
 		}
 
@@ -122,7 +122,31 @@ public:
 				boost::bind(&RosWrapper::publishRTMsg, this));
 		mb_publish_thread_ = new std::thread(
 				boost::bind(&RosWrapper::publishMbMsg, this));
-		ROS_INFO("The action server for this driver has been started");
+		ROS_DEBUG("The action server for this driver has been started");
+		/*double pi = 3.141592653589793;
+		std::vector<double> tmp, t;
+		std::vector<std::vector<double> > pos, vel;
+		tmp.push_back(-pi / 2);
+		tmp.push_back(-pi / 2);
+		tmp.push_back(-pi / 2);
+		tmp.push_back(0);
+		tmp.push_back(-pi / 2);
+		tmp.push_back(0);
+		pos.push_back(tmp);
+		tmp[5] = pi;
+		pos.push_back(tmp);
+		tmp[5] = 0;
+		pos.push_back(tmp);
+		for (int i = 0; i < 6; i++) {
+			tmp[i] = 0;
+		}
+		vel.push_back(tmp);
+		vel.push_back(tmp);
+		vel.push_back(tmp);
+		t.push_back(0.);
+		t.push_back(4.);
+		t.push_back(8.);
+		robot_.doTraj(t, pos, vel); */
 
 	}
 
@@ -184,7 +208,7 @@ private:
 			velocities.push_back(goal_.trajectory.points[i].velocities);
 
 		}
-		robot_.addTraj(timestamps, positions, velocities);
+		robot_.doTraj(timestamps, positions, velocities);
 
 		ros::Duration(timestamps.back()).sleep();
 		result_.error_code = result_.SUCCESSFUL;