Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 18:10:47 +02:00
Code Releases Activity

Updated clang-format and added clang-tidy instructions

Browse Source
This commit is contained in:
Felix Mauch
2019-04-01 11:18:59 +02:00
parent 5934612a03
commit 99533e42d8
16 changed files with 133 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
.clang-format
View File

@@ -17,7 +17,8 @@ BreakConstructorInitializersBeforeComma: true
BinPackParameters: true BinPackParameters: true
ColumnLimit: 120 ColumnLimit: 120
ConstructorInitializerAllOnOneLineOrOnePerLine: true ConstructorInitializerAllOnOneLineOrOnePerLine: true
DerivePointerBinding: true DerivePointerBinding: false
PointerBindsToType: true
ExperimentalAutoDetectBinPacking: false ExperimentalAutoDetectBinPacking: false
IndentCaseLabels: true IndentCaseLabels: true
MaxEmptyLinesToKeep: 1 MaxEmptyLinesToKeep: 1
@@ -29,14 +30,12 @@ PenaltyBreakString: 1
PenaltyBreakFirstLessLess: 1000 PenaltyBreakFirstLessLess: 1000
PenaltyExcessCharacter: 1000 PenaltyExcessCharacter: 1000
PenaltyReturnTypeOnItsOwnLine: 90 PenaltyReturnTypeOnItsOwnLine: 90
PointerBindsToType: false
SpacesBeforeTrailingComments: 2 SpacesBeforeTrailingComments: 2
Cpp11BracedListStyle: false Cpp11BracedListStyle: false
Standard: Auto Standard: Auto
IndentWidth: 2 IndentWidth: 2
TabWidth: 2 TabWidth: 2
UseTab: Never UseTab: Never
BreakBeforeBraces: Allman
IndentFunctionDeclarationAfterType: false IndentFunctionDeclarationAfterType: false
SpacesInParentheses: false SpacesInParentheses: false
SpacesInAngles: false SpacesInAngles: false
@@ -45,4 +44,23 @@ SpacesInCStyleCastParentheses: false
SpaceAfterControlStatementKeyword: true SpaceAfterControlStatementKeyword: true
SpaceBeforeAssignmentOperators: true SpaceBeforeAssignmentOperators: true
ContinuationIndentWidth: 4 ContinuationIndentWidth: 4
SortIncludes: false
SpaceAfterCStyleCast: false
# Configure each individual brace in BraceWrapping
BreakBeforeBraces: Custom
# Control of individual brace wrapping cases
BraceWrapping: {
AfterClass: 'true'
AfterControlStatement: 'true'
AfterEnum : 'true'
AfterFunction : 'true'
AfterNamespace : 'true'
AfterStruct : 'true'
AfterUnion : 'true'
BeforeCatch : 'true'
BeforeElse : 'true'
IndentBraces : 'false'
}
... ...

10
.clang-tidy Normal file
View File

@@ -0,0 +1,10 @@
Checks: '-*,readability-identifier-naming'
CheckOptions:
- { key: readability-identifier-naming.NamespaceCase, value: lower_case }
- { key: readability-identifier-naming.ClassCase, value: CamelCase }
- { key: readability-identifier-naming.PrivateMemberSuffix, value: _ }
- { key: readability-identifier-naming.StructCase, value: CamelCase }
- { key: readability-identifier-naming.FunctionCase, value: camelBack }
- { key: readability-identifier-naming.VariableCase, value: lower_case }
- { key: readability-identifier-naming.GlobalVariablePrefix, value: g_ }
- { key: readability-identifier-naming.GlobalConstantCase, value: UPPER_CASE }

4
include/ur_rtde_driver/ros/action_trajectory_follower_interface.h
View File

