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:
@@ -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);
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
Reference in New Issue
Block a user