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

Removed commented code and non-RT socket write

Browse Source
This commit is contained in:
Thomas Timm Andersen
2015-09-17 13:59:42 +02:00
parent 0310193b15
commit 36c8b70ba0
5 changed files with 8 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
include/ur_modern_driver/robot_state.h
View File

@@ -133,7 +133,7 @@ public:
bool getNewDataAvailable(); bool getNewDataAvailable();
void finishedReading(); void finishedReading();
std::vector<double> getVActual(); std::vector<double> getVActual();
int unpack(uint8_t * buf, unsigned int buf_length); void unpack(uint8_t * buf, unsigned int buf_length);
void unpackRobotMessage(uint8_t * buf, unsigned int offset, uint32_t len); void unpackRobotMessage(uint8_t * buf, unsigned int offset, uint32_t len);
void unpackRobotMessageVersion(uint8_t * buf, unsigned int offset, void unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
uint32_t len); uint32_t len);

3
include/ur_modern_driver/ur_communication.h
View File

@@ -42,8 +42,6 @@ private:
bool keepalive_; bool keepalive_;
std::thread comThread_; std::thread comThread_;
int flag_; int flag_;
std::recursive_mutex command_string_lock_;
std::string command_;
void run(); void run();
public: public:
@@ -53,7 +51,6 @@ public:
UrCommunication(std::condition_variable& msg_cond, std::string host); UrCommunication(std::condition_variable& msg_cond, std::string host);
void start(); void start();
void halt(); void halt();
void addCommandToQueue(std::string inp);
}; };

6
src/robot_state.cpp
View File

@@ -19,7 +19,7 @@ double RobotState::ntohd(uint64_t nf) {
memcpy(&x, &nf, sizeof(x)); memcpy(&x, &nf, sizeof(x));
return x; return x;
} }
int RobotState::unpack(uint8_t* buf, unsigned int buf_length) { void RobotState::unpack(uint8_t* buf, unsigned int buf_length) {
/* Returns missing bytes to unpack a message, or 0 if all data was parsed */ /* Returns missing bytes to unpack a message, or 0 if all data was parsed */
unsigned int offset = 0; unsigned int offset = 0;
while (buf_length > offset) { while (buf_length > offset) {
@@ -28,7 +28,7 @@ int RobotState::unpack(uint8_t* buf, unsigned int buf_length) {
memcpy(&len, &buf[offset], sizeof(len)); memcpy(&len, &buf[offset], sizeof(len));
len = ntohl(len); len = ntohl(len);
if (len + offset > buf_length) { if (len + offset > buf_length) {
return (len + offset - buf_length); return ;
} }
memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type)); memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type));
switch (message_type) { switch (message_type) {
@@ -46,7 +46,7 @@ int RobotState::unpack(uint8_t* buf, unsigned int buf_length) {
offset += len; offset += len;
} }
return 0; return;
} }
void RobotState::unpackRobotMessage(uint8_t * buf, unsigned int offset, void RobotState::unpackRobotMessage(uint8_t * buf, unsigned int offset,

16
src/ur_communication.cpp
View File

@@ -133,15 +133,6 @@ void UrCommunication::halt() {
comThread_.join(); comThread_.join();
} }
void UrCommunication::addCommandToQueue(std::string inp) {
if (inp.back() != '\n') {
inp.append("\n");
}
command_string_lock_.lock();
command_ += inp;
command_string_lock_.unlock();
}
void UrCommunication::run() { void UrCommunication::run() {
uint8_t buf[2048]; uint8_t buf[2048];
unsigned int bytes_read; unsigned int bytes_read;
@@ -152,13 +143,6 @@ void UrCommunication::run() {
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
sizeof(int)); sizeof(int));
robot_state_->unpack(buf, bytes_read); robot_state_->unpack(buf, bytes_read);
command_string_lock_.lock();
if (command_.length() != 0) {
write(sec_sockfd_, command_.c_str(), command_.length());
command_ = "";
}
command_string_lock_.unlock();
} }
//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));

20
src/ur_realtime_communication.cpp
View File

@@ -71,14 +71,11 @@ void UrRealtimeCommunication::halt() {
} }
void UrRealtimeCommunication::addCommandToQueue(std::string inp) { void UrRealtimeCommunication::addCommandToQueue(std::string inp) {
int bytes_written;
if (inp.back() != '\n') { if (inp.back() != '\n') {
inp.append("\n"); inp.append("\n");
} }
write(sockfd_, inp.c_str(), inp.length()); bytes_written = write(sockfd_, inp.c_str(), inp.length());
/*command_string_lock_.lock();
command_ += inp;
command_string_lock_.unlock();*/
} }
void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2, void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
@@ -96,6 +93,7 @@ void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
void UrRealtimeCommunication::run() { void UrRealtimeCommunication::run() {
uint8_t buf[2048]; uint8_t buf[2048];
int bytes_read;
bzero(buf, 2048); bzero(buf, 2048);
#ifdef ROS_BUILD #ifdef ROS_BUILD
ROS_DEBUG("Realtime port: Got connection"); ROS_DEBUG("Realtime port: Got connection");
@@ -104,26 +102,16 @@ void UrRealtimeCommunication::run() {
#endif #endif
connected_ = true; connected_ = true;
while (keepalive_) { while (keepalive_) {
read(sockfd_, buf, 2048); bytes_read = read(sockfd_, buf, 2048);
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
sizeof(int)); sizeof(int));
robot_state_->unpack(buf); robot_state_->unpack(buf);
/*command_string_lock_.lock();
if (command_.length() != 0) {
write(sockfd_, command_.c_str(), command_.length());
command_ = "";
}*/
if (safety_count_ == safety_count_max_) { if (safety_count_ == safety_count_max_) {
setSpeed(0., 0., 0., 0., 0., 0.); setSpeed(0., 0., 0., 0., 0., 0.);
//write(sockfd_, command_.c_str(), command_.length());
//command_ = "";
} }
safety_count_ += 1; safety_count_ += 1;
///command_string_lock_.unlock();
} }
setSpeed(0., 0., 0., 0., 0., 0.); setSpeed(0., 0., 0., 0., 0., 0.);
write(sockfd_, command_.c_str(), command_.length());
close(sockfd_); close(sockfd_);
} }