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

Adopted roscpp code style and naming convention

This commit is contained in:
Simon Rasmussen
2017-03-01 13:59:48 +01:00
parent e4bc40fc09
commit 474f469e97
44 changed files with 5097 additions and 4891 deletions

View File

@@ -1,97 +1,99 @@
#include "ur_modern_driver/ur/master_board.h"
#include "ur_modern_driver/ur/consumer.h"
bool SharedMasterBoardData::parse_with(BinParser& bp)
bool SharedMasterBoardData::parseWith(BinParser& bp)
{
bp.parse(analog_input_range0);
bp.parse(analog_input_range1);
bp.parse(analog_input0);
bp.parse(analog_input1);
bp.parse(analog_output_domain0);
bp.parse(analog_output_domain1);
bp.parse(analog_output0);
bp.parse(analog_output1);
bp.parse(master_board_temperature);
bp.parse(robot_voltage_48V);
bp.parse(robot_current);
bp.parse(master_IO_current);
return true;
bp.parse(analog_input_range0);
bp.parse(analog_input_range1);
bp.parse(analog_input0);
bp.parse(analog_input1);
bp.parse(analog_output_domain0);
bp.parse(analog_output_domain1);
bp.parse(analog_output0);
bp.parse(analog_output1);
bp.parse(master_board_temperature);
bp.parse(robot_voltage_48V);
bp.parse(robot_current);
bp.parse(master_IO_current);
return true;
}
bool MasterBoardData_V1_X::parse_with(BinParser& bp)
bool MasterBoardData_V1_X::parseWith(BinParser& bp)
{
if (!bp.check_size<MasterBoardData_V1_X>())
return false;
if (!bp.checkSize<MasterBoardData_V1_X>())
return false;
bp.parse(digital_input_bits);
bp.parse(digital_output_bits);
bp.parse(digital_input_bits);
bp.parse(digital_output_bits);
SharedMasterBoardData::parse_with(bp);
SharedMasterBoardData::parseWith(bp);
bp.parse(master_safety_state);
bp.parse(master_on_off_state);
bp.parse(euromap67_interface_installed);
bp.parse(master_safety_state);
bp.parse(master_on_off_state);
bp.parse(euromap67_interface_installed);
if (euromap67_interface_installed) {
if (!bp.check_size(MasterBoardData_V1_X::EURO_SIZE))
return false;
if (euromap67_interface_installed)
{
if (!bp.checkSize(MasterBoardData_V1_X::EURO_SIZE))
return false;
bp.parse(euromap_voltage);
bp.parse(euromap_current);
}
bp.parse(euromap_voltage);
bp.parse(euromap_current);
}
return true;
return true;
}
bool MasterBoardData_V3_0__1::parse_with(BinParser& bp)
bool MasterBoardData_V3_0__1::parseWith(BinParser& bp)
{
if (!bp.check_size<MasterBoardData_V3_0__1>())
return false;
if (!bp.checkSize<MasterBoardData_V3_0__1>())
return false;
bp.parse(digital_input_bits);
bp.parse(digital_output_bits);
bp.parse(digital_input_bits);
bp.parse(digital_output_bits);
SharedMasterBoardData::parse_with(bp);
SharedMasterBoardData::parseWith(bp);
bp.parse(safety_mode);
bp.parse(in_reduced_mode);
bp.parse(euromap67_interface_installed);
bp.parse(safety_mode);
bp.parse(in_reduced_mode);
bp.parse(euromap67_interface_installed);
if (euromap67_interface_installed) {
if (!bp.check_size(MasterBoardData_V3_0__1::EURO_SIZE))
return false;
if (euromap67_interface_installed)
{
if (!bp.checkSize(MasterBoardData_V3_0__1::EURO_SIZE))
return false;
bp.parse(euromap_voltage);
bp.parse(euromap_current);
}
bp.parse(euromap_voltage);
bp.parse(euromap_current);
}
bp.consume(sizeof(uint32_t));
bp.consume(sizeof(uint32_t));
return true;
return true;
}
bool MasterBoardData_V3_2::parse_with(BinParser& bp)
bool MasterBoardData_V3_2::parseWith(BinParser& bp)
{
if (!bp.check_size<MasterBoardData_V3_2>())
return false;
if (!bp.checkSize<MasterBoardData_V3_2>())
return false;
MasterBoardData_V3_0__1::parse_with(bp);
MasterBoardData_V3_0__1::parseWith(bp);
bp.parse(operational_mode_selector_input);
bp.parse(three_position_enabling_device_input);
bp.parse(operational_mode_selector_input);
bp.parse(three_position_enabling_device_input);
return true;
return true;
}
bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer& consumer)
bool MasterBoardData_V1_X::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}
bool MasterBoardData_V3_0__1::consume_with(URStatePacketConsumer& consumer)
bool MasterBoardData_V3_0__1::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}
bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer& consumer)
bool MasterBoardData_V3_2::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}

