1
0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-18 05:50:46 +02:00

Put everyting into ur_rtde_driver namespace

This commit is contained in:
Felix Mauch
2019-04-01 13:20:25 +02:00
parent 515dd41edc
commit 5840d4f406
53 changed files with 172 additions and 17 deletions

View File

@@ -29,6 +29,8 @@
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/types.h" #include "ur_rtde_driver/types.h"
namespace ur_rtde_driver
{
class BinParser class BinParser
{ {
private: private:
@@ -201,3 +203,4 @@ public:
LOG_DEBUG("BinParser: %p - %p (%zu bytes)", buf_pos_, buf_end_, buf_end_ - buf_pos_); LOG_DEBUG("BinParser: %p - %p (%zu bytes)", buf_pos_, buf_end_, buf_end_ - buf_pos_);
} }
}; };
} // namespace ur_rtde_driver

View File

@@ -23,6 +23,8 @@
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/ur/consumer.h" #include "ur_rtde_driver/ur/consumer.h"
namespace ur_rtde_driver
{
class EventCounter : public URRTPacketConsumer class EventCounter : public URRTPacketConsumer
{ {
private: private:
@@ -100,3 +102,4 @@ public:
{ {
} }
}; };
} // namespace ur_rtde_driver

View File

@@ -25,6 +25,9 @@
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/queue/readerwriterqueue.h" #include "ur_rtde_driver/queue/readerwriterqueue.h"
namespace ur_rtde_driver
{
// TODO: Remove these!!!
using namespace moodycamel; using namespace moodycamel;
using namespace std; using namespace std;
@@ -230,3 +233,4 @@ public:
notifier_.stopped(name_); notifier_.stopped(name_);
} }
}; };
} // namespace ur_rtde_driver

View File

@@ -37,6 +37,8 @@
#include "ur_rtde_driver/ur/master_board.h" #include "ur_rtde_driver/ur/master_board.h"
#include "ur_rtde_driver/ur/state.h" #include "ur_rtde_driver/ur/state.h"
namespace ur_rtde_driver
{
class ActionServer : public Service, public URRTPacketConsumer class ActionServer : public Service, public URRTPacketConsumer
{ {
private: private:
@@ -91,3 +93,4 @@ public:
virtual bool consume(RTState_V3_0__1& state); virtual bool consume(RTState_V3_0__1& state);
virtual bool consume(RTState_V3_2__3& state); virtual bool consume(RTState_V3_2__3& state);
}; };
} // namespace ur_rtde_driver

View File

@@ -26,6 +26,8 @@
#include <cstddef> #include <cstddef>
#include <vector> #include <vector>
namespace ur_rtde_driver
{
struct TrajectoryPoint struct TrajectoryPoint
{ {
std::array<double, 6> positions; std::array<double, 6> positions;
@@ -50,3 +52,4 @@ public:
virtual void stop() = 0; virtual void stop() = 0;
virtual ~ActionTrajectoryFollowerInterface(){}; virtual ~ActionTrajectoryFollowerInterface(){};
}; };
} // namespace ur_rtde_driver

View File

@@ -31,6 +31,8 @@
#include "ur_rtde_driver/ur/consumer.h" #include "ur_rtde_driver/ur/consumer.h"
#include "ur_rtde_driver/ur/rt_state.h" #include "ur_rtde_driver/ur/rt_state.h"
namespace ur_rtde_driver
{
class ROSController : private hardware_interface::RobotHW, public URRTPacketConsumer, public Service class ROSController : private hardware_interface::RobotHW, public URRTPacketConsumer, public Service
{ {
private: private:
@@ -108,3 +110,4 @@ public:
virtual void onRobotStateChange(RobotState state); virtual void onRobotStateChange(RobotState state);
}; };
} // namespace ur_rtde_driver

View File