@@ -36,7 +36,7 @@ struct TrajectoryPoint
{ {
} }
TrajectoryPoint(std::array<double, 6> &pos, std::array<double, 6> &vel, std::chrono::microseconds tfs) TrajectoryPoint(std::array<double, 6>& pos, std::array<double, 6>& vel, std::chrono::microseconds tfs)
: positions(pos), velocities(vel), time_from_start(tfs) : positions(pos), velocities(vel), time_from_start(tfs)
{ {
} }
@@ -46,7 +46,7 @@ class ActionTrajectoryFollowerInterface
{ {
public: public:
virtual bool start() = 0; virtual bool start() = 0;
virtual bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt) = 0; virtual bool execute(std::vector<TrajectoryPoint>& trajectory, std::atomic<bool>& interrupt) = 0;
virtual void stop() = 0; virtual void stop() = 0;
virtual ~ActionTrajectoryFollowerInterface(){}; virtual ~ActionTrajectoryFollowerInterface(){};
}; };

18
include/ur_rtde_driver/ros/hardware_interface.h
View File

@@ -49,8 +49,8 @@ private:
std::array<double, 6> velocities_, positions_, efforts_; std::array<double, 6> velocities_, positions_, efforts_;
public: public:
JointInterface(std::vector<std::string> &joint_names); JointInterface(std::vector<std::string>& joint_names);
void update(RTShared &packet); void update(RTShared& packet);
typedef hardware_interface::JointStateInterface parent_type; typedef hardware_interface::JointStateInterface parent_type;
static const std::string INTERFACE_NAME; static const std::string INTERFACE_NAME;
@@ -62,7 +62,7 @@ class WrenchInterface : public hardware_interface::ForceTorqueSensorInterface
public: public:
WrenchInterface(std::string tcp_link); WrenchInterface(std::string tcp_link);
void update(RTShared &packet); void update(RTShared& packet);
typedef hardware_interface::ForceTorqueSensorInterface parent_type; typedef hardware_interface::ForceTorqueSensorInterface parent_type;
static const std::string INTERFACE_NAME; static const std::string INTERFACE_NAME;
}; };
@@ -70,13 +70,13 @@ public:
class VelocityInterface : public HardwareInterface, public hardware_interface::VelocityJointInterface class VelocityInterface : public HardwareInterface, public hardware_interface::VelocityJointInterface
{ {
private: private:
URCommander &commander_; URCommander& commander_;
std::array<double, 6> velocity_cmd_, prev_velocity_cmd_; std::array<double, 6> velocity_cmd_, prev_velocity_cmd_;
double max_vel_change_; double max_vel_change_;
public: public:
VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, VelocityInterface(URCommander& commander, hardware_interface::JointStateInterface& js_interface,
std::vector<std::string> &joint_names, double max_vel_change); std::vector<std::string>& joint_names, double max_vel_change);
virtual bool write(); virtual bool write();
virtual void reset(); virtual void reset();
typedef hardware_interface::VelocityJointInterface parent_type; typedef hardware_interface::VelocityJointInterface parent_type;
@@ -86,12 +86,12 @@ public:
class PositionInterface : public HardwareInterface, public hardware_interface::PositionJointInterface class PositionInterface : public HardwareInterface, public hardware_interface::PositionJointInterface
{ {
private: private:
TrajectoryFollower &follower_; TrajectoryFollower& follower_;
std::array<double, 6> position_cmd_; std::array<double, 6> position_cmd_;
public: public:
PositionInterface(TrajectoryFollower &follower, hardware_interface::JointStateInterface &js_interface, PositionInterface(TrajectoryFollower& follower, hardware_interface::JointStateInterface& js_interface,
std::vector<std::string> &joint_names); std::vector<std::string>& joint_names);
virtual bool write(); virtual bool write();
virtual void start(); virtual void start();
virtual void stop(); virtual void stop();

8
include/ur_rtde_driver/ros/lowbandwidth_trajectory_follower.h
View File

@@ -34,7 +34,7 @@ class LowBandwidthTrajectoryFollower : public ActionTrajectoryFollowerInterface
private: private:
std::atomic<bool> running_; std::atomic<bool> running_;
std::array<double, 6> last_positions_; std::array<double, 6> last_positions_;
URCommander &commander_; URCommander& commander_;
URServer server_; URServer server_;
double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, servoj_gain_, servoj_lookahead_time_, double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, servoj_gain_, servoj_lookahead_time_,
@@ -42,14 +42,14 @@ private:
std::string program_; std::string program_;
bool execute(const std::array<double, 6> &positions, const std::array<double, 6> &velocities, double sample_number, bool execute(const std::array<double, 6>& positions, const std::array<double, 6>& velocities, double sample_number,
double time_in_seconds); double time_in_seconds);
public: public:
LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3); LowBandwidthTrajectoryFollower(URCommander& commander, std::string& reverse_ip, int reverse_port, bool version_3);
bool start(); bool start();
bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt); bool execute(std::vector<TrajectoryPoint>& trajectory, std::atomic<bool>& interrupt);
void stop(); void stop();
virtual ~LowBandwidthTrajectoryFollower(){}; virtual ~LowBandwidthTrajectoryFollower(){};

12
include/ur_rtde_driver/ros/trajectory_follower.h
View File

@@ -38,29 +38,29 @@ class TrajectoryFollower : public ActionTrajectoryFollowerInterface
private: private:
std::atomic<bool> running_; std::atomic<bool> running_;
std::array<double, 6> last_positions_; std::array<double, 6> last_positions_;
URCommander &commander_; URCommander& commander_;
URServer server_; URServer server_;
double servoj_time_, servoj_lookahead_time_, servoj_gain_; double servoj_time_, servoj_lookahead_time_, servoj_gain_;
std::string program_; std::string program_;
template <typename T> template <typename T>
size_t append(uint8_t *buffer, T &val) size_t append(uint8_t* buffer, T& val)
{ {
size_t s = sizeof(T); size_t s = sizeof(T);
std::memcpy(buffer, &val, s); std::memcpy(buffer, &val, s);
return s; return s;
} }
bool execute(std::array<double, 6> &positions, bool keep_alive); bool execute(std::array<double, 6>& positions, bool keep_alive);
double interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel); double interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel);
public: public:
TrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3); TrajectoryFollower(URCommander& commander, std::string& reverse_ip, int reverse_port, bool version_3);
bool start(); bool start();
bool execute(std::array<double, 6> &positions); bool execute(std::array<double, 6>& positions);
bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt); bool execute(std::vector<TrajectoryPoint>& trajectory, std::atomic<bool>& interrupt);
void stop(); void stop();
virtual ~TrajectoryFollower(){}; virtual ~TrajectoryFollower(){};

