mirror of
https://gitlab.com/obbart/universal_robots_ros_driver.git
synced 2026-04-10 10:00:48 +02:00
Implemented communication on port 30001 and 30002 - untested
This commit is contained in:
Thomas Timm Andersen
102
src/robot_state.cpp
Normal file
102
src/robot_state.cpp
Normal file
@@ -0,0 +1,102 @@
|
||||
/*
|
||||
* robot_state.cpp
|
||||
*
|
||||
* Created on: Sep 10, 2015
|
||||
* Author: ttan
|
||||
*/
|
||||
|
||||
#include "ur_modern_driver/robot_state.h"
|
||||
|
||||
RobotState::RobotState(std::condition_variable& msg_cond) {
|
||||
version_msg_.major_version = 0;
|
||||
version_msg_.minor_version = 0;
|
||||
new_data_available_ = false;
|
||||
pMsg_cond_ = &msg_cond;
|
||||
}
|
||||
|
||||
int 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) {
|
||||
uint32_t len;
|
||||
unsigned char message_type;
|
||||
memcpy(&len, &buf[offset], sizeof(len));
|
||||
if (len + offset > buf_length) {
|
||||
return (len + offset - buf_length);
|
||||
}
|
||||
memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type));
|
||||
|
||||
switch (message_type) {
|
||||
case messageType::ROBOT_MESSAGE:
|
||||
RobotState::unpackRobotMessage(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType
|
||||
break;
|
||||
case messageType::ROBOT_STATE:
|
||||
RobotState::unpackRobotState(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType
|
||||
break;
|
||||
case messageType::PROGRAM_STATE_MESSAGE:
|
||||
//Don't do anything atm...
|
||||
default:
|
||||
break;
|
||||
}
|
||||
offset += len;
|
||||
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
void RobotState::unpackRobotMessage(uint8_t * buf, unsigned int offset,
|
||||
uint32_t len) {
|
||||
offset += 5;
|
||||
uint64_t timestamp;
|
||||
int8_t source, robot_message_type;
|
||||
memcpy(×tamp, &buf[offset], sizeof(timestamp));
|
||||
offset += sizeof(timestamp);
|
||||
memcpy(&source, &buf[offset], sizeof(source));
|
||||
offset += sizeof(source);
|
||||
memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type));
|
||||
offset += sizeof(robot_message_type);
|
||||
val_lock_.lock();
|
||||
switch (robot_message_type) {
|
||||
case robotMessageType::ROBOT_MESSAGE_VERSION:
|
||||
version_msg_.timestamp = timestamp;
|
||||
version_msg_.source = source;
|
||||
version_msg_.robot_message_type = robot_message_type;
|
||||
memcpy(&version_msg_.project_name_size, &buf[offset],
|
||||
sizeof(version_msg_.project_name_size));
|
||||
offset += sizeof(version_msg_.project_name_size);
|
||||
memcpy(&version_msg_.project_name, &buf[offset],
|
||||
sizeof(char) * version_msg_.project_name_size);
|
||||
offset += version_msg_.project_name_size;
|
||||
memcpy(&version_msg_.major_version, &buf[offset],
|
||||
sizeof(version_msg_.major_version));
|
||||
offset += sizeof(version_msg_.major_version);
|
||||
memcpy(&version_msg_.minor_version, &buf[offset],
|
||||
sizeof(version_msg_.minor_version));
|
||||
offset += sizeof(version_msg_.minor_version);
|
||||
memcpy(&version_msg_.svn_revision, &buf[offset],
|
||||
sizeof(version_msg_.svn_revision));
|
||||
offset += sizeof(version_msg_.svn_revision);
|
||||
memcpy(&version_msg_.build_date, &buf[offset],
|
||||
sizeof(char) * len - offset);
|
||||
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
val_lock_.unlock();
|
||||
|
||||
}
|
||||
void RobotState::unpackRobotState(uint8_t * buf, unsigned int offset,
|
||||
uint32_t len) {
|
||||
|
||||
}
|
||||
|
||||
double RobotState::getVersion() {
|
||||
double ver;
|
||||
val_lock_.lock();
|
||||
ver = version_msg_.major_version + 0.1 * version_msg_.minor_version + .000001*version_msg_.svn_revision;
|
||||
val_lock_.unlock();
|
||||
return ver;
|
||||
|
||||
}
|
||||
|
||||
109
src/ur_communication.cpp
Normal file
109
src/ur_communication.cpp
Normal file
@@ -0,0 +1,109 @@
|
||||
/*
|
||||
* ur_communication.cpp
|
||||
*
|
||||
* ----------------------------------------------------------------------------
|
||||
* "THE BEER-WARE LICENSE" (Revision 42):
|
||||
* <thomas.timm.dk@gmail.com> wrote this file. As long as you retain this notice you
|
||||
* can do whatever you want with this stuff. If we meet some day, and you think
|
||||
* this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen
|
||||
* ----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "ur_modern_driver/ur_communication.h"
|
||||
|
||||
UrCommunication::UrCommunication(std::condition_variable& msg_cond,
|
||||
std::string host) {
|
||||
robot_state_ = new RobotState(msg_cond);
|
||||
bzero((char *) &pri_serv_addr_, sizeof(pri_serv_addr_));
|
||||
bzero((char *) &sec_serv_addr_, sizeof(sec_serv_addr_));
|
||||
sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
|
||||
if (sockfd_ < 0) {
|
||||
printf("ERROR opening socket");
|
||||
exit(0);
|
||||
}
|
||||
server_ = gethostbyname(host.c_str());
|
||||
if (server_ == NULL) {
|
||||
printf("ERROR, no such host\n");
|
||||
exit(0);
|
||||
}
|
||||
pri_serv_addr_.sin_family = AF_INET;
|
||||
sec_serv_addr_.sin_family = AF_INET;
|
||||
bcopy((char *) server_->h_addr, (char *)&pri_serv_addr_.sin_addr.s_addr, server_->h_length);
|
||||
bcopy((char *) server_->h_addr, (char *)&sec_serv_addr_.sin_addr.s_addr, server_->h_length);
|
||||
pri_serv_addr_.sin_port = htons(30001);
|
||||
sec_serv_addr_.sin_port = htons(30002);
|
||||
flag_ = 1;
|
||||
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int));
|
||||
setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int));
|
||||
connected_ = false;
|
||||
keepalive_ = false;
|
||||
}
|
||||
|
||||
void UrCommunication::start() {
|
||||
keepalive_ = true;
|
||||
uint8_t buf[512];
|
||||
unsigned int bytes_read;
|
||||
std::string cmd;
|
||||
bzero(buf, 512);
|
||||
|
||||
printf("Acquire firmware version: Connecting...\n");
|
||||
if (connect(sockfd_, (struct sockaddr *) &pri_serv_addr_,
|
||||
sizeof(pri_serv_addr_)) < 0) {
|
||||
printf("Error connecting");
|
||||
exit(1);
|
||||
}
|
||||
printf("Acquire firmware version: Got connection\n");
|
||||
bytes_read = read(sockfd_, buf, 512);
|
||||
setsockopt(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());
|
||||
close(sockfd_);
|
||||
|
||||
printf("Switching to secondary interface for masterboard data: Connecting...\n"); // which generates less network traffic
|
||||
if (connect(sockfd_, (struct sockaddr *) &sec_serv_addr_,
|
||||
sizeof(sec_serv_addr_)) < 0) {
|
||||
printf("Error connecting");
|
||||
exit(1);
|
||||
}
|
||||
printf("Secondary interface: Got connection\n");
|
||||
comThread_ = std::thread(&UrCommunication::run, this);
|
||||
}
|
||||
|
||||
void UrCommunication::halt() {
|
||||
keepalive_ = false;
|
||||
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;
|
||||
bzero(buf, 2048);
|
||||
printf("Got connection\n");
|
||||
connected_ = true;
|
||||
while (keepalive_) {
|
||||
bytes_read = read(sockfd_, buf, 2048); // usually only up to 1295 bytes
|
||||
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
|
||||
sizeof(int));
|
||||
robot_state_->unpack(buf, bytes_read);
|
||||
command_string_lock_.lock();
|
||||
if (command_.length() != 0) {
|
||||
write(sockfd_, command_.c_str(), command_.length());
|
||||
command_ = "";
|
||||
}
|
||||
command_string_lock_.unlock();
|
||||
|
||||
}
|
||||
close(sockfd_);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user