@@ -26,6 +26,8 @@
#include "ur_rtde_driver/ur/commander.h" #include "ur_rtde_driver/ur/commander.h"
#include "ur_rtde_driver/ur/rt_state.h" #include "ur_rtde_driver/ur/rt_state.h"
namespace ur_rtde_driver
{
class HardwareInterface class HardwareInterface
{ {
public: public:
@@ -99,3 +101,4 @@ public:
typedef hardware_interface::PositionJointInterface parent_type; typedef hardware_interface::PositionJointInterface parent_type;
static const std::string INTERFACE_NAME; static const std::string INTERFACE_NAME;
}; };
} // namespace ur_rtde_driver

View File

@@ -28,6 +28,8 @@
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/ur/commander.h" #include "ur_rtde_driver/ur/commander.h"
namespace ur_rtde_driver
{
class IOService class IOService
{ {
private: private:
@@ -77,3 +79,4 @@ public:
{ {
} }
}; };
} // namespace ur_rtde_driver

View File

@@ -29,6 +29,8 @@
#include "ur_rtde_driver/ur/commander.h" #include "ur_rtde_driver/ur/commander.h"
#include "ur_rtde_driver/ur/server.h" #include "ur_rtde_driver/ur/server.h"
namespace ur_rtde_driver
{
class LowBandwidthTrajectoryFollower : public ActionTrajectoryFollowerInterface class LowBandwidthTrajectoryFollower : public ActionTrajectoryFollowerInterface
{ {
private: private:
@@ -54,3 +56,4 @@ public:
virtual ~LowBandwidthTrajectoryFollower(){}; virtual ~LowBandwidthTrajectoryFollower(){};
}; };
} // namespace ur_rtde_driver

View File

@@ -26,6 +26,8 @@
#include "ur_rtde_driver/ur/consumer.h" #include "ur_rtde_driver/ur/consumer.h"
namespace ur_rtde_driver
{
using namespace ros; using namespace ros;
class MBPublisher : public URStatePacketConsumer class MBPublisher : public URStatePacketConsumer
@@ -77,3 +79,4 @@ public:
{ {
} }
}; };
} // namespace ur_rtde_driver

View File

@@ -31,6 +31,8 @@
#include "ur_rtde_driver/ur/consumer.h" #include "ur_rtde_driver/ur/consumer.h"
namespace ur_rtde_driver
{
using namespace ros; using namespace ros;
using namespace tf; using namespace tf;
@@ -90,3 +92,4 @@ public:
{ {
} }
}; };
} // namespace ur_rtde_driver

View File

@@ -22,6 +22,8 @@
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/ur/consumer.h" #include "ur_rtde_driver/ur/consumer.h"
namespace ur_rtde_driver
{
enum class RobotState enum class RobotState
{ {
Running, Running,
@@ -86,3 +88,4 @@ public:
return true; return true;
} }
}; };
} // namespace ur_rtde_driver

View File

@@ -33,6 +33,8 @@
#include "ur_rtde_driver/ur/commander.h" #include "ur_rtde_driver/ur/commander.h"
#include "ur_rtde_driver/ur/server.h" #include "ur_rtde_driver/ur/server.h"
namespace ur_rtde_driver
{
class TrajectoryFollower : public ActionTrajectoryFollowerInterface class TrajectoryFollower : public ActionTrajectoryFollowerInterface
{ {
private: private:
@@ -65,3 +67,4 @@ public:
virtual ~TrajectoryFollower(){}; virtual ~TrajectoryFollower(){};
}; };
} // namespace ur_rtde_driver

View File

@@ -26,6 +26,8 @@
#include "ur_rtde_driver/ros/service_stopper.h" #include "ur_rtde_driver/ros/service_stopper.h"
#include "ur_rtde_driver/ur/commander.h" #include "ur_rtde_driver/ur/commander.h"
namespace ur_rtde_driver
{
class URScriptHandler : public Service class URScriptHandler : public Service
{ {
private: private:
@@ -39,3 +41,4 @@ public:
void urscriptInterface(const std_msgs::String::ConstPtr& msg); void urscriptInterface(const std_msgs::String::ConstPtr& msg);
void onRobotStateChange(RobotState state); void onRobotStateChange(RobotState state);
}; };
} // namespace ur_rtde_driver

View File

