diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h index d6af6cd..30836a0 100644 --- a/include/ur_modern_driver/ur_driver.h +++ b/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, diff --git a/include/ur_modern_driver/ur_realtime_communication.h b/include/ur_modern_driver/ur_realtime_communication.h index 2a8be1f..f415172 100644 --- a/include/ur_modern_driver/ur_realtime_communication.h +++ b/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.); diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index 9a7bdd6..b82c0af 100644 --- a/src/ur_driver.cpp +++ b/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() { diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp index ebb2e6e..d785e5c 100644 --- a/src/ur_realtime_communication.cpp +++ b/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() { diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index e92f9e0..e20c77e 100644 --- a/src/ur_ros_wrapper.cpp +++ b/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_driver/io_states", - 1); + ros::Publisher io_pub = nh_.advertise( + "ur_driver/io_states", 1); while (ros::ok()) { ur_msgs::IOStates io_msg;