Implemented communication on port 30001 and 30002 - untested

include/ur_modern_driver/robot_state.h

@@ -0,0 +1,141 @@
+/*
+ * robot_state.h
+ *
+ *  Created on: Sep 10, 2015
+ *      Author: ttan
+ */
+
+#ifndef ROBOT_STATE_H_
+#define ROBOT_STATE_H_
+
+#include <inttypes.h>
+#include <vector>
+#include <stdlib.h>
+#include <string.h>
+#include <mutex>
+#include <condition_variable>
+
+namespace message_types {
+enum message_type {
+	ROBOT_STATE = 16, ROBOT_MESSAGE = 20, PROGRAM_STATE_MESSAGE = 25
+};
+}
+typedef message_types::message_type messageType;
+
+namespace package_types {
+enum package_type {
+	ROBOT_MODE_DATA = 0,
+	JOINT_DATA = 1,
+	TOOL_DATA = 2,
+	MASTERBOARD_DATA = 3,
+	CARTESIAN_INFO = 4,
+	KINEMATICS_INFO = 5,
+	CONFIGURATION_DATA = 6,
+	FORCE_MODE_DATA = 7,
+	ADDITIONAL_INFO = 8,
+	CALIBRATION_DATA = 9
+};
+}
+typedef package_types::package_type packageType;
+
+namespace robot_message_types {
+enum robot_message_type {
+	ROBOT_MESSAGE_TEXT = 0,
+	ROBOT_MESSAGE_PROGRAM_LABEL = 1,
+	PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2,
+	ROBOT_MESSAGE_VERSION = 3,
+	ROBOT_MESSAGE_SAFETY_MODE = 5,
+	ROBOT_MESSAGE_ERROR_CODE = 6,
+	ROBOT_MESSAGE_KEY = 7,
+	ROBOT_MESSAGE_REQUEST_VALUE = 9,
+	ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10
+};
+}
+typedef robot_message_types::robot_message_type robotMessageType;
+
+struct version_message {
+	uint64_t timestamp;
+	int8_t source;
+	int8_t robot_message_type;
+	int8_t project_name_size;
+	char* project_name;
+	uint8_t major_version;
+	uint8_t minor_version;
+	int svn_revision;
+	char* build_date;
+};
+
+struct masterboard_data {
+	int digitalInputBits;
+	int digitalOutputBits;
+	char analogInputRange0;
+	char analogInputRange1;
+	double analogInput0;
+	double analogInput1;
+	char analogOutputDomain0;
+	char analogOutputDomain1;
+	double analogOutput0;
+	double analogOutput1;
+	float masterBoardTemperature;
+	float robotVoltage48V;
+	float robotCurrent;
+	float masterIOCurrent;
+	unsigned char safetyMode;
+	unsigned char InReducedMode;
+	char euromap67InterfaceInstalled;
+	int euromapInputBits;
+	int euromapOutputBits;
+	float euromapVoltage;
+	float euromapCurrent;
+	uint32_t internal;
+
+};
+
+class RobotState {
+private:
+	version_message version_msg_;
+	masterboard_data mb_data_;
+
+	std::mutex val_lock_; // Locks the variables while unpack parses data;
+
+	std::condition_variable* pMsg_cond_; //Signals that new vars are available
+	bool new_data_available_; //to avoid spurious wakes
+
+	double ntohd(uint64_t nf);
+
+public:
+	RobotState(std::condition_variable& msg_cond);
+	~RobotState();
+	double getVersion();
+	double getTime();
+	std::vector<double> getQTarget();
+	int getDigitalInputBits();
+	int getDigitalOutputBits();
+	char getAnalogInputRange0();
+	char getAnalogInputRange1();
+	double getAnalogInput0();
+	double getAnalogInput1();
+	char getAnalogOutputDomain0();
+	char getAnalogOutputDomain1();
+	double getAnalogOutput0();
+	double getAnalogOutput1();
+	float getMasterBoardTemperature();
+	float getRobotVoltage48V();
+	float getRobotCurrent();
+	float getMasterIOCurrent();
+	unsigned char getSafetyMode();
+	unsigned char getInReducedMode();
+	char getEuromap67InterfaceInstalled();
+	int getEuromapInputBits();
+	int getEuromapOutputBits();
+	float getEuromapVoltage();
+	float getEuromapCurrent();
+	bool getNewDataAvailable();
+	void finishedReading();
+	std::vector<double> getVActual();
+	int unpack(uint8_t * buf, unsigned int buf_length);
+	void unpackRobotMessage(uint8_t * buf, unsigned int offset,uint32_t len);
+	void unpackRobotState(uint8_t * buf, unsigned int offset, uint32_t len);
+};
+
+#endif /* ROBOT_STATE_H_ */

include/ur_modern_driver/ur_communication.h