@@ -24,6 +24,8 @@
#include <mutex> #include <mutex>
#include <string> #include <string>
namespace ur_rtde_driver
{
enum class SocketState enum class SocketState
{ {
Invalid, Invalid,
@@ -70,3 +72,4 @@ public:
void close(); void close();
}; };
} // namespace ur_rtde_driver

View File

@@ -26,6 +26,8 @@
#include <random> #include <random>
#include "ur_rtde_driver/bin_parser.h" #include "ur_rtde_driver/bin_parser.h"
namespace ur_rtde_driver
{
class RandomDataTest class RandomDataTest
{ {
private: private:
@@ -78,3 +80,4 @@ public:
bp_.consume(n); bp_.consume(n);
} }
}; };
} // namespace ur_rtde_driver

View File

@@ -20,6 +20,8 @@
#include <inttypes.h> #include <inttypes.h>
namespace ur_rtde_driver
{
struct double3_t struct double3_t
{ {
double x, y, z; double x, y, z;
@@ -40,3 +42,4 @@ inline bool operator==(const cartesian_coord_t& lhs, const cartesian_coord_t& rh
{ {
return lhs.position == rhs.position && lhs.rotation == rhs.rotation; return lhs.position == rhs.position && lhs.rotation == rhs.rotation;
} }
} // namespace ur_rtde_driver

View File

@@ -22,6 +22,8 @@
#include <sstream> #include <sstream>
#include "ur_rtde_driver/ur/stream.h" #include "ur_rtde_driver/ur/stream.h"
namespace ur_rtde_driver
{
class URCommander class URCommander
{ {
private: private:
@@ -91,3 +93,4 @@ public:
virtual bool speedj(std::array<double, 6>& speeds, double acceleration); virtual bool speedj(std::array<double, 6>& speeds, double acceleration);
}; };
} // namespace ur_rtde_driver

View File

@@ -25,6 +25,8 @@
#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"
namespace ur_rtde_driver
{
class URRTPacketConsumer : public IConsumer<RTPacket> class URRTPacketConsumer : public IConsumer<RTPacket>
{ {
public: public:
@@ -66,3 +68,4 @@ public:
virtual bool consume(VersionMessage& message) = 0; virtual bool consume(VersionMessage& message) = 0;
}; };
} // namespace ur_rtde_driver

View File

@@ -27,6 +27,8 @@
#include "ur_rtde_driver/ur/state_parser.h" #include "ur_rtde_driver/ur/state_parser.h"
#include "ur_rtde_driver/ur/stream.h" #include "ur_rtde_driver/ur/stream.h"
namespace ur_rtde_driver
{
static const int UR_PRIMARY_PORT = 30001; static const int UR_PRIMARY_PORT = 30001;
class URFactory : private URMessagePacketConsumer class URFactory : private URMessagePacketConsumer
@@ -139,3 +141,4 @@ public:
} }
} }
}; };
} // namespace ur_rtde_driver

View File

@@ -25,6 +25,8 @@
#include "ur_rtde_driver/types.h" #include "ur_rtde_driver/types.h"
#include "ur_rtde_driver/ur/state.h" #include "ur_rtde_driver/ur/state.h"
namespace ur_rtde_driver
{
class SharedMasterBoardData class SharedMasterBoardData
{ {
public: public:
@@ -108,3 +110,4 @@ public:
static const size_t SIZE = MasterBoardData_V3_0__1::SIZE + sizeof(uint8_t) * 2; static const size_t SIZE = MasterBoardData_V3_0__1::SIZE + sizeof(uint8_t) * 2;
}; };
} // namespace ur_rtde_driver

View File

@@ -23,6 +23,8 @@
#include "ur_rtde_driver/bin_parser.h" #include "ur_rtde_driver/bin_parser.h"
#include "ur_rtde_driver/pipeline.h" #include "ur_rtde_driver/pipeline.h"
namespace ur_rtde_driver
{
enum class robot_message_type : uint8_t enum class robot_message_type : uint8_t
{ {
ROBOT_MESSAGE_TEXT = 0, ROBOT_MESSAGE_TEXT = 0,
@@ -67,3 +69,4 @@ public:
int32_t svn_version; int32_t svn_version;
std::string build_date; std::string build_date;
}; };
} // namespace ur_rtde_driver

