Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 18:10:47 +02:00
Code Releases Activity

Implemented communication on port 30001 and 30002 - untested

Browse Source
This commit is contained in:
Thomas Timm Andersen
2015-09-15 17:03:59 +02:00
parent cb18dd1d96
commit 889e2b70d4
5 changed files with 436 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

141
include/ur_modern_driver/robot_state.h Normal file
View File

@@ -0,0 +1,141 @@
/*
* robot_state.h
*
* Created on: Sep 10, 2015
* Author: ttan
*/
#ifndef ROBOT_STATE_H_
#define ROBOT_STATE_H_
#include <inttypes.h>
#include <vector>
#include <stdlib.h>
#include <string.h>
#include <mutex>
#include <condition_variable>
namespace message_types {
enum message_type {
ROBOT_STATE = 16, ROBOT_MESSAGE = 20, PROGRAM_STATE_MESSAGE = 25
};
}
typedef message_types::message_type messageType;
namespace package_types {
enum package_type {
ROBOT_MODE_DATA = 0,
JOINT_DATA = 1,
TOOL_DATA = 2,
MASTERBOARD_DATA = 3,
CARTESIAN_INFO = 4,
KINEMATICS_INFO = 5,
CONFIGURATION_DATA = 6,
FORCE_MODE_DATA = 7,
ADDITIONAL_INFO = 8,
CALIBRATION_DATA = 9
};
}
typedef package_types::package_type packageType;
namespace robot_message_types {
enum robot_message_type {
ROBOT_MESSAGE_TEXT = 0,
ROBOT_MESSAGE_PROGRAM_LABEL = 1,
PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2,
ROBOT_MESSAGE_VERSION = 3,
ROBOT_MESSAGE_SAFETY_MODE = 5,
ROBOT_MESSAGE_ERROR_CODE = 6,
ROBOT_MESSAGE_KEY = 7,
ROBOT_MESSAGE_REQUEST_VALUE = 9,
ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10
};
}
typedef robot_message_types::robot_message_type robotMessageType;
struct version_message {
uint64_t timestamp;
int8_t source;
int8_t robot_message_type;
int8_t project_name_size;
char* project_name;
uint8_t major_version;
uint8_t minor_version;
int svn_revision;
char* build_date;
};
struct masterboard_data {
int digitalInputBits;
int digitalOutputBits;
char analogInputRange0;
char analogInputRange1;
double analogInput0;
double analogInput1;
char analogOutputDomain0;
char analogOutputDomain1;
double analogOutput0;
double analogOutput1;
float masterBoardTemperature;
float robotVoltage48V;
float robotCurrent;
float masterIOCurrent;
unsigned char safetyMode;
unsigned char InReducedMode;
char euromap67InterfaceInstalled;
int euromapInputBits;
int euromapOutputBits;
float euromapVoltage;
float euromapCurrent;
uint32_t internal;
};
class RobotState {
private:
version_message version_msg_;
masterboard_data mb_data_;
std::mutex val_lock_; // Locks the variables while unpack parses data;
std::condition_variable* pMsg_cond_; //Signals that new vars are available
bool new_data_available_; //to avoid spurious wakes
double ntohd(uint64_t nf);
public:
RobotState(std::condition_variable& msg_cond);
~RobotState();
double getVersion();
double getTime();
std::vector<double> getQTarget();
int getDigitalInputBits();
int getDigitalOutputBits();
char getAnalogInputRange0();
char getAnalogInputRange1();
double getAnalogInput0();
double getAnalogInput1();
char getAnalogOutputDomain0();
char getAnalogOutputDomain1();
double getAnalogOutput0();
double getAnalogOutput1();
float getMasterBoardTemperature();
float getRobotVoltage48V();
float getRobotCurrent();
float getMasterIOCurrent();
unsigned char getSafetyMode();
unsigned char getInReducedMode();
char getEuromap67InterfaceInstalled();
int getEuromapInputBits();
int getEuromapOutputBits();
float getEuromapVoltage();
float getEuromapCurrent();
bool getNewDataAvailable();
void finishedReading();
std::vector<double> getVActual();
int unpack(uint8_t * buf, unsigned int buf_length);
void unpackRobotMessage(uint8_t * buf, unsigned int offset,uint32_t len);
void unpackRobotState(uint8_t * buf, unsigned int offset, uint32_t len);
};
#endif /* ROBOT_STATE_H_ */

56
include/ur_modern_driver/ur_communication.h Normal file
View File

@@ -0,0 +1,56 @@
/*
* ur_communication.h
*
* ----------------------------------------------------------------------------
* "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
* ----------------------------------------------------------------------------
*/
#ifndef UR_COMMUNICATION_H_
#define UR_COMMUNICATION_H_
#include "robot_state.h"
#include <vector>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <netdb.h>
#include <iostream>
#include <unistd.h>
#include <chrono>
class UrCommunication {
private:
int sockfd_;
struct sockaddr_in pri_serv_addr_, sec_serv_addr_;
struct hostent *server_;
bool keepalive_;
std::thread comThread_;
int flag_;
std::recursive_mutex command_string_lock_;
std::string command_;
void run();
public:
bool connected_;
RobotState* robot_state_;
UrCommunication(std::condition_variable& msg_cond, std::string host);
void start();
void halt();
void addCommandToQueue(std::string inp);
};
#endif /* UR_COMMUNICATION_H_ */

28
launch/ur_common.launch~ Normal file
View File

@@ -0,0 +1,28 @@
<?xml version="1.0"?>
<!--
Universal robot common bringup. Starts ur driver node and robot state
publisher (translates joint positions to propper tfs).
Usage:
ur_common.launch robot_ip:=<value>
-->
<launch>
<!-- robot_ip: IP-address of the robot's socket-messaging server -->
<arg name="robot_ip"/>
<arg name="min_payload"/>
<arg name="max_payload"/>
<!-- copy the specified IP address to be consistant with ROS-Industrial spec.
NOTE: The ip address is actually passed to the driver on the command line -->
<param name="/robot_ip_address" type="str" value="$(arg robot_ip)"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<!-- driver -->
<node name="ur_driver" pkg="ur_modern_driver" type="ur_driver" args="$(arg robot_ip)" output="screen">
<param name="min_payload" type="double" value="$(arg min_payload)"/>
<param name="max_payload" type="double" value="$(arg max_payload)"/>
<param name="max_velocity" type="double" value="$(arg max_velocity)"/>
</node>
</launch>

102
src/robot_state.cpp Normal file
View 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(&timestamp, &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
View 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_);
}