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

Tested producer part with real data

Browse Source
This commit is contained in:
Felix Mauch
2019-04-09 18:19:22 +02:00
parent 2f0b8eae1a
commit 51ac7ddb91
12 changed files with 119 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@@ -68,7 +68,6 @@ include_directories(
add_library(ur_rtde_driver
src/comm/tcp_socket.cpp
src/comm/shell_consumer.cpp
#src/ros/service_stopper.cpp
#src/ur/commander.cpp
#src/ur/master_board.cpp

4
include/ur_rtde_driver/comm/package.h
View File

@@ -46,8 +46,8 @@ public:
* \brief Creates a new URPackage object.
*/
URPackage();
virtual ~URPackage();
URPackage() = default;
virtual ~URPackage() = default;
using _header_type = HeaderT;

3
include/ur_rtde_driver/comm/parser.h
View File

@@ -35,6 +35,9 @@ class Parser
{
public:
Parser() = default;
virtual ~Parser() = default;
/*!
* \brief declares the parse function.
*

70
include/ur_rtde_driver/comm/pipeline.h
View File

@@ -138,10 +138,44 @@ public:
template <typename HeaderT>
class Pipeline
{
public:
using _package_type = URPackage<HeaderT>;
Pipeline(IProducer<HeaderT>& producer, IConsumer<_package_type>& consumer, std::string name, INotifier& notifier)
: producer_(producer), consumer_(consumer), name_(name), notifier_(notifier), queue_{ 32 }, running_{ false }
{
}
void run()
{
if (running_)
return;
running_ = true;
pThread_ = std::thread(&Pipeline::run_producer, this);
cThread_ = std::thread(&Pipeline::run_consumer, this);
notifier_.started(name_);
}
void stop()
{
if (!running_)
return;
LOG_INFO("Stopping pipeline! <%s>", name_.c_str());
consumer_.stopConsumer();
producer_.stopProducer();
running_ = false;
pThread_.join();
cThread_.join();
notifier_.stopped(name_);
}
private:
typedef std::chrono::high_resolution_clock Clock;
typedef Clock::time_point Time;
using _package_type = URPackage<HeaderT>;
IProducer<HeaderT>& producer_;
IConsumer<_package_type>& consumer_;
std::string name_;
@@ -203,40 +237,6 @@ private:
running_ = false;
notifier_.stopped(name_);
}
public:
Pipeline(IProducer<HeaderT>& producer, IConsumer<_package_type>& consumer, std::string name, INotifier& notifier)
: producer_(producer), consumer_(consumer), name_(name), notifier_(notifier), queue_{ 32 }, running_{ false }
{
}
void run()
{
if (running_)
return;
running_ = true;
pThread_ = std::thread(&Pipeline::run_producer, this);
cThread_ = std::thread(&Pipeline::run_consumer, this);
notifier_.started(name_);
}
void stop()
{
if (!running_)
return;
LOG_DEBUG("Stopping pipeline! <%s>", name_.c_str());
consumer_.stopConsumer();
producer_.stopProducer();
running_ = false;
pThread_.join();
cThread_.join();
notifier_.stopped(name_);
}
};
} // namespace comm
} // namespace ur_driver

4
include/ur_rtde_driver/comm/producer.h
View File

@@ -34,11 +34,11 @@ class URProducer : public IProducer<HeaderT>
{
private:
URStream<HeaderT>& stream_;
URParser<HeaderT>& parser_;
Parser<HeaderT>& parser_;
std::chrono::seconds timeout_;
public:
URProducer(URStream<HeaderT>& stream, URParser<HeaderT>& parser) : stream_(stream), parser_(parser), timeout_(1)
URProducer(URStream<HeaderT>& stream, Parser<HeaderT>& parser) : stream_(stream), parser_(parser), timeout_(1)
{
}

7
include/ur_rtde_driver/comm/shell_consumer.h
View File

@@ -15,6 +15,7 @@
#ifndef UR_RTDE_DRIVER_SHELL_CONSUMER_H_INCLUDED
#define UR_RTDE_DRIVER_SHELL_CONSUMER_H_INCLUDED
#include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/comm/pipeline.h"
#include "ur_rtde_driver/comm/package.h"
@@ -42,7 +43,11 @@ public:
{
}
virtual bool consume(std::shared_ptr<URPackage<HeaderT>> product);
virtual bool consume(std::shared_ptr<URPackage<HeaderT>> product)
{
LOG_INFO("%s", product->toString().c_str());
return true;
}
private:
/* data */

4
include/ur_rtde_driver/comm/stream.h
View File

@@ -130,7 +130,7 @@ bool URStream<HeaderT>::read(uint8_t* buf, const size_t buf_len, size_t& total)
bool initial = true;
uint8_t* buf_pos = buf;
size_t remainder = sizeof(HeaderT::_package_size_type);
size_t remainder = sizeof(typename HeaderT::_package_size_type);
size_t read = 0;
while (remainder > 0 && TCPSocket::read(buf_pos, remainder, read))
@@ -139,7 +139,7 @@ bool URStream<HeaderT>::read(uint8_t* buf, const size_t buf_len, size_t& total)
if (initial)
{
remainder = HeaderT::getPackageLength(buf);
if (remainder >= (buf_len - sizeof(HeaderT::_package_size_type)))
if (remainder >= (buf_len - sizeof(typename HeaderT::_package_size_type)))
{
LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len);
return false;

97
include/ur_rtde_driver/primary/primary_parser.h
View File

@@ -24,9 +24,9 @@
#include "ur_rtde_driver/primary/package_header.h"
#include "ur_rtde_driver/primary/robot_state.h"
#include "ur_rtde_driver/primary/robot_message.h"
#include "ur_rtde_driver/primary/robot_state/robot_mode_data.h"
//#include "ur_rtde_driver/primary/robot_state/robot_mode_data.h"
#include "ur_rtde_driver/primary/robot_state/kinematics_info.h"
#include "ur_rtde_driver/primary/robot_state/master_board.h"
//#include "ur_rtde_driver/primary/robot_state/master_board.h"
#include "ur_rtde_driver/primary/robot_message/version_message.h"
namespace ur_driver
@@ -34,54 +34,22 @@ namespace ur_driver
namespace primary_interface
{
using namespace comm;
class PrimaryParser : comm::Parser<PackageHeader>
class PrimaryParser : public comm::Parser<PackageHeader>
{
private:
RobotState* state_from_type(robot_state_type type)
{
switch (type)
{
/*case robot_state_type::ROBOT_MODE_DATA:
// SharedRobotModeData* rmd = new SharedRobotModeData();
//return new rmd;
case robot_state_type::MASTERBOARD_DATA:
return new MBD;*/
case robot_state_type::KINEMATICS_INFO:
return new KinematicsInfo;
default:
return nullptr;
}
}
RobotMessage* message_from_type(message_type type, uint64_t timestamp, uint8_t source)
{
switch (type)
{
/*case robot_state_type::ROBOT_MODE_DATA:
// SharedRobotModeData* rmd = new SharedRobotModeData();
//return new rmd;
case robot_state_type::MASTERBOARD_DATA:
return new MBD;*/
case message_type::ROBOT_MESSAGE_VERSION:
return new VersionMessage(timestamp, source);
default:
return nullptr;
}
}
public:
bool parse(BinParser& bp, std::vector<std::unique_ptr<PrimaryPackage>>& results)
PrimaryParser() = default;
virtual ~PrimaryParser() = default;
bool parse(BinParser& bp, std::vector<std::unique_ptr<URPackage<PackageHeader>>>& results)
{
int32_t packet_size;
robot_message_type type;
RobotPackageType type;
bp.parse(packet_size);
bp.parse(type);
switch (type)
{
case robot_message_type::ROBOT_STATE:
case RobotPackageType::ROBOT_STATE:
{
while (!bp.empty())
{
@@ -100,7 +68,7 @@ public:
// deconstruction of a sub parser will increment the position of the parent parser
BinParser sbp(bp, sub_size);
sbp.consume(sizeof(sub_size));
robot_state_type type;
RobotStateType type;
sbp.parse(type);
std::unique_ptr<PrimaryPackage> packet(state_from_type(type));
@@ -132,18 +100,16 @@ public:
break;
}
case robot_message_type::ROBOT_MESSAGE:
case RobotPackageType::ROBOT_MESSAGE:
{
uint64_t timestamp;
uint8_t source;
message_type message_type;
RobotMessagePackageType message_type;
bp.parse(timestamp);
bp.parse(source);
bp.parse(message_type);
bool parsed = false;
std::unique_ptr<PrimaryPackage> packet(message_from_type(message_type, timestamp, source));
if (!packet->parseWith(bp))
{
@@ -152,19 +118,54 @@ public:
}
results.push_back(std::move(packet));
return parsed;
return true;
break;
}
default:
{
LOG_WARN("Invalid state message type recieved: %u", static_cast<uint8_t>(type));
LOG_WARN("Invalid robot package type recieved: %u", static_cast<uint8_t>(type));
bp.consume();
return true;
}
}
return true;
}
private:
RobotState* state_from_type(RobotStateType type)
{
switch (type)
{
/*case robot_state_type::ROBOT_MODE_DATA:
// SharedRobotModeData* rmd = new SharedRobotModeData();
//return new rmd;
case robot_state_type::MASTERBOARD_DATA:
return new MBD;*/
case RobotStateType::KINEMATICS_INFO:
return new KinematicsInfo;
default:
return nullptr;
}
}
RobotMessage* message_from_type(RobotMessagePackageType type, uint64_t timestamp, uint8_t source)
{
switch (type)
{
/*case robot_state_type::ROBOT_MODE_DATA:
// SharedRobotModeData* rmd = new SharedRobotModeData();
//return new rmd;
case robot_state_type::MASTERBOARD_DATA:
return new MBD;*/
case RobotMessagePackageType::ROBOT_MESSAGE_VERSION:
return new VersionMessage(timestamp, source);
default:
return new RobotMessage(timestamp, source);
}
}
};
} // namespace primary_interface

14
include/ur_rtde_driver/primary/robot_state.h
View File

@@ -38,6 +38,20 @@ namespace ur_driver
{
namespace primary_interface
{
enum class RobotStateType : uint8_t
{
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
};
/*!
* \brief Abstract class for a RobotState msg. This will never be instanciated, but the underlying
* data packages will be used directly.

12
include/ur_rtde_driver/primary/robot_state/robot_mode_data.h
View File

@@ -38,7 +38,7 @@ namespace primary_interface
class SharedRobotModeData
{
public:
virtual bool parseWith(BinParser& bp);
virtual bool parseWith(comm::BinParser& bp);
uint64_t timestamp;
bool physical_robot_connected;
@@ -76,7 +76,7 @@ enum class robot_control_mode_V3_X : uint8_t
class RobotModeData_V3_0__1 : public SharedRobotModeData, public RobotState
{
public:
virtual bool parseWith(BinParser& bp);
virtual bool parseWith(comm::BinParser& bp);
robot_mode_V3_X robot_mode;
robot_control_mode_V3_X control_mode;
@@ -93,8 +93,8 @@ public:
class RobotModeData_V3_2 : public RobotModeData_V3_0__1
{
public:
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URStatePacketConsumer& consumer);
virtual bool parseWith(comm::BinParser& bp);
// virtual bool consumeWith(URStatePacketConsumer& consumer);
double target_speed_fraction_limit;
@@ -106,8 +106,8 @@ public:
class RobotModeData_V3_5 : public RobotModeData_V3_2
{
public:
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URStatePacketConsumer& consumer);
virtual bool parseWith(comm::BinParser& bp);
// virtual bool consumeWith(URStatePacketConsumer& consumer);
unsigned char unknown_internal_use;

29
src/comm/shell_consumer.cpp
View File

@@ -1,29 +0,0 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/comm/shell_consumer.h"
namespace ur_driver
{
namespace comm
{
template <typename HeaderT>
bool ShellConsumer<HeaderT>::consume(std::shared_ptr<URPackage<HeaderT>> pkg)
{
LOG_INFO("%s", pkg->toString());
return true;
}
} // namespace comm
} // namespace ur_driver

4
src/primary/robot_message.cpp
View File

@@ -32,10 +32,6 @@ namespace primary_interface
{
bool RobotMessage::parseWith(comm::BinParser& bp)
{
bp.parse(timestamp_);
bp.parse(source_);
bp.parse(message_type_);
return true;
}