View File

@@ -24,6 +24,8 @@
#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/parser.h"
namespace ur_rtde_driver
{
class URMessageParser : public URParser<MessagePacket> class URMessageParser : public URParser<MessagePacket>
{ {
public: public:
@@ -69,3 +71,4 @@ public:
return parsed; return parsed;
} }
}; };
} // namespace ur_rtde_driver

View File

@@ -21,9 +21,12 @@
#include "ur_rtde_driver/bin_parser.h" #include "ur_rtde_driver/bin_parser.h"
#include "ur_rtde_driver/pipeline.h" #include "ur_rtde_driver/pipeline.h"
namespace ur_rtde_driver
{
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 ur_rtde_driver

View File

@@ -22,6 +22,8 @@
#include "ur_rtde_driver/ur/parser.h" #include "ur_rtde_driver/ur/parser.h"
#include "ur_rtde_driver/ur/stream.h" #include "ur_rtde_driver/ur/stream.h"
namespace ur_rtde_driver
{
template <typename T> template <typename T>
class URProducer : public IProducer<T> class URProducer : public IProducer<T>
{ {
@@ -81,3 +83,4 @@ public:
return parser_.parse(bp, products); return parser_.parse(bp, products);
} }
}; };
} // namespace ur_rtde_driver

View File

@@ -24,6 +24,8 @@
#include "ur_rtde_driver/types.h" #include "ur_rtde_driver/types.h"
#include "ur_rtde_driver/ur/state.h" #include "ur_rtde_driver/ur/state.h"
namespace ur_rtde_driver
{
class SharedRobotModeData class SharedRobotModeData
{ {
public: public:
@@ -134,3 +136,4 @@ public:
static_assert(RobotModeData_V3_5::SIZE == 42, "RobotModeData_V3_5 has missmatched size"); static_assert(RobotModeData_V3_5::SIZE == 42, "RobotModeData_V3_5 has missmatched size");
}; };
} // namespace ur_rtde_driver

View File

@@ -24,6 +24,8 @@
#include "ur_rtde_driver/ur/parser.h" #include "ur_rtde_driver/ur/parser.h"
#include "ur_rtde_driver/ur/rt_state.h" #include "ur_rtde_driver/ur/rt_state.h"
namespace ur_rtde_driver
{
template <typename T> template <typename T>
class URRTStateParser : public URParser<RTPacket> class URRTStateParser : public URParser<RTPacket>
{ {
@@ -54,3 +56,4 @@ typedef URRTStateParser<RTState_V1_6__7> URRTStateParser_V1_6__7;
typedef URRTStateParser<RTState_V1_8> URRTStateParser_V1_8; typedef URRTStateParser<RTState_V1_8> URRTStateParser_V1_8;
typedef URRTStateParser<RTState_V3_0__1> URRTStateParser_V3_0__1; typedef URRTStateParser<RTState_V3_0__1> URRTStateParser_V3_0__1;
typedef URRTStateParser<RTState_V3_2__3> URRTStateParser_V3_2__3; typedef URRTStateParser<RTState_V3_2__3> URRTStateParser_V3_2__3;
} // namespace ur_rtde_driver

View File

@@ -24,6 +24,8 @@
#include "ur_rtde_driver/pipeline.h" #include "ur_rtde_driver/pipeline.h"
#include "ur_rtde_driver/types.h" #include "ur_rtde_driver/types.h"
namespace ur_rtde_driver
{
class URRTPacketConsumer; class URRTPacketConsumer;
class RTPacket class RTPacket
@@ -135,3 +137,4 @@ public:
static_assert(RTState_V3_2__3::SIZE == 936, "RTState_V3_2__3 has mismatched size!"); static_assert(RTState_V3_2__3::SIZE == 936, "RTState_V3_2__3 has mismatched size!");
}; };
} // namespace ur_rtde_driver