View File

@@ -1,20 +1,19 @@
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/consumer.h"
bool VersionMessage::parse_with(BinParser& bp)
bool VersionMessage::parseWith(BinParser& bp)
{
bp.parse(project_name);
bp.parse(major_version);
bp.parse(minor_version);
bp.parse(svn_version);
bp.consume(sizeof(uint32_t)); // undocumented field??
bp.parse_remainder(build_date);
bp.parse(project_name);
bp.parse(major_version);
bp.parse(minor_version);
bp.parse(svn_version);
bp.consume(sizeof(uint32_t)); //undocumented field??
bp.parse_remainder(build_date);
return true; //not really possible to check dynamic size packets
return true; // not really possible to check dynamic size packets
}
bool VersionMessage::consume_with(URMessagePacketConsumer& consumer)
bool VersionMessage::consumeWith(URMessagePacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}

View File

@@ -1,71 +1,71 @@
#include "ur_modern_driver/ur/robot_mode.h"
#include "ur_modern_driver/ur/consumer.h"
bool SharedRobotModeData::parse_with(BinParser& bp)
bool SharedRobotModeData::parseWith(BinParser& bp)
{
bp.parse(timestamp);
bp.parse(physical_robot_connected);
bp.parse(real_robot_enabled);
bp.parse(robot_power_on);
bp.parse(emergency_stopped);
return true;
bp.parse(timestamp);
bp.parse(physical_robot_connected);
bp.parse(real_robot_enabled);
bp.parse(robot_power_on);
bp.parse(emergency_stopped);
return true;
}
bool RobotModeData_V1_X::parse_with(BinParser& bp)
bool RobotModeData_V1_X::parseWith(BinParser& bp)
{
if (!bp.check_size<RobotModeData_V1_X>())
return false;
if (!bp.checkSize<RobotModeData_V1_X>())
return false;
SharedRobotModeData::parse_with(bp);
SharedRobotModeData::parseWith(bp);
bp.parse(security_stopped);
bp.parse(program_running);
bp.parse(program_paused);
bp.parse(robot_mode);
bp.parse(speed_fraction);
bp.parse(security_stopped);
bp.parse(program_running);
bp.parse(program_paused);
bp.parse(robot_mode);
bp.parse(speed_fraction);
return true;
return true;
}
bool RobotModeData_V3_0__1::parse_with(BinParser& bp)
bool RobotModeData_V3_0__1::parseWith(BinParser& bp)
{
if (!bp.check_size<RobotModeData_V3_0__1>())
return false;
if (!bp.checkSize<RobotModeData_V3_0__1>())
return false;
SharedRobotModeData::parse_with(bp);
SharedRobotModeData::parseWith(bp);
bp.parse(protective_stopped);
bp.parse(program_running);
bp.parse(program_paused);
bp.parse(robot_mode);
bp.parse(control_mode);
bp.parse(target_speed_fraction);
bp.parse(speed_scaling);
bp.parse(protective_stopped);
bp.parse(program_running);
bp.parse(program_paused);
bp.parse(robot_mode);
bp.parse(control_mode);
bp.parse(target_speed_fraction);
bp.parse(speed_scaling);
return true;
return true;
}
bool RobotModeData_V3_2::parse_with(BinParser& bp)
bool RobotModeData_V3_2::parseWith(BinParser& bp)
{
if (!bp.check_size<RobotModeData_V3_2>())
return false;
if (!bp.checkSize<RobotModeData_V3_2>())
return false;
RobotModeData_V3_0__1::parse_with(bp);
RobotModeData_V3_0__1::parseWith(bp);
bp.parse(target_speed_fraction_limit);
bp.parse(target_speed_fraction_limit);
return true;
return true;
}
bool RobotModeData_V1_X::consume_with(URStatePacketConsumer& consumer)
bool RobotModeData_V1_X::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}
bool RobotModeData_V3_0__1::consume_with(URStatePacketConsumer& consumer)
bool RobotModeData_V3_0__1::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}
bool RobotModeData_V3_2::consume_with(URStatePacketConsumer& consumer)
bool RobotModeData_V3_2::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}

View File

