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();
void finishedReading();
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 unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
uint32_t len);

3
include/ur_modern_driver/ur_communication.h
View File

@@ -42,8 +42,6 @@ private:
bool keepalive_;
std::thread comThread_;
int flag_;
std::recursive_mutex command_string_lock_;
std::string command_;
void run();
public:
@@ -53,7 +51,6 @@ public:
UrCommunication(std::condition_variable& msg_cond, std::string host);
void start();
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));
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 */
unsigned int offset = 0;
while (buf_length > offset) {
@@ -28,7 +28,7 @@ int RobotState::unpack(uint8_t* buf, unsigned int buf_length) {
memcpy(&len, &buf[offset], sizeof(len));
len = ntohl(len);
if (len + offset > buf_length) {
return (len + offset - buf_length);
return ;
}
memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type));
switch (message_type) {
@@ -46,7 +46,7 @@ int RobotState::unpack(uint8_t* buf, unsigned int buf_length) {
offset += len;
}
return 0;
return;
}
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();
}
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() {
uint8_t buf[2048];
unsigned int bytes_read;
@@ -152,13 +143,6 @@ void UrCommunication::run() {
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
sizeof(int));
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.
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) {
int bytes_written;
if (inp.back() != '\n') {
inp.append("\n");
}
write(sockfd_, inp.c_str(), inp.length());
/*command_string_lock_.lock();
command_ += inp;
command_string_lock_.unlock();*/
bytes_written = write(sockfd_, inp.c_str(), inp.length());
}
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() {
uint8_t buf[2048];
int bytes_read;
bzero(buf, 2048);
#ifdef ROS_BUILD
ROS_DEBUG("Realtime port: Got connection");
@@ -104,26 +102,16 @@ void UrRealtimeCommunication::run() {
#endif
connected_ = true;
while (keepalive_) {
read(sockfd_, buf, 2048);
bytes_read = read(sockfd_, buf, 2048);
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
sizeof(int));
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_) {
setSpeed(0., 0., 0., 0., 0., 0.);
//write(sockfd_, command_.c_str(), command_.length());
//command_ = "";
}
safety_count_ += 1;
///command_string_lock_.unlock();
}
setSpeed(0., 0., 0., 0., 0., 0.);
write(sockfd_, command_.c_str(), command_.length());
close(sockfd_);
}