Added catch in case communication fails

include/ur_modern_driver/ur_driver.h

@@ -46,7 +46,7 @@ public:
 			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();
+	bool start();
 	void halt();
 
 	void setSpeed(double q0, double q1, double q2, double q3, double q4,

include/ur_modern_driver/ur_realtime_communication.h

@@ -59,7 +59,7 @@ public:
 
 	UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host,
 			unsigned int safety_count_max = 12);
-	void start();
+	bool start();
 	void halt();
 	void setSpeed(double q0, double q1, double q2, double q3, double q4,
 			double q5, double acc = 100.);

src/ur_driver.cpp

@@ -296,18 +296,21 @@ void UrDriver::uploadProg() {
 
 }
 
-void UrDriver::start() {
-	if (sec_interface_->start()) {
+bool UrDriver::start() {
+	if (!sec_interface_->start())
+		return false;
 	rt_interface_->robot_state_->setVersion(
 			sec_interface_->robot_state_->getVersion());
-		rt_interface_->start();
+	if (!rt_interface_->start())
+		return false;
 	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
-	}
+	return true;
+
 }
 
 void UrDriver::halt() {

src/ur_realtime_communication.cpp

@@ -49,7 +49,7 @@ UrRealtimeCommunication::UrRealtimeCommunication(
 	safety_count_max_ = safety_count_max;
 }
 
-void UrRealtimeCommunication::start() {
+bool UrRealtimeCommunication::start() {
 	fd_set writefds;
 	struct timeval timeout;
 
@@ -71,10 +71,12 @@ void UrRealtimeCommunication::start() {
 	if (flag_ < 0) {
 #ifdef ROS_BUILD
 		ROS_FATAL("Error connecting to RT port 30003 - errno: %d (%s)", flag_, strerror(flag_));
+
 #else
 		printf("Error connecting to RT port 30003 - errno: %d (%s)\n", flag_,
 				strerror(flag_));
 #endif
+		return false;
 	}
 	sockaddr_in name;
 	socklen_t namelen = sizeof(name);
@@ -86,11 +88,14 @@ void UrRealtimeCommunication::start() {
 		printf("Could not get local IP - errno: %d (%s)", errno,
 				strerror(errno));
 #endif
+		close(sockfd_);
+		return false;
 	}
 	char str[18];
 	inet_ntop(AF_INET, &name.sin_addr, str, 18);
 	local_ip_ = str;
 	comThread_ = std::thread(&UrRealtimeCommunication::run, this);
+	return true;
 }
 
 void UrRealtimeCommunication::halt() {

src/ur_ros_wrapper.cpp

@@ -82,7 +82,8 @@ public:
 		joint_names.push_back(joint_prefix + "wrist_3_joint");
 		robot_.setJointNames(joint_names);
 
-		hardware_interface_.reset(new ros_control_ur::UrHardwareInterface(nh_, &robot_));
+		hardware_interface_.reset(
+				new ros_control_ur::UrHardwareInterface(nh_, &robot_));
 		controller_manager_.reset(
 				new controller_manager::ControllerManager(
 						hardware_interface_.get(), nh_));
@@ -110,7 +111,7 @@ public:
 					min_payload, max_payload);
 		}
 
-		robot_.start();
+		if (robot_.start()) {
 
 			//register the goal and feedback callbacks
 			//as_.registerGoalCallback(boost::bind(&RosWrapper::goalCB, this));
@@ -124,8 +125,8 @@ public:
 			urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1,
 					&RosWrapper::urscriptInterface, this);
 
-		io_srv_ = nh_.advertiseService("ur_driver/set_io", &RosWrapper::setIO,
-				this);
+			io_srv_ = nh_.advertiseService("ur_driver/set_io",
+					&RosWrapper::setIO, this);
 			payload_srv_ = nh_.advertiseService("ur_driver/set_payload",
 					&RosWrapper::setPayload, this);
 
@@ -137,6 +138,7 @@ public:
 					boost::bind(&RosWrapper::publishMbMsg, this));
 			ROS_DEBUG("The action server for this driver has been started");
 		}
+	}
 
 	void halt() {
 		robot_.halt();
@@ -365,7 +367,9 @@ private:
 			hardware_interface_->read();
 			robot_.rt_interface_->robot_state_->setControllerUpdated();
 			// Control
-			controller_manager_->update(ros::Time(current_time.tv_sec, current_time.tv_nsec), elapsed_time);
+			controller_manager_->update(
+					ros::Time(current_time.tv_sec, current_time.tv_nsec),
+					elapsed_time);
 
 			// Output
 			hardware_interface_->write();
@@ -413,8 +417,8 @@ private:
 	}
 
 	void publishMbMsg() {
-		ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>("ur_driver/io_states",
-				1);
+		ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>(
+				"ur_driver/io_states", 1);
 
 		while (ros::ok()) {
 			ur_msgs::IOStates io_msg;