@@ -3,115 +3,115 @@
void RTShared::parse_shared1(BinParser& bp)
{
bp.parse(time);
bp.parse(q_target);
bp.parse(qd_target);
bp.parse(qdd_target);
bp.parse(i_target);
bp.parse(m_target);
bp.parse(q_actual);
bp.parse(qd_actual);
bp.parse(i_actual);
bp.parse(time);
bp.parse(q_target);
bp.parse(qd_target);
bp.parse(qdd_target);
bp.parse(i_target);
bp.parse(m_target);
bp.parse(q_actual);
bp.parse(qd_actual);
bp.parse(i_actual);
}
void RTShared::parse_shared2(BinParser& bp)
{
bp.parse(digital_inputs);
bp.parse(motor_temperatures);
bp.parse(controller_time);
bp.consume(sizeof(double)); //Unused "Test value" field
bp.parse(robot_mode);
bp.parse(digital_inputs);
bp.parse(motor_temperatures);
bp.parse(controller_time);
bp.consume(sizeof(double)); // Unused "Test value" field
bp.parse(robot_mode);
}
bool RTState_V1_6__7::parse_with(BinParser& bp)
bool RTState_V1_6__7::parseWith(BinParser& bp)
{
if (!bp.check_size<RTState_V1_6__7>())
return false;
if (!bp.checkSize<RTState_V1_6__7>())
return false;
parse_shared1(bp);
parse_shared1(bp);
bp.parse(tool_accelerometer_values);
bp.consume(sizeof(double)*15);
bp.parse(tcp_force);
bp.parse(tool_vector_actual);
bp.parse(tcp_speed_actual);
bp.parse(tool_accelerometer_values);
bp.consume(sizeof(double) * 15);
bp.parse(tcp_force);
bp.parse(tool_vector_actual);
bp.parse(tcp_speed_actual);
parse_shared2(bp);
parse_shared2(bp);
return true;
return true;
}
bool RTState_V1_8::parse_with(BinParser& bp)
bool RTState_V1_8::parseWith(BinParser& bp)
{
if (!bp.check_size<RTState_V1_8>())
return false;
if (!bp.checkSize<RTState_V1_8>())
return false;
RTState_V1_6__7::parse_with(bp);
RTState_V1_6__7::parseWith(bp);
bp.parse(joint_modes);
bp.parse(joint_modes);
return true;
return true;
}
bool RTState_V3_0__1::parse_with(BinParser& bp)
bool RTState_V3_0__1::parseWith(BinParser& bp)
{
if (!bp.check_size<RTState_V3_0__1>())
return false;
if (!bp.checkSize<RTState_V3_0__1>())
return false;
parse_shared1(bp);
parse_shared1(bp);
bp.parse(i_control);
bp.parse(tool_vector_actual);
bp.parse(tcp_speed_actual);
bp.parse(tcp_force);
bp.parse(tool_vector_target);
bp.parse(tcp_speed_target);
bp.parse(i_control);
bp.parse(tool_vector_actual);
bp.parse(tcp_speed_actual);
bp.parse(tcp_force);
bp.parse(tool_vector_target);
bp.parse(tcp_speed_target);
parse_shared2(bp);
parse_shared2(bp);
bp.parse(joint_modes);
bp.parse(safety_mode);
bp.consume(sizeof(double[6])); //skip undocumented
bp.parse(tool_accelerometer_values);
bp.consume(sizeof(double[6])); //skip undocumented
bp.parse(speed_scaling);
bp.parse(linear_momentum_norm);
bp.consume(sizeof(double)); //skip undocumented
bp.consume(sizeof(double)); //skip undocumented
bp.parse(v_main);
bp.parse(v_robot);
bp.parse(i_robot);
bp.parse(v_actual);
bp.parse(joint_modes);
bp.parse(safety_mode);
bp.consume(sizeof(double[6])); // skip undocumented
bp.parse(tool_accelerometer_values);
bp.consume(sizeof(double[6])); // skip undocumented
bp.parse(speed_scaling);
bp.parse(linear_momentum_norm);
bp.consume(sizeof(double)); // skip undocumented
bp.consume(sizeof(double)); // skip undocumented
bp.parse(v_main);
bp.parse(v_robot);
bp.parse(i_robot);
bp.parse(v_actual);
return true;
return true;
}
bool RTState_V3_2__3::parse_with(BinParser& bp)
bool RTState_V3_2__3::parseWith(BinParser& bp)
{
if (!bp.check_size<RTState_V3_2__3>())
return false;
if (!bp.checkSize<RTState_V3_2__3>())
return false;
RTState_V3_0__1::parse_with(bp);
RTState_V3_0__1::parseWith(bp);
bp.parse(digital_outputs);
bp.parse(program_state);
bp.parse(digital_outputs);
bp.parse(program_state);
return true;
return true;
}
bool RTState_V1_6__7::consume_with(URRTPacketConsumer& consumer)
bool RTState_V1_6__7::consumeWith(URRTPacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}
bool RTState_V1_8::consume_with(URRTPacketConsumer& consumer)
bool RTState_V1_8::consumeWith(URRTPacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}
bool RTState_V3_0__1::consume_with(URRTPacketConsumer& consumer)
bool RTState_V3_0__1::consumeWith(URRTPacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}
bool RTState_V3_2__3::consume_with(URRTPacketConsumer& consumer)
bool RTState_V3_2__3::consumeWith(URRTPacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}

