cleaned

include/ur_modern_driver/do_output.h

@@ -30,5 +30,4 @@ void print_warning(std::string inp);
 void print_error(std::string inp);
 void print_fatal(std::string inp);
 
-
 #endif /* UR_DO_OUTPUT_H_ */

include/ur_modern_driver/robot_state.h

@@ -109,6 +109,17 @@ struct version_message {
 	char build_date[25];
 };
 
+struct key_message {
+	uint64_t timestamp;
+	int8_t source;
+	int8_t robot_message_type;
+	int8_t robot_message_code;
+	int8_t robot_message_argument;
+	unsigned char title_size;
+	char message_title[64];
+	char text_message[64];
+};
+
 struct masterboard_data {
 	int digitalInputBits;
 	int digitalOutputBits;
@@ -150,6 +161,7 @@ struct robot_mode_data {
 
 class RobotState {
 private:
+	key_message key_msg_;
 	version_message version_msg_;
 	masterboard_data mb_data_;
 	robot_mode_data robot_mode_;
@@ -166,6 +178,7 @@ class RobotState {
 	RobotState(std::condition_variable& msg_cond);
 	~RobotState();
 	double getVersion();
+	key_message getKeyMessage();
 	double getTime();
 	std::vector<double> getQTarget();
 	int getDigitalInputBits();
@@ -208,8 +221,8 @@ class RobotState {
 
 	void unpack(uint8_t * buf, unsigned int buf_length);
 	void unpackRobotMessage(uint8_t * buf, unsigned int offset, uint32_t len);
-	void unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
-			uint32_t len);
+	void unpackRobotMessageVersion(uint8_t * buf, unsigned int offset, uint32_t len);
+	void unpackRobotMessageKey(uint8_t * buf, unsigned int offset, unsigned int message_offset, uint32_t len);
 	void unpackRobotState(uint8_t * buf, unsigned int offset, uint32_t len);
 	void unpackRobotStateMasterboard(uint8_t * buf, unsigned int offset);
 	void unpackRobotMode(uint8_t * buf, unsigned int offset);

include/ur_modern_driver/ur_driver.h

@@ -53,22 +53,21 @@ class UrDriver {
 	double servoj_lookahead_time_;
 	double servoj_gain_;
 
-  std::string host_;
+	std::string host_;
 
 public:
 	UrRealtimeCommunication* rt_interface_;
 	UrCommunication* sec_interface_;
 
 	UrDriver(std::condition_variable& rt_msg_cond,
 			std::condition_variable& msg_cond, std::string host,
-			unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max =
-					12, double max_time_step = 0.08, double min_payload = 0.,
+			unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max = 12,
+			double max_time_step = 0.08, double min_payload = 0.,
 			double max_payload = 1., double servoj_lookahead_time=0.03, double servoj_gain=300.);
 	bool start();
 	void halt();
 
-	void setSpeed(double q0, double q1, double q2, double q3, double q4,
-			double q5, double acc = 100.);
+	void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.);
 
 	bool doTraj(std::vector<double> inp_timestamps,
 			std::vector<std::vector<double> > inp_positions,
@@ -99,7 +98,7 @@ class UrDriver {
 	void setServojLookahead(double t);
 	void setServojGain(double g);
 
-  bool sendDashboardCmd(const char* cmd);
+	bool sendDashboardCmd(const char* cmd);
 };
 
 #endif /* UR_DRIVER_H_ */

include/ur_modern_driver/ur_hardware_interface.h

@@ -122,7 +122,7 @@ class UrHardwareInterface: public hardware_interface::RobotHW {
 	std::vector<double> joint_position_command_;
 	std::vector<double> joint_velocity_command_;
 	std::vector<double> prev_joint_velocity_command_;
-		std::size_t num_joints_;
+	std::size_t num_joints_;
 	double robot_force_[3] = { 0., 0., 0. };
 	double robot_torque_[3] = { 0., 0., 0. };
 

src/robot_state.cpp

@@ -16,6 +16,7 @@
  * limitations under the License.
  */
 
+#include "stdio.h"
 #include "ur_modern_driver/robot_state.h"
 
 RobotState::RobotState(std::condition_variable& msg_cond) {
@@ -62,66 +63,65 @@ void RobotState::unpack(uint8_t* buf, unsigned int buf_length) {
 	return;
 }
 
-void RobotState::unpackRobotMessage(uint8_t * buf, unsigned int offset,
-		uint32_t len) {
-	offset += 5;
+void RobotState::unpackRobotMessage(uint8_t * buf, unsigned int offset, uint32_t len) {
+	unsigned int message_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);
+	memcpy(&timestamp, &buf[offset+message_offset], sizeof(timestamp));
+	message_offset += sizeof(timestamp);
+	memcpy(&source, &buf[offset+message_offset], sizeof(source));
+	message_offset += sizeof(source);
+	memcpy(&robot_message_type, &buf[offset+message_offset], sizeof(robot_message_type));
+	message_offset += sizeof(robot_message_type);
+
 	switch (robot_message_type) {
-	case robotMessageType::ROBOT_MESSAGE_VERSION:
-		val_lock_.lock();
-		version_msg_.timestamp = timestamp;
-		version_msg_.source = source;
-		version_msg_.robot_message_type = robot_message_type;
-		RobotState::unpackRobotMessageVersion(buf, offset, len);
-		val_lock_.unlock();
-		break;
-	default:
-		break;
+		case robotMessageType::ROBOT_MESSAGE_VERSION:
+			val_lock_.lock();
+			version_msg_.timestamp = timestamp;
+			version_msg_.source = source;
+			version_msg_.robot_message_type = robot_message_type;
+			RobotState::unpackRobotMessageVersion(buf, offset+message_offset, len);
+			val_lock_.unlock();
+			break;
+		case robotMessageType::ROBOT_MESSAGE_KEY:
+			val_lock_.lock();
+			key_msg_.timestamp = timestamp;
+			key_msg_.source = source;
+			key_msg_.robot_message_type = robot_message_type;
+			RobotState::unpackRobotMessageKey(buf, offset, message_offset, len);
+			val_lock_.unlock();
+			break;
 	}
 
 }
 
-void RobotState::unpackRobotState(uint8_t * buf, unsigned int offset,
-		uint32_t len) {
+void RobotState::unpackRobotState(uint8_t * buf, unsigned int offset, uint32_t len) {
 	offset += 5;
 	while (offset < len) {
 		int32_t length;
 		uint8_t package_type;
 		memcpy(&length, &buf[offset], sizeof(length));
 		length = ntohl(length);
-		memcpy(&package_type, &buf[offset + sizeof(length)],
-				sizeof(package_type));
+		memcpy(&package_type, &buf[offset + sizeof(length)], sizeof(package_type));
 		switch (package_type) {
-		case packageType::ROBOT_MODE_DATA:
-			val_lock_.lock();
-			RobotState::unpackRobotMode(buf, offset + 5);
-			val_lock_.unlock();
-			break;
-
-		case packageType::MASTERBOARD_DATA:
-			val_lock_.lock();
-			RobotState::unpackRobotStateMasterboard(buf, offset + 5);
-			val_lock_.unlock();
-			break;
-		default:
-			break;
+			case packageType::ROBOT_MODE_DATA:
+				val_lock_.lock();
+				RobotState::unpackRobotMode(buf, offset + 5);
+				val_lock_.unlock();
+				break;
+			case packageType::MASTERBOARD_DATA:
+				val_lock_.lock();
+				RobotState::unpackRobotStateMasterboard(buf, offset + 5);
+				val_lock_.unlock();
+				break;
 		}
 		offset += length;
 	}
 	new_data_available_ = true;
 	pMsg_cond_->notify_all();
-
 }
 
-void RobotState::unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
-		uint32_t len) {
+void RobotState::unpackRobotMessageVersion(uint8_t * buf, unsigned int offset, uint32_t len) {
 	memcpy(&version_msg_.project_name_size, &buf[offset],
 			sizeof(version_msg_.project_name_size));
 	offset += sizeof(version_msg_.project_name_size);
@@ -146,6 +146,37 @@ void RobotState::unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
 	}
 }
 
+void RobotState::unpackRobotMessageKey(uint8_t * buf, unsigned int offset, unsigned int message_offset, uint32_t len) {
+	int robot_message_code, robot_message_argument;
+
+	memcpy(&robot_message_code, &buf[offset+message_offset], sizeof(robot_message_code));
+	robot_message_code = ntohl(robot_message_code);
+	message_offset += sizeof(robot_message_code);
+	key_msg_.robot_message_code = robot_message_code;
+
+	memcpy(&robot_message_argument, &buf[offset+message_offset], sizeof(robot_message_argument));
+	robot_message_code = ntohl(robot_message_argument);
+	message_offset += sizeof(robot_message_argument);
+	key_msg_.robot_message_argument = robot_message_argument;
+
+	unsigned char title_size;
+	memcpy(&title_size, &buf[offset+message_offset], sizeof(title_size));
+	message_offset += sizeof(title_size);
+	key_msg_.title_size = title_size;
+	int size = title_size;
+
+	char message_title[64];
+	memcpy(&message_title, &buf[offset+message_offset], sizeof(char) * size);
+	message_offset += size;
+	message_title[size] = '\0';
+	strcpy(key_msg_.message_title,message_title);
+
+	char text_message[64];
+	memcpy(&text_message, &buf[offset+message_offset], len - message_offset);
+	text_message[254] = '\0';
+	strcpy(key_msg_.text_message,text_message);
+}
+
 void RobotState::unpackRobotMode(uint8_t * buf, unsigned int offset) {
 	memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp));
 	offset += sizeof(robot_mode_.timestamp);
@@ -322,6 +353,17 @@ double RobotState::getVersion() {
 
 }
 
+key_message RobotState::getKeyMessage(){
+		key_message new_key_msg;
+		val_lock_.lock();
+		strcpy(new_key_msg.message_title,key_msg_.message_title);
+		strcpy(new_key_msg.text_message,key_msg_.text_message);
+		memset(key_msg_.message_title,'\0',64);
+		memset(key_msg_.text_message,'\0',64);
+		val_lock_.unlock();
+		return new_key_msg;
+}
+
 void RobotState::finishedReading() {
 	new_data_available_ = false;
 }

src/ur_communication.cpp

@@ -40,7 +40,7 @@ UrCommunication::UrCommunication(std::condition_variable& msg_cond,
 	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);
+	sec_serv_addr_.sin_port = htons(30001);
 	flag_ = 1;
 	setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
 			sizeof(int));

src/ur_driver.cpp

@@ -19,23 +19,22 @@
 #include "ur_modern_driver/ur_driver.h"
 
 UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
-		std::condition_variable& msg_cond, std::string host,
-		unsigned int reverse_port, double servoj_time,
-		unsigned int safety_count_max, double max_time_step, double min_payload,
-		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),
-    host_(host) {
+	std::condition_variable& msg_cond, std::string host,
+	unsigned int reverse_port, double servoj_time,
+	unsigned int safety_count_max, double max_time_step, double min_payload,
+	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),
+	host_(host) {
 	char buffer[256];
 	struct sockaddr_in serv_addr;
 	int n, flag;
 
 	firmware_version_ = 0;
 	reverse_connected_ = false;
 	executing_traj_ = false;
-	rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host,
-			safety_count_max);
+	rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, safety_count_max);
 	new_sockfd_ = -1;
 	sec_interface_ = new UrCommunication(msg_cond, host);
 
@@ -154,6 +153,7 @@ bool UrDriver::uploadProg() {
 	sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_);
 	cmd_str += buf;
 
+	cmd_str += "\tposition_deviation_warning(True,0.1)\n";
 	cmd_str += "\tSERVO_IDLE = 0\n";
 	cmd_str += "\tSERVO_RUNNING = 1\n";
 	cmd_str += "\tcmd_servo_state = SERVO_IDLE\n";
@@ -301,12 +301,12 @@ void UrDriver::setDigitalOut(unsigned int n, bool b) {
 	if (firmware_version_ < 2) {
 		sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n,
 				b ? "True" : "False");
-    } else if (n > 15) {
-        sprintf(buf,
-                "sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n",
-                n - 16, b ? "True" : "False");
+	} else if (n > 15) {
+		sprintf(buf,
+				"sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n",
+				n - 16, b ? "True" : "False");
 	} else if (n > 7) {
-        sprintf(buf, "sec setOut():\n\tset_configurable_digital_out(%d, %s)\nend\n",
+		sprintf(buf, "sec setOut():\n\tset_configurable_digital_out(%d, %s)\nend\n",
 				n - 8, b ? "True" : "False");
 
 	} else {