Added catch in case communication fails

src/ur_driver.cpp

@@ -296,18 +296,21 @@ void UrDriver::uploadProg() {
 
 }
 
-void UrDriver::start() {
-	if (sec_interface_->start()) {
-		rt_interface_->robot_state_->setVersion(
-				sec_interface_->robot_state_->getVersion());
-		rt_interface_->start();
-		ip_addr_ = rt_interface_->getLocalIp(); //inet_ntoa(serv_addr.sin_addr);
+bool UrDriver::start() {
+	if (!sec_interface_->start())
+		return false;
+	rt_interface_->robot_state_->setVersion(
+			sec_interface_->robot_state_->getVersion());
+	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_);
+			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_);
+	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,32 +111,33 @@ public:
 					min_payload, max_payload);
 		}
 
-		robot_.start();
+		if (robot_.start()) {
 
-		//register the goal and feedback callbacks
-		//as_.registerGoalCallback(boost::bind(&RosWrapper::goalCB, this));
-		//as_.registerPreemptCallback(boost::bind(&RosWrapper::preemptCB, this));
+			//register the goal and feedback callbacks
+			//as_.registerGoalCallback(boost::bind(&RosWrapper::goalCB, this));
+			//as_.registerPreemptCallback(boost::bind(&RosWrapper::preemptCB, this));
 
-		//as_.start();
+			//as_.start();
 
-		//subscribe to the data topic of interest
-		speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1,
-				&RosWrapper::speedInterface, this);
-		urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1,
-				&RosWrapper::urscriptInterface, this);
+			//subscribe to the data topic of interest
+			speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1,
+					&RosWrapper::speedInterface, this);
+			urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1,
+					&RosWrapper::urscriptInterface, this);
 
-		io_srv_ = nh_.advertiseService("ur_driver/set_io", &RosWrapper::setIO,
-				this);
-		payload_srv_ = nh_.advertiseService("ur_driver/set_payload",
-				&RosWrapper::setPayload, this);
+			io_srv_ = nh_.advertiseService("ur_driver/set_io",
+					&RosWrapper::setIO, this);
+			payload_srv_ = nh_.advertiseService("ur_driver/set_payload",
+					&RosWrapper::setPayload, this);
 
-		ros_control_thread_ = new std::thread(
-				boost::bind(&RosWrapper::rosControlLoop, this));
-		/*rt_publish_thread_ = new std::thread(
-				boost::bind(&RosWrapper::publishRTMsg, this)); */
-		mb_publish_thread_ = new std::thread(
-				boost::bind(&RosWrapper::publishMbMsg, this));
-		ROS_DEBUG("The action server for this driver has been started");
+			ros_control_thread_ = new std::thread(
+					boost::bind(&RosWrapper::rosControlLoop, this));
+			/*rt_publish_thread_ = new std::thread(
+			 boost::bind(&RosWrapper::publishRTMsg, this)); */
+			mb_publish_thread_ = new std::thread(
+					boost::bind(&RosWrapper::publishMbMsg, this));
+			ROS_DEBUG("The action server for this driver has been started");
+		}
 	}
 
 	void 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;