@@ -0,0 +1,56 @@
+/*
+ * ur_communication.h
+ *
+ * ----------------------------------------------------------------------------
+ * "THE BEER-WARE LICENSE" (Revision 42):
+ * <thomas.timm.dk@gmail.com> wrote this file.  As long as you retain this notice you
+ * can do whatever you want with this stuff. If we meet some day, and you think
+ * this stuff is worth it, you can buy me a beer in return.   Thomas Timm Andersen
+ * ----------------------------------------------------------------------------
+ */
+
+#ifndef UR_COMMUNICATION_H_
+#define UR_COMMUNICATION_H_
+
+#include "robot_state.h"
+#include <vector>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <sys/time.h>
+#include <thread>
+#include <mutex>
+#include <condition_variable>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netinet/tcp.h>
+#include <netdb.h>
+#include <iostream>
+#include <unistd.h>
+#include <chrono>
+
+class UrCommunication {
+private:
+	int sockfd_;
+	struct sockaddr_in pri_serv_addr_, sec_serv_addr_;
+	struct hostent *server_;
+	bool keepalive_;
+	std::thread comThread_;
+	int flag_;
+	std::recursive_mutex command_string_lock_;
+	std::string command_;
+	void run();
+
+public:
+	bool connected_;
+	RobotState* robot_state_;
+
+	UrCommunication(std::condition_variable& msg_cond, std::string host);
+	void start();
+	void halt();
+	void addCommandToQueue(std::string inp);
+
+};
+
+#endif /* UR_COMMUNICATION_H_ */

launch/ur_common.launch~

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<!--
+  Universal robot common bringup.  Starts ur driver node and robot state
+  publisher (translates joint positions to propper tfs).
+  
+  Usage:
+    ur_common.launch robot_ip:=<value>
+-->
+<launch>
+  <!-- robot_ip: IP-address of the robot's socket-messaging server -->
+  <arg name="robot_ip"/>
+  <arg name="min_payload"/>
+  <arg name="max_payload"/>
+  
+  
+  <!-- copy the specified IP address to be consistant with ROS-Industrial spec.
+  NOTE: The ip address is actually passed to the driver on the command line -->
+  <param name="/robot_ip_address" type="str" value="$(arg robot_ip)"/>
+  
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+
+  <!-- driver -->
+  <node name="ur_driver" pkg="ur_modern_driver" type="ur_driver" args="$(arg robot_ip)" output="screen">
+    <param name="min_payload" type="double" value="$(arg min_payload)"/>
+    <param name="max_payload" type="double" value="$(arg max_payload)"/>
+    <param name="max_velocity" type="double" value="$(arg max_velocity)"/>
+  </node>
+</launch>

src/robot_state.cpp

@@ -0,0 +1,102 @@
+/*
+ * robot_state.cpp
+ *
+ *  Created on: Sep 10, 2015
+ *      Author: ttan
+ */
+
+#include "ur_modern_driver/robot_state.h"
+
+RobotState::RobotState(std::condition_variable& msg_cond) {
+	version_msg_.major_version = 0;
+	version_msg_.minor_version = 0;
+	new_data_available_ = false;
+	pMsg_cond_ = &msg_cond;
+}
+
+int RobotState::unpack(uint8_t* buf, unsigned int buf_length) {
+	/* Returns missing bytes to unpack a message, or 0 if all data was parsed */
+	unsigned int offset = 0;
+	while (buf_length > offset) {
+		uint32_t len;
+		unsigned char message_type;
+		memcpy(&len, &buf[offset], sizeof(len));
+		if (len + offset > buf_length) {
+			return (len + offset - buf_length);
+		}
+		memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type));
+
+		switch (message_type) {
+		case messageType::ROBOT_MESSAGE:
+			RobotState::unpackRobotMessage(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType
+			break;
+		case messageType::ROBOT_STATE:
+			RobotState::unpackRobotState(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType
+			break;
+		case messageType::PROGRAM_STATE_MESSAGE:
+			//Don't do anything atm...
+		default:
+			break;
+		}
+		offset += len;
+
+	}
+	return 0;
+}
+
+void RobotState::unpackRobotMessage(uint8_t * buf, unsigned int offset,
+		uint32_t len) {
+	offset += 5;
+	uint64_t timestamp;
+	int8_t source, robot_message_type;
+	memcpy(&timestamp, &buf[offset], sizeof(timestamp));
+	offset += sizeof(timestamp);
+	memcpy(&source, &buf[offset], sizeof(source));
+	offset += sizeof(source);
+	memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type));
+	offset += sizeof(robot_message_type);
+	val_lock_.lock();
+	switch (robot_message_type) {
+	case robotMessageType::ROBOT_MESSAGE_VERSION:
+		version_msg_.timestamp = timestamp;
+		version_msg_.source = source;
+		version_msg_.robot_message_type = robot_message_type;
+		memcpy(&version_msg_.project_name_size, &buf[offset],
+				sizeof(version_msg_.project_name_size));
+		offset += sizeof(version_msg_.project_name_size);
+		memcpy(&version_msg_.project_name, &buf[offset],
+				sizeof(char) * version_msg_.project_name_size);
+		offset += version_msg_.project_name_size;
+		memcpy(&version_msg_.major_version, &buf[offset],
+				sizeof(version_msg_.major_version));
+		offset += sizeof(version_msg_.major_version);
+		memcpy(&version_msg_.minor_version, &buf[offset],
+				sizeof(version_msg_.minor_version));
+		offset += sizeof(version_msg_.minor_version);
+		memcpy(&version_msg_.svn_revision, &buf[offset],
+				sizeof(version_msg_.svn_revision));
+		offset += sizeof(version_msg_.svn_revision);
+		memcpy(&version_msg_.build_date, &buf[offset],
+				sizeof(char) * len - offset);
+
+		break;
+	default:
+		break;
+	}
+	val_lock_.unlock();
+
+}
+void RobotState::unpackRobotState(uint8_t * buf, unsigned int offset,
+		uint32_t len) {
+
+}
+
+double RobotState::getVersion() {
+	double ver;
+	val_lock_.lock();
+	ver = version_msg_.major_version + 0.1 * version_msg_.minor_version + .000001*version_msg_.svn_revision;
+	val_lock_.unlock();
+	return ver;
+
+}
+

