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

ROSify output based on compiler flag

Browse Source
This commit is contained in:
Thomas Timm Andersen
2015-09-16 14:06:39 +02:00
parent fb95d2188d
commit 1b995a928b
7 changed files with 107 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -1,6 +1,6 @@
cmake_minimum_required(VERSION 2.8.3) cmake_minimum_required(VERSION 2.8.3)
project(ur_modern_driver) project(ur_modern_driver)
add_definitions( -DROS_BUILD )
## Find catkin macros and libraries ## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages ## is used, also find other catkin packages

4
include/ur_modern_driver/ur_communication.h
View File

@@ -30,6 +30,10 @@
#include <unistd.h> #include <unistd.h>
#include <chrono> #include <chrono>
#ifdef ROS_BUILD
#include <ros/ros.h>
#endif
class UrCommunication { class UrCommunication {
private: private:
int pri_sockfd_, sec_sockfd_; int pri_sockfd_, sec_sockfd_;

5
include/ur_modern_driver/ur_realtime_communication.h
View File

@@ -29,9 +29,12 @@
#include <iostream> #include <iostream>
#include <unistd.h> #include <unistd.h>
#ifdef ROS_BUILD
#include <ros/ros.h>
#endif
class UrRealtimeCommunication { class UrRealtimeCommunication {
private: private:
const double SAMPLETIME_;
unsigned int safety_count_max_; unsigned int safety_count_max_;
int sockfd_; int sockfd_;
struct sockaddr_in serv_addr_; struct sockaddr_in serv_addr_;

1
src/robot_state.cpp
View File

@@ -143,7 +143,6 @@ void RobotState::unpackRobotStateMasterboard(uint8_t * buf,
offset += sizeof(mb_data_.digitalOutputBits); offset += sizeof(mb_data_.digitalOutputBits);
mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits); mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits);
} }
printf("output bits: %X\n", mb_data_.digitalOutputBits);
memcpy(&mb_data_.analogInputRange0, &buf[offset], memcpy(&mb_data_.analogInputRange0, &buf[offset],
sizeof(mb_data_.analogInputRange0)); sizeof(mb_data_.analogInputRange0));

87
src/ur_communication.cpp
View File

@@ -18,18 +18,33 @@ UrCommunication::UrCommunication(std::condition_variable& msg_cond,
bzero((char *) &sec_serv_addr_, sizeof(sec_serv_addr_)); bzero((char *) &sec_serv_addr_, sizeof(sec_serv_addr_));
pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (pri_sockfd_ < 0) { if (pri_sockfd_ < 0) {
#ifdef ROS_BUILD
ROS_FATAL("ERROR opening socket");
ros::shutdown();
#else
printf("ERROR opening socket"); printf("ERROR opening socket");
exit(0); exit(1);
#endif
} }
sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sec_sockfd_ < 0) { if (sec_sockfd_ < 0) {
#ifdef ROS_BUILD
ROS_FATAL("ERROR opening socket");
ros::shutdown();
#else
printf("ERROR opening socket"); printf("ERROR opening socket");
exit(0); exit(1);
#endif
} }
server_ = gethostbyname(host.c_str()); server_ = gethostbyname(host.c_str());
if (server_ == NULL) { if (server_ == NULL) {
#ifdef ROS_BUILD
ROS_FATAL("ERROR, no such host");
ros::shutdown();
#else
printf("ERROR, no such host\n"); printf("ERROR, no such host\n");
exit(0); exit(1);
#endif
} }
pri_serv_addr_.sin_family = AF_INET; pri_serv_addr_.sin_family = AF_INET;
sec_serv_addr_.sin_family = AF_INET; sec_serv_addr_.sin_family = AF_INET;
@@ -38,10 +53,14 @@ UrCommunication::UrCommunication(std::condition_variable& msg_cond,
pri_serv_addr_.sin_port = htons(30001); pri_serv_addr_.sin_port = htons(30001);
sec_serv_addr_.sin_port = htons(30002); sec_serv_addr_.sin_port = htons(30002);
flag_ = 1; flag_ = 1;
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
sizeof(int));
setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
sizeof(int));
connected_ = false; connected_ = false;
keepalive_ = false; keepalive_ = false;
} }
@@ -52,29 +71,60 @@ void UrCommunication::start() {
unsigned int bytes_read; unsigned int bytes_read;
std::string cmd; std::string cmd;
bzero(buf, 512); bzero(buf, 512);
#ifdef ROS_BUILD
ROS_DEBUG("Acquire firmware version: Connecting...");
#else
printf("Acquire firmware version: Connecting...\n"); printf("Acquire firmware version: Connecting...\n");
#endif
if (connect(pri_sockfd_, (struct sockaddr *) &pri_serv_addr_, if (connect(pri_sockfd_, (struct sockaddr *) &pri_serv_addr_,
sizeof(pri_serv_addr_)) < 0) { sizeof(pri_serv_addr_)) < 0) {
printf("Error connecting"); #ifdef ROS_BUILD
ROS_FATAL("Error connecting");
ros::shutdown();
#else
printf("Error connecting\n");
exit(1); exit(1);
#endif
} }
#ifdef ROS_BUILD
ROS_DEBUG("Acquire firmware version: Got connection");
#else
printf("Acquire firmware version: Got connection\n"); printf("Acquire firmware version: Got connection\n");
#endif
bytes_read = read(pri_sockfd_, buf, 512); bytes_read = read(pri_sockfd_, buf, 512);
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
sizeof(int));
robot_state_->unpack(buf, bytes_read); robot_state_->unpack(buf, bytes_read);
//wait for some traffic so the UR socket doesn't die in version 3.1. //wait for some traffic so the UR socket doesn't die in version 3.1.
std::this_thread::sleep_for(std::chrono::milliseconds(500)); std::this_thread::sleep_for(std::chrono::milliseconds(500));
printf("Firmware version: %f\n\n", robot_state_->getVersion()); #ifdef ROS_BUILD
ROS_DEBUG("Firmware version detected: %f", robot_state_->getVersion());
#else
printf("Firmware version detected: %f\n", robot_state_->getVersion());
#endif
close(pri_sockfd_); close(pri_sockfd_);
printf("Switching to secondary interface for masterboard data: Connecting...\n"); // which generates less network traffic #ifdef ROS_BUILD
if (connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_, ROS_DEBUG("Switching to secondary interface for masterboard data: Connecting...");
sizeof(sec_serv_addr_)) < 0) { #else
printf("Error connecting"); printf(
exit(1); "Switching to secondary interface for masterboard data: Connecting...\n"); // which generates less network traffic
} #endif
printf("Secondary interface: Got connection\n"); if (connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_,
sizeof(sec_serv_addr_)) < 0) {
#ifdef ROS_BUILD
ROS_FATAL("Error connecting");
ros::shutdown();
#else
printf("Error connecting\n");
exit(1);
#endif
}
#ifdef ROS_BUILD
ROS_DEBUG("Secondary interface: Got connection");
#else
printf("Secondary interface: Got connection\n");
#endif
comThread_ = std::thread(&UrCommunication::run, this); comThread_ = std::thread(&UrCommunication::run, this);
} }
@@ -96,7 +146,6 @@ void UrCommunication::run() {
uint8_t buf[2048]; uint8_t buf[2048];
unsigned int bytes_read; unsigned int bytes_read;
bzero(buf, 2048); bzero(buf, 2048);
printf("Got connection\n");
connected_ = true; connected_ = true;
while (keepalive_) { while (keepalive_) {
bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes

35
src/ur_realtime_communication.cpp
View File

@@ -13,19 +13,28 @@
UrRealtimeCommunication::UrRealtimeCommunication( UrRealtimeCommunication::UrRealtimeCommunication(
std::condition_variable& msg_cond, std::string host, std::condition_variable& msg_cond, std::string host,
unsigned int safety_count_max) : unsigned int safety_count_max) {
SAMPLETIME_(0.008) {
robot_state_ = new RobotStateRT(msg_cond); robot_state_ = new RobotStateRT(msg_cond);
bzero((char *) &serv_addr_, sizeof(serv_addr_)); bzero((char *) &serv_addr_, sizeof(serv_addr_));
sockfd_ = socket(AF_INET, SOCK_STREAM, 0); sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd_ < 0) { if (sockfd_ < 0) {
#ifdef ROS_BUILD
ROS_FATAL("ERROR opening socket");
ros::shutdown();
#else
printf("ERROR opening socket"); printf("ERROR opening socket");
exit(0); exit(1);
#endif
} }
server_ = gethostbyname(host.c_str()); server_ = gethostbyname(host.c_str());
if (server_ == NULL) { if (server_ == NULL) {
#ifdef ROS_BUILD
ROS_FATAL("ERROR, no such host");
ros::shutdown();
#else
printf("ERROR, no such host\n"); printf("ERROR, no such host\n");
exit(0); exit(1);
#endif
} }
serv_addr_.sin_family = AF_INET; serv_addr_.sin_family = AF_INET;
bcopy((char *) server_->h_addr, (char *)&serv_addr_.sin_addr.s_addr, server_->h_length); bcopy((char *) server_->h_addr, (char *)&serv_addr_.sin_addr.s_addr, server_->h_length);
@@ -35,16 +44,24 @@ UrRealtimeCommunication::UrRealtimeCommunication(
setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int));
connected_ = false; connected_ = false;
keepalive_ = false; keepalive_ = false;
safety_count_ = safety_count_max+1; safety_count_ = safety_count_max + 1;
safety_count_max_ = safety_count_max; safety_count_max_ = safety_count_max;
} }
void UrRealtimeCommunication::start() { void UrRealtimeCommunication::start() {
keepalive_ = true; keepalive_ = true;
#ifdef ROS_BUILD
ROS_DEBUG("Realtime port: Connecting...");
#else
printf("Realtime port: Connecting...\n");
#endif
if (connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_)) if (connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_))
< 0) < 0)
printf("Error connecting to RT port 30003"); #ifdef ROS_BUILD
printf("Realtime port: Connecting...\n"); ROS_FATAL("Error connecting to RT port 30003");
#else
printf("Error connecting to RT port 30003\n");
#endif
comThread_ = std::thread(&UrRealtimeCommunication::run, this); comThread_ = std::thread(&UrRealtimeCommunication::run, this);
} }
@@ -78,7 +95,11 @@ void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
void UrRealtimeCommunication::run() { void UrRealtimeCommunication::run() {
uint8_t buf[2048]; uint8_t buf[2048];
bzero(buf, 2048); bzero(buf, 2048);
#ifdef ROS_BUILD
ROS_DEBUG("Realtime port: Got connection");
#else
printf("Realtime port: Got connection\n"); printf("Realtime port: Got connection\n");
#endif
connected_ = true; connected_ = true;
while (keepalive_) { while (keepalive_) {
read(sockfd_, buf, 2048); read(sockfd_, buf, 2048);

4
src/ur_ros_wrapper.cpp
View File

@@ -88,11 +88,11 @@ public:
double min_payload = 0.; double min_payload = 0.;
double max_payload = 1.; double max_payload = 1.;
if (ros::param::get("~min_payload", min_payload)) { if (ros::param::get("~min_payload", min_payload)) {
ROS_DEBUG("Min payload accepted by ur_driver: %f [kg]", ROS_DEBUG("Min payload set to: %f [kg]",
min_payload); min_payload);
} }
if (ros::param::get("~max_payload", max_payload)) { if (ros::param::get("~max_payload", max_payload)) {
ROS_DEBUG("Max payload accepted by ur_driver: %f [kg]", ROS_DEBUG("Max payload set to: %f [kg]",
max_payload); max_payload);
} }
robot_.setMinPayload(min_payload); robot_.setMinPayload(min_payload);