View File

@@ -26,6 +26,8 @@
#include <string> #include <string>
#include "ur_rtde_driver/tcp_socket.h" #include "ur_rtde_driver/tcp_socket.h"
namespace ur_rtde_driver
{
#define MAX_SERVER_BUF_LEN 50 #define MAX_SERVER_BUF_LEN 50
class URServer : private TCPSocket class URServer : private TCPSocket
@@ -47,3 +49,4 @@ public:
bool readLine(char* buffer, size_t buf_len); bool readLine(char* buffer, size_t buf_len);
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 ur_rtde_driver

View File

@@ -24,6 +24,8 @@
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/pipeline.h" #include "ur_rtde_driver/pipeline.h"
namespace ur_rtde_driver
{
enum class package_type : uint8_t enum class package_type : uint8_t
{ {
ROBOT_MODE_DATA = 0, ROBOT_MODE_DATA = 0,
@@ -59,3 +61,4 @@ public:
virtual bool parseWith(BinParser& bp) = 0; virtual bool parseWith(BinParser& bp) = 0;
virtual bool consumeWith(URStatePacketConsumer& consumer) = 0; virtual bool consumeWith(URStatePacketConsumer& consumer) = 0;
}; };
} // namespace ur_rtde_driver

View File

@@ -26,6 +26,8 @@
#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
{
template <typename RMD, typename MBD> template <typename RMD, typename MBD>
class URStateParser : public URParser<StatePacket> class URStateParser : public URParser<StatePacket>
{ {
@@ -115,3 +117,4 @@ typedef URStateParser<RobotModeData_V1_X, MasterBoardData_V1_X> URStateParser_V1
typedef URStateParser<RobotModeData_V3_0__1, MasterBoardData_V3_0__1> URStateParser_V3_0__1; typedef URStateParser<RobotModeData_V3_0__1, MasterBoardData_V3_0__1> URStateParser_V3_0__1;
typedef URStateParser<RobotModeData_V3_2, MasterBoardData_V3_2> URStateParser_V3_2; typedef URStateParser<RobotModeData_V3_2, MasterBoardData_V3_2> URStateParser_V3_2;
typedef URStateParser<RobotModeData_V3_5, MasterBoardData_V3_2> URStateParser_V3_5; typedef URStateParser<RobotModeData_V3_5, MasterBoardData_V3_2> URStateParser_V3_5;
} // namespace ur_rtde_driver

View File

@@ -26,6 +26,8 @@
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/tcp_socket.h" #include "ur_rtde_driver/tcp_socket.h"
namespace ur_rtde_driver
{
class URStream : public TCPSocket class URStream : public TCPSocket
{ {
private: private:
@@ -62,3 +64,4 @@ 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 ur_rtde_driver

View File

@@ -21,6 +21,8 @@
#include "ur_rtde_driver/ros/action_server.h" #include "ur_rtde_driver/ros/action_server.h"
#include <cmath> #include <cmath>
namespace ur_rtde_driver
{
ActionServer::ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names, ActionServer::ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names,
double max_velocity) double max_velocity)
: as_(nh_, "follow_joint_trajectory", boost::bind(&ActionServer::onGoal, this, _1), : as_(nh_, "follow_joint_trajectory", boost::bind(&ActionServer::onGoal, this, _1),
@@ -367,3 +369,4 @@ void ActionServer::trajectoryThread()
lk.unlock(); lk.unlock();
} }
} }
} // namespace ur_rtde_driver

View File

@@ -18,6 +18,8 @@
#include "ur_rtde_driver/ros/controller.h" #include "ur_rtde_driver/ros/controller.h"
namespace ur_rtde_driver
{
ROSController::ROSController(URCommander& commander, TrajectoryFollower& follower, ROSController::ROSController(URCommander& commander, TrajectoryFollower& follower,
std::vector<std::string>& joint_names, double max_vel_change, std::string tcp_link) std::vector<std::string>& joint_names, double max_vel_change, std::string tcp_link)
: controller_(this, nh_) : controller_(this, nh_)
@@ -147,3 +149,4 @@ void ROSController::onRobotStateChange(RobotState state)
service_enabled_ = next; service_enabled_ = next;
service_cooldown_ = 125; service_cooldown_ = 125;
} }
} // namespace ur_rtde_driver