6
include/ur_rtde_driver/ros/urscript_handler.h
View File

@@ -30,12 +30,12 @@ class URScriptHandler : public Service
{ {
private: private:
ros::NodeHandle nh_; ros::NodeHandle nh_;
URCommander &commander_; URCommander& commander_;
ros::Subscriber urscript_sub_; ros::Subscriber urscript_sub_;
RobotState state_; RobotState state_;
public: public:
URScriptHandler(URCommander &commander); URScriptHandler(URCommander& commander);
void urscriptInterface(const std_msgs::String::ConstPtr &msg); void urscriptInterface(const std_msgs::String::ConstPtr& msg);
void onRobotStateChange(RobotState state); void onRobotStateChange(RobotState state);
}; };

10
include/ur_rtde_driver/tcp_socket.h
View File

@@ -39,13 +39,13 @@ private:
std::atomic<SocketState> state_; std::atomic<SocketState> state_;
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)
{ {
return false; return false;
} }
virtual void setOptions(int socket_fd); virtual void setOptions(int socket_fd);
bool setup(std::string &host, int port); bool setup(std::string& host, int port);
public: public:
TCPSocket(); TCPSocket();
@@ -64,9 +64,9 @@ public:
std::string getIP(); std::string getIP();
bool read(char *character); bool read(char* character);
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);
void close(); void close();
}; };

28
include/ur_rtde_driver/ur/commander.h
View File