src/ur_communication.cpp

@@ -0,0 +1,109 @@
+/*
+ * ur_communication.cpp
+ *
+ * ----------------------------------------------------------------------------
+ * "THE BEER-WARE LICENSE" (Revision 42):
+ * <thomas.timm.dk@gmail.com> wrote this file.  As long as you retain this notice you
+ * can do whatever you want with this stuff. If we meet some day, and you think
+ * this stuff is worth it, you can buy me a beer in return.   Thomas Timm Andersen
+ * ----------------------------------------------------------------------------
+ */
+
+#include "ur_modern_driver/ur_communication.h"
+
+UrCommunication::UrCommunication(std::condition_variable& msg_cond,
+		std::string host) {
+	robot_state_ = new RobotState(msg_cond);
+	bzero((char *) &pri_serv_addr_, sizeof(pri_serv_addr_));
+	bzero((char *) &sec_serv_addr_, sizeof(sec_serv_addr_));
+	sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+	if (sockfd_ < 0) {
+		printf("ERROR opening socket");
+		exit(0);
+	}
+	server_ = gethostbyname(host.c_str());
+	if (server_ == NULL) {
+		printf("ERROR, no such host\n");
+		exit(0);
+	}
+	pri_serv_addr_.sin_family = AF_INET;
+	sec_serv_addr_.sin_family = AF_INET;
+	bcopy((char *) server_->h_addr, (char *)&pri_serv_addr_.sin_addr.s_addr, server_->h_length);
+	bcopy((char *) server_->h_addr, (char *)&sec_serv_addr_.sin_addr.s_addr, server_->h_length);
+	pri_serv_addr_.sin_port = htons(30001);
+	sec_serv_addr_.sin_port = htons(30002);
+	flag_ = 1;
+	setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int));
+	setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int));
+	connected_ = false;
+	keepalive_ = false;
+}
+
+void UrCommunication::start() {
+	keepalive_ = true;
+	uint8_t buf[512];
+	unsigned int bytes_read;
+	std::string cmd;
+	bzero(buf, 512);
+
+	printf("Acquire firmware version: Connecting...\n");
+	if (connect(sockfd_, (struct sockaddr *) &pri_serv_addr_,
+			sizeof(pri_serv_addr_)) < 0) {
+		printf("Error connecting");
+		exit(1);
+	}
+	printf("Acquire firmware version: Got connection\n");
+	bytes_read = read(sockfd_, buf, 512);
+	setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int));
+	robot_state_->unpack(buf, bytes_read);
+	//wait for some traffic so the UR socket doesn't die in version 3.1.
+	std::this_thread::sleep_for(std::chrono::milliseconds(500));
+	printf("Firmware version: %f\n\n", robot_state_->getVersion());
+	close(sockfd_);
+
+	printf("Switching to secondary interface for masterboard data: Connecting...\n"); // which generates less network traffic
+		if (connect(sockfd_, (struct sockaddr *) &sec_serv_addr_,
+				sizeof(sec_serv_addr_)) < 0) {
+			printf("Error connecting");
+			exit(1);
+		}
+		printf("Secondary interface: Got connection\n");
+	comThread_ = std::thread(&UrCommunication::run, this);
+}
+
+void UrCommunication::halt() {
+	keepalive_ = false;
+	comThread_.join();
+}
+
+void UrCommunication::addCommandToQueue(std::string inp) {
+	if (inp.back() != '\n') {
+		inp.append("\n");
+	}
+	command_string_lock_.lock();
+	command_ += inp;
+	command_string_lock_.unlock();
+}
+
+void UrCommunication::run() {
+	uint8_t buf[2048];
+	unsigned int bytes_read;
+	bzero(buf, 2048);
+	printf("Got connection\n");
+	connected_ = true;
+	while (keepalive_) {
+		bytes_read = read(sockfd_, buf, 2048); // usually only up to 1295 bytes
+		setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
+				sizeof(int));
+		robot_state_->unpack(buf, bytes_read);
+		command_string_lock_.lock();
+		if (command_.length() != 0) {
+			write(sockfd_, command_.c_str(), command_.length());
+			command_ = "";
+		}
+		command_string_lock_.unlock();
+
+	}
+	close(sockfd_);
+}
+