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