@@ -25,23 +25,23 @@
class URCommander class URCommander
{ {
private: private:
URStream &stream_; 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(URStream& stream) : stream_(stream)
{ {
} }
virtual bool speedj(std::array<double, 6> &speeds, double acceleration) = 0; virtual bool speedj(std::array<double, 6>& speeds, double acceleration) = 0;
virtual bool setDigitalOut(uint8_t pin, bool value) = 0; virtual bool setDigitalOut(uint8_t pin, bool value) = 0;
virtual bool setAnalogOut(uint8_t pin, double value) = 0; virtual bool setAnalogOut(uint8_t pin, double value) = 0;
// shared // shared
bool uploadProg(const std::string &s); bool uploadProg(const std::string& s);
bool stopj(double a = 10.0); bool stopj(double a = 10.0);
bool setToolVoltage(uint8_t voltage); bool setToolVoltage(uint8_t voltage);
bool setFlag(uint8_t pin, bool value); bool setFlag(uint8_t pin, bool value);
@@ -51,11 +51,11 @@ 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(URStream& stream) : URCommander(stream)
{ {
} }
virtual bool speedj(std::array<double, 6> &speeds, double acceleration); virtual bool speedj(std::array<double, 6>& speeds, double acceleration);
virtual bool setDigitalOut(uint8_t pin, bool value); virtual bool setDigitalOut(uint8_t pin, bool value);
virtual bool setAnalogOut(uint8_t pin, double value); virtual bool setAnalogOut(uint8_t pin, double value);
}; };
@@ -63,11 +63,11 @@ 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(URStream& stream) : URCommander(stream)
{ {
} }
virtual bool speedj(std::array<double, 6> &speeds, double acceleration) = 0; virtual bool speedj(std::array<double, 6>& speeds, double acceleration) = 0;
virtual bool setDigitalOut(uint8_t pin, bool value); virtual bool setDigitalOut(uint8_t pin, bool value);
virtual bool setAnalogOut(uint8_t pin, double value); virtual bool setAnalogOut(uint8_t pin, double value);
}; };
@@ -75,19 +75,19 @@ 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(URStream& stream) : URCommander_V3_X(stream)
{ {
} }
virtual bool speedj(std::array<double, 6> &speeds, double acceleration); virtual bool speedj(std::array<double, 6>& speeds, double acceleration);
}; };
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(URStream& stream) : URCommander_V3_X(stream)
{ {
} }
virtual bool speedj(std::array<double, 6> &speeds, double acceleration); virtual bool speedj(std::array<double, 6>& speeds, double acceleration);
}; };

6
include/ur_rtde_driver/ur/server.h
View File

@@ -35,7 +35,7 @@ private:
TCPSocket client_; 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);
public: public:
URServer(int port); URServer(int port);
@@ -44,6 +44,6 @@ public:
bool bind(); bool bind();
bool accept(); bool accept();
void disconnectClient(); void disconnectClient();
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);
}; };

18
src/ros/hardware_interface.cpp
View File

