Fixed a bug related to malformed RT messages in 3.0.

updated readme

README.md

@@ -16,5 +16,32 @@ The driver is designed to be a drop-in replacement of the ur\_driver package. It
 
 Just use the modified launch files included in this package instead of those in ur\_bringup. Everything else should work as usual.
 
+---
+
+__Improvements__
+
+
+The driver exposes the same functionality as the previous ur\_driver:
+*Action interface on _/follow\_joint\_trajectory_ for seamless integration with MoveIt
+*Publishes robot joint state on _/joint\_states_
+*Publishes TCP force on state on _/wrench_
+*Publishes IO states state on _/io\_states_
+*Service call to set outputs and payload (Note: I am not sure if setting the payload actually works, as the robot GUI does not update. This is also true for the old ur\_driver  )
+
+Besides this, the driver subscribes to two new topics:
+*/ur\_driver/URScript : takes messages of type _std\_msgs/String_ and directly forwards it to the robot. Note that no control is done on the input, so use at your own risk! Inteded for sending movel/movej commands directly to the robot
+*/joint\_speed : takes messages of type trajectory\_msgs/JointTrajectory, parses the first JointTracetoryPoint and sends the specified joint speeds and accelerations to the robot. Remember to set values for all 6 joints. Ignores the field joint\_names, so set the values in the correct order.
+
+
+This driver is written in c++, which should make it possible to integrate it with ros_control. If you fell like undertaking this task, please dive right in. I don't have the posibility to do this.
+
+No script is sent to the robot. This means that the teach pendant can be used to move the robot around while the driver is running.
+
+---
+Should be compatible with all robots and control boxes with the newest firmware.
+Tested with:
+*Simulated UR3 running 3.1.18024
+*Real UR10 with CB2 running 1.8.14035
+*Real UR5 with CB2 running 1.8.14035
 
  

include/ur_modern_driver/robot_state_RT.h

@@ -17,6 +17,7 @@
 #include <stdlib.h>
 #include <string.h>
 #include <mutex>
+#include <netinet/in.h>
 #include <condition_variable>
 
 class RobotStateRT {

src/robot_state_RT.cpp

@@ -299,20 +299,16 @@ void RobotStateRT::unpack(uint8_t * buf) {
 	uint64_t unpack_to;
 	uint16_t offset = 0;
 	val_lock_.lock();
-	if (version_ == 0.0) {
+	int len;
-		uint32_t len;
 	memcpy(&len, &buf[offset], sizeof(len));
-		if (len <= 756) {
+
-			version_ = 1.6;
+	offset += sizeof(len);
-		} else if (len <= 764) {
+	len = ntohl(len);
-			version_ = 1.7;
+	if (len > 1500) {
-		} else if (len <= 812) {
+		//In 3.0, every 3rd? package is malformed...?
-			version_ = 1.8;
+		val_lock_.unlock();
-		} else if (len <= 1044) {
+		return;
-			version_ = 3.0;
 	}
-	}
-	offset += 4;
 	memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
 	time_ = RobotStateRT::ntohd(unpack_to);
 	offset += sizeof(double);

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_DEBUG("Firmware version detected: %f", robot_state_->getVersion());
+	ROS_INFO("Firmware version detected: %f", robot_state_->getVersion());
 #else
 	printf("Firmware version detected: %f\n", robot_state_->getVersion());
 #endif