Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 01:50:46 +02:00
Code Releases Activity

Added catch in case communication fails

Browse Source
This commit is contained in:
Thomas Timm Andersen
2015-09-24 16:26:19 +02:00
parent ef7acd55b6
commit 6f3bd5d06f
5 changed files with 49 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
include/ur_modern_driver/ur_driver.h
View File

@@ -46,7 +46,7 @@ public:
unsigned int reverse_port = 50007, unsigned int safety_count_max = unsigned int reverse_port = 50007, unsigned int safety_count_max =
12, double max_time_step = 0.08, double min_payload = 0., 12, double max_time_step = 0.08, double min_payload = 0.,
double max_payload = 1.); double max_payload = 1.);
void start(); bool start();
void halt(); void halt();
void setSpeed(double q0, double q1, double q2, double q3, double q4, void setSpeed(double q0, double q1, double q2, double q3, double q4,

2
include/ur_modern_driver/ur_realtime_communication.h
View File

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

21
src/ur_driver.cpp
View File

@@ -296,18 +296,21 @@ void UrDriver::uploadProg() {
} }
void UrDriver::start() { bool UrDriver::start() {
if (sec_interface_->start()) { if (!sec_interface_->start())
rt_interface_->robot_state_->setVersion( return false;
sec_interface_->robot_state_->getVersion()); rt_interface_->robot_state_->setVersion(
rt_interface_->start(); sec_interface_->robot_state_->getVersion());
ip_addr_ = rt_interface_->getLocalIp(); //inet_ntoa(serv_addr.sin_addr); if (!rt_interface_->start())
return false;
ip_addr_ = rt_interface_->getLocalIp(); //inet_ntoa(serv_addr.sin_addr);
#ifdef ROS_BUILD #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 #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 #endif
} return true;
} }
void UrDriver::halt() { void UrDriver::halt() {

7
src/ur_realtime_communication.cpp
View File

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

54
src/ur_ros_wrapper.cpp
View File

@@ -82,7 +82,8 @@ public:
joint_names.push_back(joint_prefix + "wrist_3_joint"); joint_names.push_back(joint_prefix + "wrist_3_joint");
robot_.setJointNames(joint_names); 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( controller_manager_.reset(
new controller_manager::ControllerManager( new controller_manager::ControllerManager(
hardware_interface_.get(), nh_)); hardware_interface_.get(), nh_));
@@ -110,32 +111,33 @@ public:
min_payload, max_payload); min_payload, max_payload);
} }
robot_.start(); if (robot_.start()) {
//register the goal and feedback callbacks //register the goal and feedback callbacks
//as_.registerGoalCallback(boost::bind(&RosWrapper::goalCB, this)); //as_.registerGoalCallback(boost::bind(&RosWrapper::goalCB, this));
//as_.registerPreemptCallback(boost::bind(&RosWrapper::preemptCB, this)); //as_.registerPreemptCallback(boost::bind(&RosWrapper::preemptCB, this));
//as_.start(); //as_.start();
//subscribe to the data topic of interest //subscribe to the data topic of interest
speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1, speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1,
&RosWrapper::speedInterface, this); &RosWrapper::speedInterface, this);
urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1,
&RosWrapper::urscriptInterface, this); &RosWrapper::urscriptInterface, this);
io_srv_ = nh_.advertiseService("ur_driver/set_io", &RosWrapper::setIO, io_srv_ = nh_.advertiseService("ur_driver/set_io",
this); &RosWrapper::setIO, this);
payload_srv_ = nh_.advertiseService("ur_driver/set_payload", payload_srv_ = nh_.advertiseService("ur_driver/set_payload",
&RosWrapper::setPayload, this); &RosWrapper::setPayload, this);
ros_control_thread_ = new std::thread( ros_control_thread_ = new std::thread(
boost::bind(&RosWrapper::rosControlLoop, this)); boost::bind(&RosWrapper::rosControlLoop, this));
/*rt_publish_thread_ = new std::thread( /*rt_publish_thread_ = new std::thread(
boost::bind(&RosWrapper::publishRTMsg, this)); */ boost::bind(&RosWrapper::publishRTMsg, this)); */
mb_publish_thread_ = new std::thread( mb_publish_thread_ = new std::thread(
boost::bind(&RosWrapper::publishMbMsg, this)); boost::bind(&RosWrapper::publishMbMsg, this));
ROS_DEBUG("The action server for this driver has been started"); ROS_DEBUG("The action server for this driver has been started");
}
} }
void halt() { void halt() {
@@ -365,7 +367,9 @@ private:
hardware_interface_->read(); hardware_interface_->read();
robot_.rt_interface_->robot_state_->setControllerUpdated(); robot_.rt_interface_->robot_state_->setControllerUpdated();
// Control // 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 // Output
hardware_interface_->write(); hardware_interface_->write();
@@ -413,8 +417,8 @@ private:
} }
void publishMbMsg() { void publishMbMsg() {
ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>("ur_driver/io_states", ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>(
1); "ur_driver/io_states", 1);
while (ros::ok()) { while (ros::ok()) {
ur_msgs::IOStates io_msg; ur_msgs::IOStates io_msg;