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

put communication-related classes to namespace

This commit is contained in:
Felix Mauch
2019-04-01 14:54:39 +02:00
parent 5840d4f406
commit 31746259cf
21 changed files with 108 additions and 89 deletions

View File

@@ -78,6 +78,8 @@ target_link_libraries(ur_rtde_hardware_interface ${catkin_LIBRARIES})
set(${PROJECT_NAME}_SOURCES set(${PROJECT_NAME}_SOURCES
src/comm/stream.cpp
src/comm/tcp_socket.cpp
src/ros/action_server.cpp src/ros/action_server.cpp
src/ros/lowbandwidth_trajectory_follower.cpp src/ros/lowbandwidth_trajectory_follower.cpp
src/ros/mb_publisher.cpp src/ros/mb_publisher.cpp
@@ -85,14 +87,13 @@ set(${PROJECT_NAME}_SOURCES
src/ros/service_stopper.cpp src/ros/service_stopper.cpp
src/ros/trajectory_follower.cpp src/ros/trajectory_follower.cpp
src/ros/urscript_handler.cpp src/ros/urscript_handler.cpp
src/tcp_socket.cpp
src/ur/commander.cpp src/ur/commander.cpp
src/ur/master_board.cpp src/ur/master_board.cpp
src/ur/messages.cpp src/ur/messages.cpp
src/ur/robot_mode.cpp src/ur/robot_mode.cpp
src/ur/rt_state.cpp src/ur/rt_state.cpp
src/ur/server.cpp src/ur/server.cpp
src/ur/stream.cpp) )
add_executable(ur_rtde_driver ${${PROJECT_NAME}_SOURCES} src/ros_main.cpp) add_executable(ur_rtde_driver ${${PROJECT_NAME}_SOURCES} src/ros_main.cpp)
add_dependencies(ur_rtde_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(ur_rtde_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

View File

@@ -19,14 +19,18 @@
#pragma once #pragma once
#include <vector> #include <vector>
#include "ur_rtde_driver/bin_parser.h" #include "ur_rtde_driver/bin_parser.h"
#include "ur_rtde_driver/pipeline.h" #include "ur_rtde_driver/comm/pipeline.h"
namespace ur_rtde_driver namespace ur_rtde_driver
{ {
namespace comm
{
template <typename T> template <typename T>
class URParser class URParser
{ {
public: public:
virtual bool parse(BinParser& bp, std::vector<std::unique_ptr<T>>& results) = 0; virtual bool parse(BinParser& bp, std::vector<std::unique_ptr<T>>& results) = 0;
}; }; // namespace commtemplate<typenameT>classURParser
} // namespace comm
} // namespace ur_rtde_driver } // namespace ur_rtde_driver

View File

@@ -27,9 +27,10 @@
namespace ur_rtde_driver namespace ur_rtde_driver
{ {
namespace comm
{
// TODO: Remove these!!! // TODO: Remove these!!!
using namespace moodycamel; using namespace moodycamel;
using namespace std;
template <typename T> template <typename T>
class IConsumer class IConsumer
@@ -48,7 +49,7 @@ public:
{ {
} }
virtual bool consume(shared_ptr<T> product) = 0; virtual bool consume(std::shared_ptr<T> product) = 0;
}; };
template <typename T> template <typename T>
@@ -91,7 +92,7 @@ public:
} }
} }
bool consume(shared_ptr<T> product) bool consume(std::shared_ptr<T> product)
{ {
bool res = true; bool res = true;
for (auto& con : consumers_) for (auto& con : consumers_)
@@ -117,7 +118,7 @@ public:
{ {
} }
virtual bool tryGet(std::vector<unique_ptr<T>>& products) = 0; virtual bool tryGet(std::vector<std::unique_ptr<T>>& products) = 0;
}; };
class INotifier class INotifier
@@ -141,14 +142,14 @@ private:
IConsumer<T>& consumer_; IConsumer<T>& consumer_;
std::string name_; std::string name_;
INotifier& notifier_; INotifier& notifier_;
BlockingReaderWriterQueue<unique_ptr<T>> queue_; BlockingReaderWriterQueue<std::unique_ptr<T>> queue_;
atomic<bool> running_; std::atomic<bool> running_;
thread pThread_, cThread_; std::thread pThread_, cThread_;
void run_producer() void run_producer()
{ {
producer_.setupProducer(); producer_.setupProducer();
std::vector<unique_ptr<T>> products; std::vector<std::unique_ptr<T>> products;
while (running_) while (running_)
{ {
if (!producer_.tryGet(products)) if (!producer_.tryGet(products))
@@ -176,7 +177,7 @@ private:
void run_consumer() void run_consumer()
{ {
consumer_.setupConsumer(); consumer_.setupConsumer();
unique_ptr<T> product; std::unique_ptr<T> product;
while (running_) while (running_)
{ {
// timeout was chosen because we should receive messages // timeout was chosen because we should receive messages
@@ -211,8 +212,8 @@ public:
return; return;
running_ = true; running_ = true;
pThread_ = thread(&Pipeline::run_producer, this); pThread_ = std::thread(&Pipeline::run_producer, this);
cThread_ = thread(&Pipeline::run_consumer, this); cThread_ = std::thread(&Pipeline::run_consumer, this);
notifier_.started(name_); notifier_.started(name_);
} }
@@ -233,4 +234,5 @@ public:
notifier_.stopped(name_); notifier_.stopped(name_);
} }
}; };
} // namespace comm
} // namespace ur_rtde_driver } // namespace ur_rtde_driver

View File

@@ -24,10 +24,12 @@
#include <mutex> #include <mutex>
#include <string> #include <string>
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/tcp_socket.h" #include "ur_rtde_driver/comm/tcp_socket.h"
namespace ur_rtde_driver namespace ur_rtde_driver
{ {
namespace comm
{
class URStream : public TCPSocket class URStream : public TCPSocket
{ {
private: private:
@@ -64,4 +66,5 @@ public:
bool read(uint8_t* buf, size_t buf_len, size_t& read); bool read(uint8_t* buf, size_t buf_len, size_t& read);
bool write(const uint8_t* buf, size_t buf_len, size_t& written); bool write(const uint8_t* buf, size_t buf_len, size_t& written);
}; };
} // namespace comm
} // namespace ur_rtde_driver } // namespace ur_rtde_driver

View File

@@ -26,6 +26,8 @@
namespace ur_rtde_driver namespace ur_rtde_driver
{ {
namespace comm
{
enum class SocketState enum class SocketState
{ {
Invalid, Invalid,
@@ -72,4 +74,5 @@ public:
void close(); void close();
}; };
} // namespace comm
} // namespace ur_rtde_driver } // namespace ur_rtde_driver

View File

@@ -20,21 +20,21 @@
#include <array> #include <array>
#include <iomanip> #include <iomanip>
#include <sstream> #include <sstream>
#include "ur_rtde_driver/ur/stream.h" #include "ur_rtde_driver/comm/stream.h"
namespace ur_rtde_driver namespace ur_rtde_driver
{ {
class URCommander class URCommander
{ {
private: private:
URStream& stream_; comm::URStream& stream_;
protected: protected:
bool write(const std::string& s); bool write(const std::string& s);
void formatArray(std::ostringstream& out, std::array<double, 6>& values); void formatArray(std::ostringstream& out, std::array<double, 6>& values);
public: public:
URCommander(URStream& stream) : stream_(stream) URCommander(comm::URStream& stream) : stream_(stream)
{ {
} }
@@ -53,7 +53,7 @@ public:
class URCommander_V1_X : public URCommander class URCommander_V1_X : public URCommander
{ {
public: public:
URCommander_V1_X(URStream& stream) : URCommander(stream) URCommander_V1_X(comm::URStream& stream) : URCommander(stream)
{ {
} }
@@ -65,7 +65,7 @@ public:
class URCommander_V3_X : public URCommander class URCommander_V3_X : public URCommander
{ {
public: public:
URCommander_V3_X(URStream& stream) : URCommander(stream) URCommander_V3_X(comm::URStream& stream) : URCommander(stream)
{ {
} }
@@ -77,7 +77,7 @@ public:
class URCommander_V3_1__2 : public URCommander_V3_X class URCommander_V3_1__2 : public URCommander_V3_X
{ {
public: public:
URCommander_V3_1__2(URStream& stream) : URCommander_V3_X(stream) URCommander_V3_1__2(comm::URStream& stream) : URCommander_V3_X(stream)
{ {
} }
@@ -87,7 +87,7 @@ public:
class URCommander_V3_3 : public URCommander_V3_X class URCommander_V3_3 : public URCommander_V3_X
{ {
public: public:
URCommander_V3_3(URStream& stream) : URCommander_V3_X(stream) URCommander_V3_3(comm::URStream& stream) : URCommander_V3_X(stream)
{ {
} }

View File

@@ -18,7 +18,7 @@
#pragma once #pragma once
#include "ur_rtde_driver/pipeline.h" #include "ur_rtde_driver/comm/pipeline.h"
#include "ur_rtde_driver/ur/master_board.h" #include "ur_rtde_driver/ur/master_board.h"
#include "ur_rtde_driver/ur/messages.h" #include "ur_rtde_driver/ur/messages.h"
#include "ur_rtde_driver/ur/robot_mode.h" #include "ur_rtde_driver/ur/robot_mode.h"
@@ -27,10 +27,10 @@
namespace ur_rtde_driver namespace ur_rtde_driver
{ {
class URRTPacketConsumer : public IConsumer<RTPacket> class URRTPacketConsumer : public comm::IConsumer<RTPacket>
{ {
public: public:
virtual bool consume(shared_ptr<RTPacket> packet) virtual bool consume(std::shared_ptr<RTPacket> packet)
{ {
return packet->consumeWith(*this); return packet->consumeWith(*this);
} }
@@ -41,10 +41,10 @@ public:
virtual bool consume(RTState_V3_2__3& state) = 0; virtual bool consume(RTState_V3_2__3& state) = 0;
}; };
class URStatePacketConsumer : public IConsumer<StatePacket> class URStatePacketConsumer : public comm::IConsumer<StatePacket>
{ {
public: public:
virtual bool consume(shared_ptr<StatePacket> packet) virtual bool consume(std::shared_ptr<StatePacket> packet)
{ {
return packet->consumeWith(*this); return packet->consumeWith(*this);
} }
@@ -58,10 +58,10 @@ public:
virtual bool consume(RobotModeData_V3_2& data) = 0; virtual bool consume(RobotModeData_V3_2& data) = 0;
}; };
class URMessagePacketConsumer : public IConsumer<MessagePacket> class URMessagePacketConsumer : public comm::IConsumer<MessagePacket>
{ {
public: public:
virtual bool consume(shared_ptr<MessagePacket> packet) virtual bool consume(std::shared_ptr<MessagePacket> packet)
{ {
return packet->consumeWith(*this); return packet->consumeWith(*this);
} }

View File

@@ -19,13 +19,13 @@
#pragma once #pragma once
#include <cstdlib> #include <cstdlib>
#include "ur_rtde_driver/comm/parser.h"
#include "ur_rtde_driver/comm/stream.h"
#include "ur_rtde_driver/ur/consumer.h" #include "ur_rtde_driver/ur/consumer.h"
#include "ur_rtde_driver/ur/messages_parser.h" #include "ur_rtde_driver/ur/messages_parser.h"
#include "ur_rtde_driver/ur/parser.h"
#include "ur_rtde_driver/ur/producer.h" #include "ur_rtde_driver/ur/producer.h"
#include "ur_rtde_driver/ur/rt_parser.h" #include "ur_rtde_driver/ur/rt_parser.h"
#include "ur_rtde_driver/ur/state_parser.h" #include "ur_rtde_driver/ur/state_parser.h"
#include "ur_rtde_driver/ur/stream.h"
namespace ur_rtde_driver namespace ur_rtde_driver
{ {
@@ -34,7 +34,7 @@ static const int UR_PRIMARY_PORT = 30001;
class URFactory : private URMessagePacketConsumer class URFactory : private URMessagePacketConsumer
{ {
private: private:
URStream stream_; comm::URStream stream_;
URMessageParser parser_; URMessageParser parser_;
uint8_t major_version_; uint8_t major_version_;
@@ -67,7 +67,7 @@ public:
URFactory(std::string& host) : stream_(host, UR_PRIMARY_PORT) URFactory(std::string& host) : stream_(host, UR_PRIMARY_PORT)
{ {
URProducer<MessagePacket> prod(stream_, parser_); URProducer<MessagePacket> prod(stream_, parser_);
std::vector<unique_ptr<MessagePacket>> results; std::vector<std::unique_ptr<MessagePacket>> results;
prod.setupProducer(); prod.setupProducer();
@@ -96,7 +96,7 @@ public:
return major_version_ == 3; return major_version_ == 3;
} }
std::unique_ptr<URCommander> getCommander(URStream& stream) std::unique_ptr<URCommander> getCommander(comm::URStream& stream)
{ {
if (major_version_ == 1) if (major_version_ == 1)
return std::unique_ptr<URCommander>(new URCommander_V1_X(stream)); return std::unique_ptr<URCommander>(new URCommander_V1_X(stream));
@@ -106,38 +106,38 @@ public:
return std::unique_ptr<URCommander>(new URCommander_V3_3(stream)); return std::unique_ptr<URCommander>(new URCommander_V3_3(stream));
} }
std::unique_ptr<URParser<StatePacket>> getStateParser() std::unique_ptr<comm::URParser<StatePacket>> getStateParser()
{ {
if (major_version_ == 1) if (major_version_ == 1)
{ {
return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V1_X); return std::unique_ptr<comm::URParser<StatePacket>>(new URStateParser_V1_X);
} }
else else
{ {
if (minor_version_ < 3) if (minor_version_ < 3)
return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_0__1); return std::unique_ptr<comm::URParser<StatePacket>>(new URStateParser_V3_0__1);
else if (minor_version_ < 5) else if (minor_version_ < 5)
return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_2); return std::unique_ptr<comm::URParser<StatePacket>>(new URStateParser_V3_2);
else else
return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_5); return std::unique_ptr<comm::URParser<StatePacket>>(new URStateParser_V3_5);
} }
} }
std::unique_ptr<URParser<RTPacket>> getRTParser() std::unique_ptr<comm::URParser<RTPacket>> getRTParser()
{ {
if (major_version_ == 1) if (major_version_ == 1)
{ {
if (minor_version_ < 8) if (minor_version_ < 8)
return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V1_6__7); return std::unique_ptr<comm::URParser<RTPacket>>(new URRTStateParser_V1_6__7);
else else
return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V1_8); return std::unique_ptr<comm::URParser<RTPacket>>(new URRTStateParser_V1_8);
} }
else else
{ {
if (minor_version_ < 3) if (minor_version_ < 3)
return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_0__1); return std::unique_ptr<comm::URParser<RTPacket>>(new URRTStateParser_V3_0__1);
else else
return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_2__3); return std::unique_ptr<comm::URParser<RTPacket>>(new URRTStateParser_V3_2__3);
} }
} }
}; };

View File

@@ -21,7 +21,7 @@
#include <inttypes.h> #include <inttypes.h>
#include <cstddef> #include <cstddef>
#include "ur_rtde_driver/bin_parser.h" #include "ur_rtde_driver/bin_parser.h"
#include "ur_rtde_driver/pipeline.h" #include "ur_rtde_driver/comm/pipeline.h"
namespace ur_rtde_driver namespace ur_rtde_driver
{ {

View File

@@ -20,16 +20,16 @@
#include <vector> #include <vector>
#include "ur_rtde_driver/bin_parser.h" #include "ur_rtde_driver/bin_parser.h"
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/pipeline.h" #include "ur_rtde_driver/comm/parser.h"
#include "ur_rtde_driver/comm/pipeline.h"
#include "ur_rtde_driver/ur/messages.h" #include "ur_rtde_driver/ur/messages.h"
#include "ur_rtde_driver/ur/parser.h"
namespace ur_rtde_driver namespace ur_rtde_driver
{ {
class URMessageParser : public URParser<MessagePacket> class URMessageParser : public comm::URParser<MessagePacket>
{ {
public: public:
bool parse(BinParser& bp, std::vector<unique_ptr<MessagePacket>>& results) bool parse(BinParser& bp, std::vector<std::unique_ptr<MessagePacket>>& results)
{ {
int32_t packet_size; int32_t packet_size;
message_type type; message_type type;

View File

@@ -18,22 +18,22 @@
#pragma once #pragma once
#include <chrono> #include <chrono>
#include "ur_rtde_driver/pipeline.h" #include "ur_rtde_driver/comm/pipeline.h"
#include "ur_rtde_driver/ur/parser.h" #include "ur_rtde_driver/comm/parser.h"
#include "ur_rtde_driver/ur/stream.h" #include "ur_rtde_driver/comm/stream.h"
namespace ur_rtde_driver namespace ur_rtde_driver
{ {
template <typename T> template <typename T>
class URProducer : public IProducer<T> class URProducer : public comm::IProducer<T>
{ {
private: private:
URStream& stream_; comm::URStream& stream_;
URParser<T>& parser_; comm::URParser<T>& parser_;
std::chrono::seconds timeout_; std::chrono::seconds timeout_;
public: public:
URProducer(URStream& stream, URParser<T>& parser) : stream_(stream), parser_(parser), timeout_(1) URProducer(comm::URStream& stream, comm::URParser<T>& parser) : stream_(stream), parser_(parser), timeout_(1)
{ {
} }
@@ -50,7 +50,7 @@ public:
stream_.disconnect(); stream_.disconnect();
} }
bool tryGet(std::vector<unique_ptr<T>>& products) bool tryGet(std::vector<std::unique_ptr<T>>& products)
{ {
// 4KB should be enough to hold any packet received from UR // 4KB should be enough to hold any packet received from UR
uint8_t buf[4096]; uint8_t buf[4096];

View File

@@ -20,14 +20,14 @@
#include <vector> #include <vector>
#include "ur_rtde_driver/bin_parser.h" #include "ur_rtde_driver/bin_parser.h"
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/pipeline.h" #include "ur_rtde_driver/comm/pipeline.h"
#include "ur_rtde_driver/ur/parser.h" #include "ur_rtde_driver/comm/parser.h"
#include "ur_rtde_driver/ur/rt_state.h" #include "ur_rtde_driver/ur/rt_state.h"
namespace ur_rtde_driver namespace ur_rtde_driver
{ {
template <typename T> template <typename T>
class URRTStateParser : public URParser<RTPacket> class URRTStateParser : public comm::URParser<RTPacket>
{ {
public: public:
bool parse(BinParser& bp, std::vector<std::unique_ptr<RTPacket>>& results) bool parse(BinParser& bp, std::vector<std::unique_ptr<RTPacket>>& results)

View File

@@ -21,7 +21,7 @@
#include <inttypes.h> #include <inttypes.h>
#include <cstddef> #include <cstddef>
#include "ur_rtde_driver/bin_parser.h" #include "ur_rtde_driver/bin_parser.h"
#include "ur_rtde_driver/pipeline.h" #include "ur_rtde_driver/comm/pipeline.h"
#include "ur_rtde_driver/types.h" #include "ur_rtde_driver/types.h"
namespace ur_rtde_driver namespace ur_rtde_driver

View File

@@ -24,17 +24,17 @@
#include <cstdlib> #include <cstdlib>
#include <mutex> #include <mutex>
#include <string> #include <string>
#include "ur_rtde_driver/tcp_socket.h" #include "ur_rtde_driver/comm/tcp_socket.h"
namespace ur_rtde_driver namespace ur_rtde_driver
{ {
#define MAX_SERVER_BUF_LEN 50 #define MAX_SERVER_BUF_LEN 50
class URServer : private TCPSocket class URServer : private comm::TCPSocket
{ {
private: private:
int port_; int port_;
TCPSocket client_; comm::TCPSocket client_;
protected: protected:
virtual bool open(int socket_fd, struct sockaddr* address, size_t address_len); virtual bool open(int socket_fd, struct sockaddr* address, size_t address_len);

View File

@@ -22,7 +22,7 @@
#include <cstddef> #include <cstddef>
#include "ur_rtde_driver/bin_parser.h" #include "ur_rtde_driver/bin_parser.h"
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/pipeline.h" #include "ur_rtde_driver/comm/pipeline.h"
namespace ur_rtde_driver namespace ur_rtde_driver
{ {

View File

@@ -20,16 +20,16 @@
#include <vector> #include <vector>
#include "ur_rtde_driver/bin_parser.h" #include "ur_rtde_driver/bin_parser.h"
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/pipeline.h" #include "ur_rtde_driver/comm/parser.h"
#include "ur_rtde_driver/comm/pipeline.h"
#include "ur_rtde_driver/ur/master_board.h" #include "ur_rtde_driver/ur/master_board.h"
#include "ur_rtde_driver/ur/parser.h"
#include "ur_rtde_driver/ur/robot_mode.h" #include "ur_rtde_driver/ur/robot_mode.h"
#include "ur_rtde_driver/ur/state.h" #include "ur_rtde_driver/ur/state.h"
namespace ur_rtde_driver namespace ur_rtde_driver
{ {
template <typename RMD, typename MBD> template <typename RMD, typename MBD>
class URStateParser : public URParser<StatePacket> class URStateParser : public comm::URParser<StatePacket>
{ {
private: private:
StatePacket* from_type(package_type type) StatePacket* from_type(package_type type)

View File

@@ -22,10 +22,12 @@
#include <cstring> #include <cstring>
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/ur/stream.h" #include "ur_rtde_driver/comm/stream.h"
namespace ur_rtde_driver namespace ur_rtde_driver
{ {
namespace comm
{
bool URStream::write(const uint8_t* buf, size_t buf_len, size_t& written) bool URStream::write(const uint8_t* buf, size_t buf_len, size_t& written)
{ {
std::lock_guard<std::mutex> lock(write_mutex_); std::lock_guard<std::mutex> lock(write_mutex_);
@@ -62,4 +64,5 @@ bool URStream::read(uint8_t* buf, size_t buf_len, size_t& total)
return remainder == 0; return remainder == 0;
} }
} // namespace comm
} // namespace ur_rtde_driver } // namespace ur_rtde_driver

View File

@@ -25,10 +25,12 @@
#include <cstring> #include <cstring>
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/tcp_socket.h" #include "ur_rtde_driver/comm/tcp_socket.h"
namespace ur_rtde_driver namespace ur_rtde_driver
{ {
namespace comm
{
TCPSocket::TCPSocket() : socket_fd_(-1), state_(SocketState::Invalid) TCPSocket::TCPSocket() : socket_fd_(-1), state_(SocketState::Invalid)
{ {
} }
@@ -186,4 +188,5 @@ bool TCPSocket::write(const uint8_t* buf, size_t buf_len, size_t& written)
return true; return true;
} }
} // namespace comm
} // namespace ur_rtde_driver } // namespace ur_rtde_driver

View File

@@ -45,7 +45,7 @@ void ActionServer::start()
LOG_INFO("Starting ActionServer"); LOG_INFO("Starting ActionServer");
running_ = true; running_ = true;
tj_thread_ = thread(&ActionServer::trajectoryThread, this); tj_thread_ = std::thread(&ActionServer::trajectoryThread, this);
as_.start(); as_.start();
} }

View File

@@ -25,7 +25,8 @@
#include <thread> #include <thread>
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/pipeline.h" #include "ur_rtde_driver/comm/parser.h"
#include "ur_rtde_driver/comm/pipeline.h"
#include "ur_rtde_driver/ros/action_server.h" #include "ur_rtde_driver/ros/action_server.h"
#include "ur_rtde_driver/ros/controller.h" #include "ur_rtde_driver/ros/controller.h"
#include "ur_rtde_driver/ros/io_service.h" #include "ur_rtde_driver/ros/io_service.h"
@@ -38,7 +39,6 @@
#include "ur_rtde_driver/ur/commander.h" #include "ur_rtde_driver/ur/commander.h"
#include "ur_rtde_driver/ur/factory.h" #include "ur_rtde_driver/ur/factory.h"
#include "ur_rtde_driver/ur/messages.h" #include "ur_rtde_driver/ur/messages.h"
#include "ur_rtde_driver/ur/parser.h"
#include "ur_rtde_driver/ur/producer.h" #include "ur_rtde_driver/ur/producer.h"
#include "ur_rtde_driver/ur/rt_state.h" #include "ur_rtde_driver/ur/rt_state.h"
#include "ur_rtde_driver/ur/state.h" #include "ur_rtde_driver/ur/state.h"
@@ -81,7 +81,7 @@ public:
bool shutdown_on_disconnect; bool shutdown_on_disconnect;
}; };
class IgnorePipelineStoppedNotifier : public INotifier class IgnorePipelineStoppedNotifier : public comm::INotifier
{ {
public: public:
void started(std::string name) void started(std::string name)
@@ -94,7 +94,7 @@ public:
} }
}; };
class ShutdownOnPipelineStoppedNotifier : public INotifier class ShutdownOnPipelineStoppedNotifier : public comm::INotifier
{ {
public: public:
void started(std::string name) void started(std::string name)
@@ -132,7 +132,7 @@ bool parse_args(ProgArgs& args)
std::string getLocalIPAccessibleFromHost(std::string& host) std::string getLocalIPAccessibleFromHost(std::string& host)
{ {
URStream stream(host, UR_RT_PORT); comm::URStream stream(host, UR_RT_PORT);
return stream.connect() ? stream.getIP() : std::string(); return stream.connect() ? stream.getIP() : std::string();
} }
@@ -153,17 +153,17 @@ int main(int argc, char** argv)
std::string local_ip(getLocalIPAccessibleFromHost(args.host)); std::string local_ip(getLocalIPAccessibleFromHost(args.host));
URFactory factory(args.host); URFactory factory(args.host);
vector<Service*> services; std::vector<Service*> services;
// RT packets // RT packets
auto rt_parser = factory.getRTParser(); auto rt_parser = factory.getRTParser();
URStream rt_stream(args.host, UR_RT_PORT); comm::URStream rt_stream(args.host, UR_RT_PORT);
URProducer<RTPacket> rt_prod(rt_stream, *rt_parser); URProducer<RTPacket> rt_prod(rt_stream, *rt_parser);
RTPublisher rt_pub(args.prefix, args.base_frame, args.tool_frame, args.use_ros_control); RTPublisher rt_pub(args.prefix, args.base_frame, args.tool_frame, args.use_ros_control);
auto rt_commander = factory.getCommander(rt_stream); auto rt_commander = factory.getCommander(rt_stream);
vector<IConsumer<RTPacket>*> rt_vec{ &rt_pub }; std::vector<comm::IConsumer<RTPacket>*> rt_vec{ &rt_pub };
INotifier* notifier(nullptr); comm::INotifier* notifier(nullptr);
ROSController* controller(nullptr); ROSController* controller(nullptr);
ActionServer* action_server(nullptr); ActionServer* action_server(nullptr);
if (args.use_ros_control) if (args.use_ros_control)
@@ -208,20 +208,20 @@ int main(int argc, char** argv)
notifier = new IgnorePipelineStoppedNotifier(); notifier = new IgnorePipelineStoppedNotifier();
} }
MultiConsumer<RTPacket> rt_cons(rt_vec); comm::MultiConsumer<RTPacket> rt_cons(rt_vec);
Pipeline<RTPacket> rt_pl(rt_prod, rt_cons, "RTPacket", *notifier); comm::Pipeline<RTPacket> rt_pl(rt_prod, rt_cons, "RTPacket", *notifier);
// Message packets // Message packets
auto state_parser = factory.getStateParser(); auto state_parser = factory.getStateParser();
URStream state_stream(args.host, UR_SECONDARY_PORT); comm::URStream state_stream(args.host, UR_SECONDARY_PORT);
URProducer<StatePacket> state_prod(state_stream, *state_parser); URProducer<StatePacket> state_prod(state_stream, *state_parser);
MBPublisher state_pub; MBPublisher state_pub;
ServiceStopper service_stopper(services); ServiceStopper service_stopper(services);
vector<IConsumer<StatePacket>*> state_vec{ &state_pub, &service_stopper }; std::vector<comm::IConsumer<StatePacket>*> state_vec{ &state_pub, &service_stopper };
MultiConsumer<StatePacket> state_cons(state_vec); comm::MultiConsumer<StatePacket> state_cons(state_vec);
Pipeline<StatePacket> state_pl(state_prod, state_cons, "StatePacket", *notifier); comm::Pipeline<StatePacket> state_pl(state_prod, state_cons, "StatePacket", *notifier);
LOG_INFO("Starting main loop"); LOG_INFO("Starting main loop");

View File

@@ -76,7 +76,7 @@ bool URServer::bind()
bool URServer::accept() bool URServer::accept()
{ {
if (TCPSocket::getState() != SocketState::Connected || client_.getSocketFD() > 0) if (TCPSocket::getState() != comm::SocketState::Connected || client_.getSocketFD() > 0)
return false; return false;
struct sockaddr addr; struct sockaddr addr;
@@ -98,7 +98,7 @@ bool URServer::accept()
void URServer::disconnectClient() void URServer::disconnectClient()
{ {
if (client_.getState() != SocketState::Connected) if (client_.getState() != comm::SocketState::Connected)
return; return;
client_.close(); client_.close();