Implemented communication on port 30001 and 30002 - untested

CMakeLists.txt

@@ -146,8 +146,10 @@ include_directories(include
 ## Declare a C++ executable
 set(${PROJECT_NAME}_SOURCES
     src/ur_ros_wrapper.cpp
-    src/ur_realtime_communication.cpp
     src/ur_driver.cpp
+    src/ur_realtime_communication.cpp
+    src/ur_communication.cpp
+    src/robot_state.cpp
     src/robot_state_RT.cpp)
 add_executable(ur_driver ${${PROJECT_NAME}_SOURCES})
 

include/ur_modern_driver/robot_state_RT.h

@@ -94,6 +94,9 @@ public:
 	double getVRobot();
 	double getIRobot();
 	bool getNewDataAvailable();
+
+	void setVersion(double ver);
+
 	void finishedReading();
 	std::vector<double> getVActual();
 	void unpack(uint8_t * buf);

include/ur_modern_driver/ur_driver.h

@@ -15,6 +15,7 @@
 #include <mutex>
 #include <condition_variable>
 #include "ur_realtime_communication.h"
+#include "ur_communication.h"
 #include <vector>
 #include <math.h>
 #include <string>
@@ -27,8 +28,9 @@ private:
 	std::vector<std::string> joint_names_;
 public:
 	UrRealtimeCommunication* rt_interface_;
+	UrCommunication* sec_interface_;
 
-	UrDriver(std::condition_variable& msg_cond, std::string host,
+	UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host,
 			unsigned int safety_count_max = 12, double max_time_step = 0.08,
 			double min_payload = 0., double max_payload = 1.);
 	void start();

src/robot_state_RT.cpp

@@ -85,6 +85,12 @@ std::vector<bool> RobotStateRT::unpackDigitalInputBits(int64_t data) {
 	return ret;
 }
 
+void RobotStateRT::setVersion(double ver) {
+	val_lock_.lock();
+	version_ = ver;
+	val_lock_.unlock();
+}
+
 double RobotStateRT::getVersion() {
 	double ret;
 	val_lock_.lock();

src/ur_driver.cpp

@@ -11,13 +11,14 @@
 
 #include "ur_modern_driver/ur_driver.h"
 
-UrDriver::UrDriver(std::condition_variable& msg_cond, std::string host,
+UrDriver::UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host,
 		unsigned int safety_count_max, double max_time_step, double min_payload,
 		double max_payload) :
 		maximum_time_step_(max_time_step), minimum_payload_(min_payload), maximum_payload_(
 				max_payload) {
 	rt_interface_ = new UrRealtimeCommunication(msg_cond, host,
 			safety_count_max);
+	sec_interface_ = new UrCommunication(msg_cond, host);
 
 }
 
@@ -96,10 +97,13 @@ void UrDriver::stopTraj() {
 }
 
 void UrDriver::start() {
+	sec_interface_->start();
+	rt_interface_->robot_state_->setVersion(sec_interface_->robot_state_->getVersion());
 	rt_interface_->start();
 }
 
 void UrDriver::halt() {
+	sec_interface_->halt();
 	rt_interface_->halt();
 }
 

src/ur_realtime_communication.cpp

@@ -43,8 +43,8 @@ void UrRealtimeCommunication::start() {
 	keepalive_ = true;
 	if (connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_))
 			< 0)
-		printf("Error connecting");
-	printf("connecting...\n");
+		printf("Error connecting to RT port 30003");
+	printf("Realtime port: Connecting...\n");
 	comThread_ = std::thread(&UrRealtimeCommunication::run, this);
 }
 
@@ -78,7 +78,7 @@ void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
 void UrRealtimeCommunication::run() {
 	uint8_t buf[2048];
 	bzero(buf, 2048);
-	printf("Got connection\n");
+	printf("Realtime port: Got connection\n");
 	connected_ = true;
 	while (keepalive_) {
 		read(sockfd_, buf, 2048);

src/ur_ros_wrapper.cpp

@@ -35,6 +35,7 @@
 class RosWrapper {
 protected:
 	UrDriver robot_;
+	std::condition_variable rt_msg_cond_;
 	std::condition_variable msg_cond_;
 	ros::NodeHandle nh_;
 	actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_;
@@ -51,7 +52,7 @@ protected:
 
 public:
 	RosWrapper(std::string host) :
-			as_(nh_, "follow_joint_trajectory", false), robot_(msg_cond_, host), io_flag_delay_(
+			as_(nh_, "follow_joint_trajectory", false), robot_(rt_msg_cond_, msg_cond_, host), io_flag_delay_(
 					0.05), joint_offsets_(6, 0.0) {
 
 		std::string joint_prefix = "";
@@ -327,7 +328,7 @@ private:
 			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);
 			while (!robot_.rt_interface_->robot_state_->getNewDataAvailable()) {
-				msg_cond_.wait(locker);
+				rt_msg_cond_.wait(locker);
 			}
 			joint_msg.header.stamp = ros::Time::now();
 			joint_msg.position =