From 1b995a928b251070edc80afaa6332775166c487e Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Wed, 16 Sep 2015 14:06:39 +0200 Subject: [PATCH] ROSify output based on compiler flag --- CMakeLists.txt | 2 +- include/ur_modern_driver/ur_communication.h | 4 + .../ur_realtime_communication.h | 5 +- src/robot_state.cpp | 1 - src/ur_communication.cpp | 87 +++++++++++++++---- src/ur_realtime_communication.cpp | 35 ++++++-- src/ur_ros_wrapper.cpp | 4 +- 7 files changed, 107 insertions(+), 31 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 846a846..f77beb0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 2.8.3) project(ur_modern_driver) - +add_definitions( -DROS_BUILD ) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages diff --git a/include/ur_modern_driver/ur_communication.h b/include/ur_modern_driver/ur_communication.h index bf97f7c..44d9b54 100644 --- a/include/ur_modern_driver/ur_communication.h +++ b/include/ur_modern_driver/ur_communication.h @@ -30,6 +30,10 @@ #include #include +#ifdef ROS_BUILD +#include +#endif + class UrCommunication { private: int pri_sockfd_, sec_sockfd_; diff --git a/include/ur_modern_driver/ur_realtime_communication.h b/include/ur_modern_driver/ur_realtime_communication.h index 1d15e96..77c6873 100644 --- a/include/ur_modern_driver/ur_realtime_communication.h +++ b/include/ur_modern_driver/ur_realtime_communication.h @@ -29,9 +29,12 @@ #include #include +#ifdef ROS_BUILD +#include +#endif + class UrRealtimeCommunication { private: - const double SAMPLETIME_; unsigned int safety_count_max_; int sockfd_; struct sockaddr_in serv_addr_; diff --git a/src/robot_state.cpp b/src/robot_state.cpp index 0fcd777..a5677e8 100644 --- a/src/robot_state.cpp +++ b/src/robot_state.cpp @@ -143,7 +143,6 @@ void RobotState::unpackRobotStateMasterboard(uint8_t * buf, offset += sizeof(mb_data_.digitalOutputBits); mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits); } - printf("output bits: %X\n", mb_data_.digitalOutputBits); memcpy(&mb_data_.analogInputRange0, &buf[offset], sizeof(mb_data_.analogInputRange0)); diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp index aac4925..a5a2d3f 100644 --- a/src/ur_communication.cpp +++ b/src/ur_communication.cpp @@ -18,18 +18,33 @@ UrCommunication::UrCommunication(std::condition_variable& msg_cond, bzero((char *) &sec_serv_addr_, sizeof(sec_serv_addr_)); pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); if (pri_sockfd_ < 0) { +#ifdef ROS_BUILD + ROS_FATAL("ERROR opening socket"); + ros::shutdown(); +#else printf("ERROR opening socket"); - exit(0); + exit(1); +#endif } sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); if (sec_sockfd_ < 0) { +#ifdef ROS_BUILD + ROS_FATAL("ERROR opening socket"); + ros::shutdown(); +#else printf("ERROR opening socket"); - exit(0); + exit(1); +#endif } server_ = gethostbyname(host.c_str()); if (server_ == NULL) { +#ifdef ROS_BUILD + ROS_FATAL("ERROR, no such host"); + ros::shutdown(); +#else printf("ERROR, no such host\n"); - exit(0); + exit(1); +#endif } pri_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); sec_serv_addr_.sin_port = htons(30002); flag_ = 1; - setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); - setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); - setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); + setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, + sizeof(int)); + setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, + 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; keepalive_ = false; } @@ -52,29 +71,60 @@ void UrCommunication::start() { unsigned int bytes_read; std::string cmd; bzero(buf, 512); - +#ifdef ROS_BUILD + ROS_DEBUG("Acquire firmware version: Connecting..."); +#else printf("Acquire firmware version: Connecting...\n"); +#endif if (connect(pri_sockfd_, (struct sockaddr *) &pri_serv_addr_, sizeof(pri_serv_addr_)) < 0) { - printf("Error connecting"); +#ifdef ROS_BUILD + ROS_FATAL("Error connecting"); + ros::shutdown(); +#else + printf("Error connecting\n"); exit(1); +#endif } +#ifdef ROS_BUILD + ROS_DEBUG("Acquire firmware version: Got connection"); +#else printf("Acquire firmware version: Got connection\n"); +#endif 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); //wait for some traffic so the UR socket doesn't die in version 3.1. 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_); - printf("Switching to secondary interface for masterboard data: Connecting...\n"); // which generates less network traffic - if (connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_, - sizeof(sec_serv_addr_)) < 0) { - printf("Error connecting"); - exit(1); - } - printf("Secondary interface: Got connection\n"); +#ifdef ROS_BUILD + ROS_DEBUG("Switching to secondary interface for masterboard data: Connecting..."); +#else + printf( + "Switching to secondary interface for masterboard data: Connecting...\n"); // which generates less network traffic +#endif + 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); } @@ -96,7 +146,6 @@ void UrCommunication::run() { uint8_t buf[2048]; unsigned int bytes_read; bzero(buf, 2048); - printf("Got connection\n"); connected_ = true; while (keepalive_) { bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp index 99b1348..8e66b4d 100644 --- a/src/ur_realtime_communication.cpp +++ b/src/ur_realtime_communication.cpp @@ -13,19 +13,28 @@ UrRealtimeCommunication::UrRealtimeCommunication( std::condition_variable& msg_cond, std::string host, - unsigned int safety_count_max) : - SAMPLETIME_(0.008) { + unsigned int safety_count_max) { robot_state_ = new RobotStateRT(msg_cond); bzero((char *) &serv_addr_, sizeof(serv_addr_)); sockfd_ = socket(AF_INET, SOCK_STREAM, 0); if (sockfd_ < 0) { +#ifdef ROS_BUILD + ROS_FATAL("ERROR opening socket"); + ros::shutdown(); +#else printf("ERROR opening socket"); - exit(0); + exit(1); +#endif } server_ = gethostbyname(host.c_str()); if (server_ == NULL) { +#ifdef ROS_BUILD + ROS_FATAL("ERROR, no such host"); + ros::shutdown(); +#else printf("ERROR, no such host\n"); - exit(0); + exit(1); +#endif } serv_addr_.sin_family = AF_INET; 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)); connected_ = false; keepalive_ = false; - safety_count_ = safety_count_max+1; + safety_count_ = safety_count_max + 1; safety_count_max_ = safety_count_max; } void UrRealtimeCommunication::start() { 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_)) < 0) - printf("Error connecting to RT port 30003"); - printf("Realtime port: Connecting...\n"); +#ifdef ROS_BUILD + ROS_FATAL("Error connecting to RT port 30003"); +#else + printf("Error connecting to RT port 30003\n"); +#endif comThread_ = std::thread(&UrRealtimeCommunication::run, this); } @@ -78,7 +95,11 @@ void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2, void UrRealtimeCommunication::run() { uint8_t buf[2048]; bzero(buf, 2048); +#ifdef ROS_BUILD + ROS_DEBUG("Realtime port: Got connection"); +#else printf("Realtime port: Got connection\n"); +#endif connected_ = true; while (keepalive_) { read(sockfd_, buf, 2048); diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 8de6ab6..0079f76 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -88,11 +88,11 @@ public: double min_payload = 0.; double max_payload = 1.; 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); } 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); } robot_.setMinPayload(min_payload);