View File

@@ -1,25 +1,25 @@
#include "ur_modern_driver/ur/state.h"
#include "ur_modern_driver/log.h"
//StatePacket::~StatePacket() { }
// StatePacket::~StatePacket() { }
/*
bool RobotState::parse_with(BinParser &bp) {
bool RobotState::parseWith(BinParser &bp) {
//continue as long as there are bytes to read
while(!bp.empty()) {
if(!bp.check_size(sizeof(uint32_t))){
if(!bp.checkSize(sizeof(uint32_t))){
LOG_ERROR("Failed to read sub-package length, there's likely a parsing error");
return false;
return false;
}
uint32_t sub_size = bp.peek<uint32_t>();
if(!bp.check_size(static_cast<size_t>(sub_size))) {
if(!bp.checkSize(static_cast<size_t>(sub_size))) {
LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size);
return false;
}
//deconstruction of a sub parser will increment the position of the parent parser
//deconstruction of a sub parser will increment the position of the parent parser
BinParser sub_parser(bp, sub_size);
sub_parser.consume(sizeof(sub_size));
@@ -43,19 +43,19 @@ bool parse_base(BinParser &bp, T &pkg) {
case package_type::ROBOT_MODE_DATA:
LOG_DEBUG("Parsing robot_mode");
bp.consume(sizeof(package_type));
pkg.robot_mode.parse_with(bp);
pkg.robot_mode.parseWith(bp);
break;
case package_type::MASTERBOARD_DATA:
LOG_DEBUG("Parsing master_board");
bp.consume(sizeof(package_type));
pkg.master_board.parse_with(bp);
pkg.master_board.parseWith(bp);
break;
case package_type::TOOL_DATA:
case package_type::CARTESIAN_INFO:
case package_type::JOINT_DATA:
LOG_DEBUG("Skipping tool, cartesian or joint data");
//for unhandled packages we consume the rest of the
LOG_DEBUG("Skipping tool, cartesian or joint data");
//for unhandled packages we consume the rest of the
//package buffer
bp.consume();
break;
@@ -79,8 +79,8 @@ bool parse_advanced(BinParser &bp, T &pkg) {
case package_type::ADDITIONAL_INFO:
case package_type::CALIBRATION_DATA:
case package_type::FORCE_MODE_DATA:
LOG_DEBUG("Skipping kinematics, config, additional, calibration or force mode data");
//for unhandled packages we consume the rest of the
LOG_DEBUG("Skipping kinematics, config, additional, calibration or force mode data");
//for unhandled packages we consume the rest of the
//package buffer
bp.consume();
break;

View File

@@ -1,126 +1,133 @@
#include <cstring>
#include <endian.h>
#include <netinet/tcp.h>
#include <unistd.h>
#include <cstring>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/stream.h"
bool URStream::connect()
{
if (_initialized)
return false;
if (initialized_)
return false;
LOG_INFO("Connecting to UR @ %s:%d", _host.c_str(), _port);
LOG_INFO("Connecting to UR @ %s:%d", host_.c_str(), port_);
//gethostbyname() is deprecated so use getadderinfo() as described in:
//http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo
// gethostbyname() is deprecated so use getadderinfo() as described in:
// http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo
std::string service = std::to_string(_port);
struct addrinfo hints, *result;
std::memset(&hints, 0, sizeof(hints));
std::string service = std::to_string(port_);
struct addrinfo hints, *result;
std::memset(&hints, 0, sizeof(hints));
hints.ai_family = AF_UNSPEC;
hints.ai_socktype = SOCK_STREAM;
hints.ai_flags = AI_PASSIVE;
hints.ai_family = AF_UNSPEC;
hints.ai_socktype = SOCK_STREAM;
hints.ai_flags = AI_PASSIVE;
if (getaddrinfo(_host.c_str(), service.c_str(), &hints, &result) != 0) {
LOG_ERROR("Failed to get host name");
return false;
}
if (getaddrinfo(host_.c_str(), service.c_str(), &hints, &result) != 0)
{
LOG_ERROR("Failed to get host name");
return false;
}
//loop through the list of addresses untill we find one that's connectable
for (struct addrinfo* p = result; p != nullptr; p = p->ai_next) {
_socket_fd = socket(p->ai_family, p->ai_socktype, p->ai_protocol);
// loop through the list of addresses untill we find one that's connectable
for (struct addrinfo* p = result; p != nullptr; p = p->ai_next)
{
socket_fd_ = socket(p->ai_family, p->ai_socktype, p->ai_protocol);
if (_socket_fd == -1) //socket error?
continue;
if (socket_fd_ == -1) // socket error?
continue;
if (::connect(_socket_fd, p->ai_addr, p->ai_addrlen) != 0) {
if (_stopping)
break;
else
continue; //try next addrinfo if connect fails
}
//disable Nagle's algorithm to ensure we sent packets as fast as possible
int flag = 1;
setsockopt(_socket_fd, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag));
_initialized = true;
LOG_INFO("Connection successfully established");
if (::connect(socket_fd_, p->ai_addr, p->ai_addrlen) != 0)
{
if (stopping_)
break;
else
continue; // try next addrinfo if connect fails
}
freeaddrinfo(result);
if (!_initialized)
LOG_ERROR("Connection failed");
// disable Nagle's algorithm to ensure we sent packets as fast as possible
int flag = 1;
setsockopt(socket_fd_, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag));
initialized_ = true;
LOG_INFO("Connection successfully established");
break;
}
return _initialized;
freeaddrinfo(result);
if (!initialized_)
LOG_ERROR("Connection failed");
return initialized_;
}
void URStream::disconnect()
{
if (!_initialized || _stopping)
return;
if (!initialized_ || stopping_)
return;
LOG_INFO("Disconnecting from %s:%d", _host.c_str(), _port);
LOG_INFO("Disconnecting from %s:%d", host_.c_str(), port_);
_stopping = true;
close(_socket_fd);
_initialized = false;
stopping_ = true;
close(socket_fd_);
initialized_ = false;
}
ssize_t URStream::send(uint8_t* buf, size_t buf_len)
{
if (!_initialized)
return -1;
if (_stopping)
return 0;
if (!initialized_)
return -1;
if (stopping_)
return 0;
size_t total = 0;
size_t remaining = buf_len;
size_t total = 0;
size_t remaining = buf_len;
//TODO: handle reconnect?
//handle partial sends
while (total < buf_len) {
ssize_t sent = ::send(_socket_fd, buf + total, remaining, 0);
if (sent <= 0)
return _stopping ? 0 : sent;
total += sent;
remaining -= sent;
}
// TODO: handle reconnect?
// handle partial sends
while (total < buf_len)
{
ssize_t sent = ::send(socket_fd_, buf + total, remaining, 0);
if (sent <= 0)
return stopping_ ? 0 : sent;
total += sent;
remaining -= sent;
}
return total;
return total;
}
ssize_t URStream::receive(uint8_t* buf, size_t buf_len)
{
if (!_initialized)
if (!initialized_)
return -1;
if (stopping_)
return 0;
size_t remainder = sizeof(int32_t);
uint8_t* buf_pos = buf;
bool initial = true;
do
{
ssize_t read = recv(socket_fd_, buf_pos, remainder, 0);
if (read <= 0) // failed reading from socket
return stopping_ ? 0 : read;
if (initial)
{
remainder = be32toh(*(reinterpret_cast<int32_t*>(buf)));
if (remainder >= (buf_len - sizeof(int32_t)))
{
LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len);
return -1;
if (_stopping)
return 0;
}
initial = false;
}
size_t remainder = sizeof(int32_t);
uint8_t* buf_pos = buf;
bool initial = true;
buf_pos += read;
remainder -= read;
} while (remainder > 0);
do {
ssize_t read = recv(_socket_fd, buf_pos, remainder, 0);
if (read <= 0) //failed reading from socket
return _stopping ? 0 : read;
if (initial) {
remainder = be32toh(*(reinterpret_cast<int32_t*>(buf)));
if (remainder >= (buf_len - sizeof(int32_t))) {
LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len);
return -1;
}
initial = false;
}
buf_pos += read;
remainder -= read;
} while (remainder > 0);
return buf_pos - buf;
return buf_pos - buf;
}