View File

@@ -19,6 +19,8 @@
#include "ur_rtde_driver/ros/hardware_interface.h" #include "ur_rtde_driver/ros/hardware_interface.h"
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
namespace ur_rtde_driver
{
const std::string JointInterface::INTERFACE_NAME = "hardware_interface::JointStateInterface"; const std::string JointInterface::INTERFACE_NAME = "hardware_interface::JointStateInterface";
JointInterface::JointInterface(std::vector<std::string>& joint_names) JointInterface::JointInterface(std::vector<std::string>& joint_names)
{ {
@@ -104,3 +106,4 @@ void PositionInterface::stop()
{ {
follower_.stop(); follower_.stop();
} }
} // namespace ur_rtde_driver

View File

@@ -19,6 +19,8 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <cmath> #include <cmath>
namespace ur_rtde_driver
{
static const std::array<double, 6> EMPTY_VALUES = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; static const std::array<double, 6> EMPTY_VALUES = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
static const std::string TIME_INTERVAL("{{TIME_INTERVAL}}"); static const std::string TIME_INTERVAL("{{TIME_INTERVAL}}");
@@ -402,3 +404,4 @@ void LowBandwidthTrajectoryFollower::stop()
server_.disconnectClient(); server_.disconnectClient();
running_ = false; running_ = false;
} }
} // namespace ur_rtde_driver

View File

@@ -18,6 +18,8 @@
#include "ur_rtde_driver/ros/mb_publisher.h" #include "ur_rtde_driver/ros/mb_publisher.h"
namespace ur_rtde_driver
{
inline void appendAnalog(std::vector<ur_msgs::Analog>& vec, double val, uint8_t pin) inline void appendAnalog(std::vector<ur_msgs::Analog>& vec, double val, uint8_t pin)
{ {
ur_msgs::Analog ana; ur_msgs::Analog ana;
@@ -130,3 +132,4 @@ bool MBPublisher::consume(RobotModeData_V3_2& data)
publishRobotStatus(data); publishRobotStatus(data);
return true; return true;
} }
} // namespace ur_rtde_driver

View File

@@ -18,6 +18,8 @@
#include "ur_rtde_driver/ros/rt_publisher.h" #include "ur_rtde_driver/ros/rt_publisher.h"
namespace ur_rtde_driver
{
bool RTPublisher::publishJoints(RTShared& packet, Time& t) bool RTPublisher::publishJoints(RTShared& packet, Time& t)
{ {
sensor_msgs::JointState joint_msg; sensor_msgs::JointState joint_msg;
@@ -134,3 +136,4 @@ bool RTPublisher::consume(RTState_V3_2__3& state)
{ {
return publish(state); return publish(state);
} }
} // namespace ur_rtde_driver

View File

@@ -18,6 +18,8 @@
#include "ur_rtde_driver/ros/service_stopper.h" #include "ur_rtde_driver/ros/service_stopper.h"
namespace ur_rtde_driver
{
ServiceStopper::ServiceStopper(std::vector<Service*> services) ServiceStopper::ServiceStopper(std::vector<Service*> services)
: enable_service_(nh_.advertiseService("ur_driver/robot_enable", &ServiceStopper::enableCallback, this)) : enable_service_(nh_.advertiseService("ur_driver/robot_enable", &ServiceStopper::enableCallback, this))
, services_(services) , services_(services)
@@ -92,3 +94,4 @@ bool ServiceStopper::handle(SharedRobotModeData& data, bool error)
return true; return true;
} }
} // namespace ur_rtde_driver

View File