@@ -20,7 +20,7 @@
#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/log.h"
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)
{ {
for (size_t i = 0; i < 6; i++) for (size_t i = 0; i < 6; i++)
{ {
@@ -28,7 +28,7 @@ JointInterface::JointInterface(std::vector<std::string> &joint_names)
} }
} }
void JointInterface::update(RTShared &packet) void JointInterface::update(RTShared& packet)
{ {
positions_ = packet.q_actual; positions_ = packet.q_actual;
velocities_ = packet.qd_actual; velocities_ = packet.qd_actual;
@@ -41,14 +41,14 @@ WrenchInterface::WrenchInterface(std::string tcp_link)
registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", tcp_link, tcp_.begin(), tcp_.begin() + 3)); registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", tcp_link, tcp_.begin(), tcp_.begin() + 3));
} }
void WrenchInterface::update(RTShared &packet) void WrenchInterface::update(RTShared& packet)
{ {
tcp_ = packet.tcp_force; tcp_ = packet.tcp_force;
} }
const std::string VelocityInterface::INTERFACE_NAME = "hardware_interface::VelocityJointInterface"; const std::string VelocityInterface::INTERFACE_NAME = "hardware_interface::VelocityJointInterface";
VelocityInterface::VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, VelocityInterface::VelocityInterface(URCommander& commander, hardware_interface::JointStateInterface& js_interface,
std::vector<std::string> &joint_names, double max_vel_change) std::vector<std::string>& joint_names, double max_vel_change)
: commander_(commander), max_vel_change_(max_vel_change), prev_velocity_cmd_({ 0, 0, 0, 0, 0, 0 }) : commander_(commander), max_vel_change_(max_vel_change), prev_velocity_cmd_({ 0, 0, 0, 0, 0, 0 })
{ {
for (size_t i = 0; i < 6; i++) for (size_t i = 0; i < 6; i++)
@@ -72,16 +72,16 @@ bool VelocityInterface::write()
void VelocityInterface::reset() void VelocityInterface::reset()
{ {
for (auto &val : prev_velocity_cmd_) for (auto& val : prev_velocity_cmd_)
{ {
val = 0; val = 0;
} }
} }
const std::string PositionInterface::INTERFACE_NAME = "hardware_interface::PositionJointInterface"; const std::string PositionInterface::INTERFACE_NAME = "hardware_interface::PositionJointInterface";
PositionInterface::PositionInterface(TrajectoryFollower &follower, PositionInterface::PositionInterface(TrajectoryFollower& follower,
hardware_interface::JointStateInterface &js_interface, hardware_interface::JointStateInterface& js_interface,
std::vector<std::string> &joint_names) std::vector<std::string>& joint_names)
: follower_(follower) : follower_(follower)
{ {
for (size_t i = 0; i < 6; i++) for (size_t i = 0; i < 6; i++)

22
src/ros/lowbandwidth_trajectory_follower.cpp
View File

@@ -249,7 +249,7 @@ end
)"; )";
LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander& commander, std::string& reverse_ip,
int reverse_port, bool version_3) int reverse_port, bool version_3)
: running_(false) : running_(false)
, commander_(commander) , commander_(commander)
@@ -323,8 +323,8 @@ bool LowBandwidthTrajectoryFollower::start()
return (running_ = true); return (running_ = true);
} }
bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6> &positions, bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6>& positions,
const std::array<double, 6> &velocities, double sample_number, const std::array<double, 6>& velocities, double sample_number,
double time_in_seconds) double time_in_seconds)
{ {
if (!running_) if (!running_)
@@ -334,11 +334,11 @@ bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6> &positi
out << "("; out << "(";
out << sample_number << ","; out << sample_number << ",";
for (auto const &pos : positions) for (auto const& pos : positions)
{ {
out << pos << ","; out << pos << ",";
} }
for (auto const &vel : velocities) for (auto const& vel : velocities)
{ {
out << vel << ","; out << vel << ",";
} }
@@ -347,8 +347,8 @@ bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6> &positi
// I know it's ugly but it's the most efficient and fastest way // I know it's ugly but it's the most efficient and fastest way
// We have only ASCII characters and we can cast char -> uint_8 // We have only ASCII characters and we can cast char -> uint_8
const std::string tmp = out.str(); const std::string tmp = out.str();
const char *formatted_message = tmp.c_str(); const char* formatted_message = tmp.c_str();
const uint8_t *buf = (uint8_t *)formatted_message; const uint8_t* buf = (uint8_t*)formatted_message;
size_t written; size_t written;
LOG_DEBUG("Sending message %s", formatted_message); LOG_DEBUG("Sending message %s", formatted_message);
@@ -356,26 +356,26 @@ bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6> &positi
return server_.write(buf, strlen(formatted_message) + 1, written); return server_.write(buf, strlen(formatted_message) + 1, written);
} }
bool LowBandwidthTrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt) bool LowBandwidthTrajectoryFollower::execute(std::vector<TrajectoryPoint>& trajectory, std::atomic<bool>& interrupt)
{ {
if (!running_) if (!running_)
return false; return false;
bool finished = false; bool finished = false;
char *line[MAX_SERVER_BUF_LEN]; char* line[MAX_SERVER_BUF_LEN];
bool res = true; bool res = true;
while (!finished && !interrupt) while (!finished && !interrupt)
{ {
if (!server_.readLine((char *)line, MAX_SERVER_BUF_LEN)) if (!server_.readLine((char*)line, MAX_SERVER_BUF_LEN))
{ {
LOG_DEBUG("Connection closed. Finishing!"); LOG_DEBUG("Connection closed. Finishing!");
finished = true; finished = true;
break; break;
} }
unsigned int message_num = atoi((const char *)line); unsigned int message_num = atoi((const char*)line);
LOG_DEBUG("Received request %i", message_num); LOG_DEBUG("Received request %i", message_num);
if (message_num < trajectory.size()) if (message_num < trajectory.size())
{ {

18
src/ros/trajectory_follower.cpp
View File

@@ -83,7 +83,7 @@ def driverProg():
end end
)"; )";
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)
, commander_(commander) , commander_(commander)
@@ -141,7 +141,7 @@ bool TrajectoryFollower::start()
return (running_ = true); return (running_ = true);
} }
bool TrajectoryFollower::execute(std::array<double, 6> &positions, bool keep_alive) bool TrajectoryFollower::execute(std::array<double, 6>& positions, bool keep_alive)
{ {
if (!running_) if (!running_)
return false; return false;
@@ -152,9 +152,9 @@ bool TrajectoryFollower::execute(std::array<double, 6> &positions, bool keep_ali
last_positions_ = positions; last_positions_ = positions;
uint8_t buf[sizeof(uint32_t) * 7]; uint8_t buf[sizeof(uint32_t) * 7];
uint8_t *idx = buf; uint8_t* idx = buf;
for (auto const &pos : positions) for (auto const& pos : positions)
{ {
int32_t val = static_cast<int32_t>(pos * MULT_JOINTSTATE_); int32_t val = static_cast<int32_t>(pos * MULT_JOINTSTATE_);
val = htobe32(val); val = htobe32(val);
@@ -178,12 +178,12 @@ double TrajectoryFollower::interpolate(double t, double T, double p0_pos, double
return a + b * t + c * pow(t, 2) + d * pow(t, 3); return a + b * t + c * pow(t, 2) + d * pow(t, 3);
} }
bool TrajectoryFollower::execute(std::array<double, 6> &positions) bool TrajectoryFollower::execute(std::array<double, 6>& positions)
{ {
return execute(positions, true); return execute(positions, true);
} }
bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt) bool TrajectoryFollower::execute(std::vector<TrajectoryPoint>& trajectory, std::atomic<bool>& interrupt)
{ {
if (!running_) if (!running_)
return false; return false;
@@ -193,15 +193,15 @@ bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::
typedef high_resolution_clock Clock; typedef high_resolution_clock Clock;
typedef Clock::time_point Time; typedef Clock::time_point Time;
auto &last = trajectory[trajectory.size() - 1]; auto& last = trajectory[trajectory.size() - 1];
auto &prev = trajectory[0]; auto& prev = trajectory[0];
Time t0 = Clock::now(); Time t0 = Clock::now();
Time latest = t0; Time latest = t0;
std::array<double, 6> positions; std::array<double, 6> positions;
for (auto const &point : trajectory) for (auto const& point : trajectory)
{ {
// skip t0 // skip t0
if (&point == &prev) if (&point == &prev)

22
src/ros_main.cpp
View File

@@ -107,7 +107,7 @@ public:
} }
}; };
bool parse_args(ProgArgs &args) bool parse_args(ProgArgs& args)
{ {
if (!ros::param::get(IP_ADDR_ARG, args.host)) if (!ros::param::get(IP_ADDR_ARG, args.host))
{ {
@@ -128,13 +128,13 @@ bool parse_args(ProgArgs &args)
return true; return true;
} }
std::string getLocalIPAccessibleFromHost(std::string &host) std::string getLocalIPAccessibleFromHost(std::string& host)
{ {
URStream stream(host, UR_RT_PORT); URStream stream(host, UR_RT_PORT);
return stream.connect() ? stream.getIP() : std::string(); return stream.connect() ? stream.getIP() : std::string();
} }
int main(int argc, char **argv) int main(int argc, char** argv)
{ {
ros::init(argc, argv, "ur_driver"); ros::init(argc, argv, "ur_driver");
@@ -151,7 +151,7 @@ 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; vector<Service*> services;
// RT packets // RT packets
auto rt_parser = factory.getRTParser(); auto rt_parser = factory.getRTParser();
@@ -159,15 +159,15 @@ int main(int argc, char **argv)
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 }; vector<IConsumer<RTPacket>*> rt_vec{ &rt_pub };
INotifier *notifier(nullptr); 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)
{ {
LOG_INFO("ROS control enabled"); LOG_INFO("ROS control enabled");
TrajectoryFollower *traj_follower = TrajectoryFollower* traj_follower =
new TrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3()); new TrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
controller = new ROSController(*rt_commander, *traj_follower, args.joint_names, args.max_vel_change, args.tcp_link); controller = new ROSController(*rt_commander, *traj_follower, args.joint_names, args.max_vel_change, args.tcp_link);
rt_vec.push_back(controller); rt_vec.push_back(controller);
@@ -176,7 +176,7 @@ int main(int argc, char **argv)
else else
{ {
LOG_INFO("ActionServer enabled"); LOG_INFO("ActionServer enabled");
ActionTrajectoryFollowerInterface *traj_follower(nullptr); ActionTrajectoryFollowerInterface* traj_follower(nullptr);
if (args.use_lowbandwidth_trajectory_follower) if (args.use_lowbandwidth_trajectory_follower)
{ {
LOG_INFO("Use low bandwidth trajectory follower"); LOG_INFO("Use low bandwidth trajectory follower");
@@ -217,7 +217,7 @@ int main(int argc, char **argv)
ServiceStopper service_stopper(services); ServiceStopper service_stopper(services);
vector<IConsumer<StatePacket> *> state_vec{ &state_pub, &service_stopper }; vector<IConsumer<StatePacket>*> state_vec{ &state_pub, &service_stopper };
MultiConsumer<StatePacket> state_cons(state_vec); MultiConsumer<StatePacket> state_cons(state_vec);
Pipeline<StatePacket> state_pl(state_prod, state_cons, "StatePacket", *notifier); Pipeline<StatePacket> state_pl(state_prod, state_cons, "StatePacket", *notifier);

16
src/tcp_socket.cpp
View File

@@ -42,7 +42,7 @@ void TCPSocket::setOptions(int socket_fd)
setsockopt(socket_fd, IPPROTO_TCP, TCP_QUICKACK, &flag, sizeof(int)); setsockopt(socket_fd, IPPROTO_TCP, TCP_QUICKACK, &flag, sizeof(int));
} }
bool TCPSocket::setup(std::string &host, int port) bool TCPSocket::setup(std::string& host, int port)
{ {
if (state_ == SocketState::Connected) if (state_ == SocketState::Connected)
return false; return false;
@@ -52,7 +52,7 @@ bool TCPSocket::setup(std::string &host, int port)
// gethostbyname() is deprecated so use getadderinfo() as described in: // gethostbyname() is deprecated so use getadderinfo() as described in:
// http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo // http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo
const char *host_name = host.empty() ? nullptr : host.c_str(); const char* host_name = host.empty() ? nullptr : host.c_str();
std::string service = std::to_string(port); std::string service = std::to_string(port);
struct addrinfo hints, *result; struct addrinfo hints, *result;
std::memset(&hints, 0, sizeof(hints)); std::memset(&hints, 0, sizeof(hints));
@@ -69,7 +69,7 @@ bool TCPSocket::setup(std::string &host, int port)
bool connected = false; bool connected = false;
// loop through the list of addresses untill we find one that's connectable // loop through the list of addresses untill we find one that's connectable
for (struct addrinfo *p = result; p != nullptr; p = p->ai_next) for (struct addrinfo* p = result; p != nullptr; p = p->ai_next)
{ {
socket_fd_ = ::socket(p->ai_family, p->ai_socktype, p->ai_protocol); socket_fd_ = ::socket(p->ai_family, p->ai_socktype, p->ai_protocol);
@@ -118,7 +118,7 @@ std::string TCPSocket::getIP()
{ {
sockaddr_in name; sockaddr_in name;
socklen_t len = sizeof(name); socklen_t len = sizeof(name);
int res = ::getsockname(socket_fd_, (sockaddr *)&name, &len); int res = ::getsockname(socket_fd_, (sockaddr*)&name, &len);
if (res < 0) if (res < 0)
{ {
@@ -131,16 +131,16 @@ std::string TCPSocket::getIP()
return std::string(buf); return std::string(buf);
} }
bool TCPSocket::read(char *character) bool TCPSocket::read(char* character)
{ {
size_t read_chars; size_t read_chars;
// It's inefficient, but in our case we read very small messages // It's inefficient, but in our case we read very small messages
// and the overhead connected with reading character by character is // and the overhead connected with reading character by character is
// negligible - adding buffering would complicate the code needlessly. // negligible - adding buffering would complicate the code needlessly.
return read((uint8_t *)character, 1, read_chars); return read((uint8_t*)character, 1, read_chars);
} }
bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read) bool TCPSocket::read(uint8_t* buf, size_t buf_len, size_t& read)
{ {
read = 0; read = 0;
@@ -161,7 +161,7 @@ bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read)
return true; return true;
} }
bool TCPSocket::write(const uint8_t *buf, size_t buf_len, size_t &written) bool TCPSocket::write(const uint8_t* buf, size_t buf_len, size_t& written)
{ {
written = 0; written = 0;

16
src/ur/commander.cpp
View File

@@ -19,18 +19,18 @@
#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"
bool URCommander::write(const std::string &s) bool URCommander::write(const std::string& s)
{ {
size_t len = s.size(); size_t len = s.size();
const uint8_t *data = reinterpret_cast<const uint8_t *>(s.c_str()); const uint8_t* data = reinterpret_cast<const uint8_t*>(s.c_str());
size_t written; size_t written;
return stream_.write(data, len, written); return stream_.write(data, len, written);
} }
void URCommander::formatArray(std::ostringstream &out, std::array<double, 6> &values) void URCommander::formatArray(std::ostringstream& out, std::array<double, 6>& values)
{ {
std::string mod("["); std::string mod("[");
for (auto const &val : values) for (auto const& val : values)
{ {
out << mod << val; out << mod << val;
mod = ","; mod = ",";
@@ -38,7 +38,7 @@ void URCommander::formatArray(std::ostringstream &out, std::array<double, 6> &va
out << "]"; out << "]";
} }
bool URCommander::uploadProg(const std::string &s) bool URCommander::uploadProg(const std::string& s)
{ {
LOG_DEBUG("Sending program [%s]", s.c_str()); LOG_DEBUG("Sending program [%s]", s.c_str());
return write(s); return write(s);
@@ -78,7 +78,7 @@ bool URCommander::stopj(double a)
return write(s); return write(s);
} }
bool URCommander_V1_X::speedj(std::array<double, 6> &speeds, double acceleration) bool URCommander_V1_X::speedj(std::array<double, 6>& speeds, double acceleration)
{ {
std::ostringstream out; std::ostringstream out;
out << std::fixed << std::setprecision(5); out << std::fixed << std::setprecision(5);
@@ -147,7 +147,7 @@ bool URCommander_V3_X::setDigitalOut(uint8_t pin, bool value)
return write(s); return write(s);
} }
bool URCommander_V3_1__2::speedj(std::array<double, 6> &speeds, double acceleration) bool URCommander_V3_1__2::speedj(std::array<double, 6>& speeds, double acceleration)
{ {
std::ostringstream out; std::ostringstream out;
out << std::fixed << std::setprecision(5); out << std::fixed << std::setprecision(5);
@@ -158,7 +158,7 @@ bool URCommander_V3_1__2::speedj(std::array<double, 6> &speeds, double accelerat
return write(s); return write(s);
} }
bool URCommander_V3_3::speedj(std::array<double, 6> &speeds, double acceleration) bool URCommander_V3_3::speedj(std::array<double, 6>& speeds, double acceleration)
{ {
std::ostringstream out; std::ostringstream out;
out << std::fixed << std::setprecision(5); out << std::fixed << std::setprecision(5);