1
0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-12 11:00:47 +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

@@ -22,10 +22,12 @@
#include <cstring>
#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 comm
{
bool URStream::write(const uint8_t* buf, size_t buf_len, size_t& written)
{
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;
}
} // namespace comm
} // namespace ur_rtde_driver

View File

@@ -25,10 +25,12 @@
#include <cstring>
#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 comm
{
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;
}
} // namespace comm
} // namespace ur_rtde_driver

View File

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

View File

@@ -25,7 +25,8 @@
#include <thread>
#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/controller.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/factory.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/rt_state.h"
#include "ur_rtde_driver/ur/state.h"
@@ -81,7 +81,7 @@ public:
bool shutdown_on_disconnect;
};
class IgnorePipelineStoppedNotifier : public INotifier
class IgnorePipelineStoppedNotifier : public comm::INotifier
{
public:
void started(std::string name)
@@ -94,7 +94,7 @@ public:
}
};
class ShutdownOnPipelineStoppedNotifier : public INotifier
class ShutdownOnPipelineStoppedNotifier : public comm::INotifier
{
public:
void started(std::string name)
@@ -132,7 +132,7 @@ bool parse_args(ProgArgs& args)
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();
}
@@ -153,17 +153,17 @@ int main(int argc, char** argv)
std::string local_ip(getLocalIPAccessibleFromHost(args.host));
URFactory factory(args.host);
vector<Service*> services;
std::vector<Service*> services;
// RT packets
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);
RTPublisher rt_pub(args.prefix, args.base_frame, args.tool_frame, args.use_ros_control);
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);
ActionServer* action_server(nullptr);
if (args.use_ros_control)
@@ -208,20 +208,20 @@ int main(int argc, char** argv)
notifier = new IgnorePipelineStoppedNotifier();
}
MultiConsumer<RTPacket> rt_cons(rt_vec);
Pipeline<RTPacket> rt_pl(rt_prod, rt_cons, "RTPacket", *notifier);
comm::MultiConsumer<RTPacket> rt_cons(rt_vec);
comm::Pipeline<RTPacket> rt_pl(rt_prod, rt_cons, "RTPacket", *notifier);
// Message packets
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);
MBPublisher state_pub;
ServiceStopper service_stopper(services);
vector<IConsumer<StatePacket>*> state_vec{ &state_pub, &service_stopper };
MultiConsumer<StatePacket> state_cons(state_vec);
Pipeline<StatePacket> state_pl(state_prod, state_cons, "StatePacket", *notifier);
std::vector<comm::IConsumer<StatePacket>*> state_vec{ &state_pub, &service_stopper };
comm::MultiConsumer<StatePacket> state_cons(state_vec);
comm::Pipeline<StatePacket> state_pl(state_prod, state_cons, "StatePacket", *notifier);
LOG_INFO("Starting main loop");

View File

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