@@ -83,6 +83,8 @@ def driverProg():
end end
)"; )";
namespace ur_rtde_driver
{
TrajectoryFollower::TrajectoryFollower(URCommander& commander, std::string& reverse_ip, int reverse_port, TrajectoryFollower::TrajectoryFollower(URCommander& commander, std::string& reverse_ip, int reverse_port,
bool version_3) bool version_3)
: running_(false) : running_(false)
@@ -259,3 +261,4 @@ void TrajectoryFollower::stop()
server_.disconnectClient(); server_.disconnectClient();
running_ = false; running_ = false;
} }
} // namespace ur_rtde_driver

View File

@@ -19,6 +19,8 @@
#include "ur_rtde_driver/ros/urscript_handler.h" #include "ur_rtde_driver/ros/urscript_handler.h"
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
namespace ur_rtde_driver
{
URScriptHandler::URScriptHandler(URCommander& commander) : commander_(commander), state_(RobotState::Error) URScriptHandler::URScriptHandler(URCommander& commander) : commander_(commander), state_(RobotState::Error)
{ {
LOG_INFO("Initializing ur_driver/URScript subscriber"); LOG_INFO("Initializing ur_driver/URScript subscriber");
@@ -60,3 +62,4 @@ void URScriptHandler::onRobotStateChange(RobotState state)
{ {
state_ = state; state_ = state;
} }
} // namespace ur_rtde_driver

View File

@@ -61,6 +61,8 @@ static const std::vector<std::string> DEFAULT_JOINTS = { "shoulder_pan_joint", "
static const int UR_SECONDARY_PORT = 30002; static const int UR_SECONDARY_PORT = 30002;
static const int UR_RT_PORT = 30003; static const int UR_RT_PORT = 30003;
using namespace ur_rtde_driver;
struct ProgArgs struct ProgArgs
{ {
public: public:

View File

@@ -27,6 +27,8 @@
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/tcp_socket.h" #include "ur_rtde_driver/tcp_socket.h"
namespace ur_rtde_driver
{
TCPSocket::TCPSocket() : socket_fd_(-1), state_(SocketState::Invalid) TCPSocket::TCPSocket() : socket_fd_(-1), state_(SocketState::Invalid)
{ {
} }
@@ -184,3 +186,4 @@ bool TCPSocket::write(const uint8_t* buf, size_t buf_len, size_t& written)
return true; return true;
} }
} // namespace ur_rtde_driver

View File

@@ -19,6 +19,8 @@
#include "ur_rtde_driver/ur/commander.h" #include "ur_rtde_driver/ur/commander.h"
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
namespace ur_rtde_driver
{
bool URCommander::write(const std::string& s) bool URCommander::write(const std::string& s)
{ {
size_t len = s.size(); size_t len = s.size();
@@ -168,3 +170,4 @@ bool URCommander_V3_3::speedj(std::array<double, 6>& speeds, double acceleration
std::string s(out.str()); std::string s(out.str());
return write(s); return write(s);
} }
} // namespace ur_rtde_driver

View File

@@ -19,6 +19,8 @@
#include "ur_rtde_driver/ur/master_board.h" #include "ur_rtde_driver/ur/master_board.h"
#include "ur_rtde_driver/ur/consumer.h" #include "ur_rtde_driver/ur/consumer.h"
namespace ur_rtde_driver
{
bool SharedMasterBoardData::parseWith(BinParser& bp) bool SharedMasterBoardData::parseWith(BinParser& bp)
{ {
bp.parse(analog_input_range0); bp.parse(analog_input_range0);
@@ -119,3 +121,4 @@ bool MasterBoardData_V3_2::consumeWith(URStatePacketConsumer& consumer)
{ {
return consumer.consume(*this); return consumer.consume(*this);
} }
} // namespace ur_rtde_driver

View File

@@ -19,6 +19,8 @@
#include "ur_rtde_driver/ur/messages.h" #include "ur_rtde_driver/ur/messages.h"
#include "ur_rtde_driver/ur/consumer.h" #include "ur_rtde_driver/ur/consumer.h"
namespace ur_rtde_driver
{
bool VersionMessage::parseWith(BinParser& bp) bool VersionMessage::parseWith(BinParser& bp)
{ {
bp.parse(project_name); bp.parse(project_name);
@@ -35,3 +37,4 @@ bool VersionMessage::consumeWith(URMessagePacketConsumer& consumer)
{ {
return consumer.consume(*this); return consumer.consume(*this);
} }
} // namespace ur_rtde_driver

View File

@@ -19,6 +19,8 @@
#include "ur_rtde_driver/ur/robot_mode.h" #include "ur_rtde_driver/ur/robot_mode.h"
#include "ur_rtde_driver/ur/consumer.h" #include "ur_rtde_driver/ur/consumer.h"
namespace ur_rtde_driver
{
bool SharedRobotModeData::parseWith(BinParser& bp) bool SharedRobotModeData::parseWith(BinParser& bp)
{ {
bp.parse(timestamp); bp.parse(timestamp);
@@ -100,3 +102,4 @@ bool RobotModeData_V3_5::consumeWith(URStatePacketConsumer& consumer)
{ {
return consumer.consume(*this); return consumer.consume(*this);
} }
} // namespace ur_rtde_driver

View File

@@ -19,6 +19,8 @@
#include "ur_rtde_driver/ur/rt_state.h" #include "ur_rtde_driver/ur/rt_state.h"
#include "ur_rtde_driver/ur/consumer.h" #include "ur_rtde_driver/ur/consumer.h"
namespace ur_rtde_driver
{
void RTShared::parse_shared1(BinParser& bp) void RTShared::parse_shared1(BinParser& bp)
{ {
bp.parse(time); bp.parse(time);
@@ -133,3 +135,4 @@ bool RTState_V3_2__3::consumeWith(URRTPacketConsumer& consumer)
{ {
return consumer.consume(*this); return consumer.consume(*this);
} }
} // namespace ur_rtde_driver

View File

@@ -25,6 +25,8 @@
#include <cstring> #include <cstring>
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
namespace ur_rtde_driver
{
URServer::URServer(int port) : port_(port) URServer::URServer(int port) : port_(port)
{ {
} }
@@ -149,3 +151,4 @@ bool URServer::readLine(char* buffer, size_t buf_len)
*current_pointer = '\0'; *current_pointer = '\0';
return true; return true;
} }
} // namespace ur_rtde_driver

View File

@@ -24,6 +24,8 @@
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/ur/stream.h" #include "ur_rtde_driver/ur/stream.h"
namespace ur_rtde_driver
{
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_);
@@ -60,3 +62,4 @@ bool URStream::read(uint8_t* buf, size_t buf_len, size_t& total)
return remainder == 0; return remainder == 0;
} }
} // namespace ur_rtde_driver

View File

@@ -22,6 +22,8 @@
#include "ur_rtde_driver/test/utils.h" #include "ur_rtde_driver/test/utils.h"
#include "ur_rtde_driver/types.h" #include "ur_rtde_driver/types.h"
using namespace ur_rtde_driver;
TEST(MasterBoardData_V1_X, testRandomDataParsing) TEST(MasterBoardData_V1_X, testRandomDataParsing)
{ {
RandomDataTest rdt(71); RandomDataTest rdt(71);

View File

@@ -22,6 +22,7 @@
#include "ur_rtde_driver/test/utils.h" #include "ur_rtde_driver/test/utils.h"
#include "ur_rtde_driver/types.h" #include "ur_rtde_driver/types.h"
using namespace ur_rtde_driver;
TEST(RobotModeData_V1_X, testRandomDataParsing) TEST(RobotModeData_V1_X, testRandomDataParsing)
{ {
RandomDataTest rdt(24); RandomDataTest rdt(24);

View File

@@ -22,6 +22,8 @@
#include "ur_rtde_driver/test/utils.h" #include "ur_rtde_driver/test/utils.h"
#include "ur_rtde_driver/types.h" #include "ur_rtde_driver/types.h"
using namespace ur_rtde_driver;
TEST(RTState_V1_6__7, testRandomDataParsing) TEST(RTState_V1_6__7, testRandomDataParsing)
{ {
RandomDataTest rdt(764); RandomDataTest rdt(764);