Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-12 19: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

18
src/ros/hardware_interface.cpp
View File

@@ -20,7 +20,7 @@
#include "ur_rtde_driver/log.h"
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++)
{
@@ -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;
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));
}
void WrenchInterface::update(RTShared &packet)
void WrenchInterface::update(RTShared& packet)
{
tcp_ = packet.tcp_force;
}
const std::string VelocityInterface::INTERFACE_NAME = "hardware_interface::VelocityJointInterface";
VelocityInterface::VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface,
std::vector<std::string> &joint_names, double max_vel_change)
VelocityInterface::VelocityInterface(URCommander& commander, hardware_interface::JointStateInterface& js_interface,
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 })
{
for (size_t i = 0; i < 6; i++)
@@ -72,16 +72,16 @@ bool VelocityInterface::write()
void VelocityInterface::reset()
{
for (auto &val : prev_velocity_cmd_)
for (auto& val : prev_velocity_cmd_)
{
val = 0;
}
}
const std::string PositionInterface::INTERFACE_NAME = "hardware_interface::PositionJointInterface";
PositionInterface::PositionInterface(TrajectoryFollower &follower,
hardware_interface::JointStateInterface &js_interface,
std::vector<std::string> &joint_names)
PositionInterface::PositionInterface(TrajectoryFollower& follower,
hardware_interface::JointStateInterface& js_interface,
std::vector<std::string>& joint_names)
: follower_(follower)
{
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)
: running_(false)
, commander_(commander)
@@ -323,8 +323,8 @@ bool LowBandwidthTrajectoryFollower::start()
return (running_ = true);
}
bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6> &positions,
const std::array<double, 6> &velocities, double sample_number,
bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6>& positions,
const std::array<double, 6>& velocities, double sample_number,
double time_in_seconds)
{
if (!running_)
@@ -334,11 +334,11 @@ bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6> &positi
out << "(";
out << sample_number << ",";
for (auto const &pos : positions)
for (auto const& pos : positions)
{
out << pos << ",";
}
for (auto const &vel : velocities)
for (auto const& vel : velocities)
{
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
// We have only ASCII characters and we can cast char -> uint_8
const std::string tmp = out.str();
const char *formatted_message = tmp.c_str();
const uint8_t *buf = (uint8_t *)formatted_message;
const char* formatted_message = tmp.c_str();
const uint8_t* buf = (uint8_t*)formatted_message;
size_t written;
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);
}
bool LowBandwidthTrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt)
bool LowBandwidthTrajectoryFollower::execute(std::vector<TrajectoryPoint>& trajectory, std::atomic<bool>& interrupt)
{
if (!running_)
return false;
bool finished = false;
char *line[MAX_SERVER_BUF_LEN];
char* line[MAX_SERVER_BUF_LEN];
bool res = true;
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!");
finished = true;
break;
}
unsigned int message_num = atoi((const char *)line);
unsigned int message_num = atoi((const char*)line);
LOG_DEBUG("Received request %i", message_num);
if (message_num < trajectory.size())
{

18
src/ros/trajectory_follower.cpp
View File

@@ -83,7 +83,7 @@ def driverProg():
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)
: running_(false)
, commander_(commander)
@@ -141,7 +141,7 @@ bool TrajectoryFollower::start()
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_)
return false;
@@ -152,9 +152,9 @@ bool TrajectoryFollower::execute(std::array<double, 6> &positions, bool keep_ali
last_positions_ = positions;
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_);
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);
}
bool TrajectoryFollower::execute(std::array<double, 6> &positions)
bool TrajectoryFollower::execute(std::array<double, 6>& positions)
{
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_)
return false;
@@ -193,15 +193,15 @@ bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::
typedef high_resolution_clock Clock;
typedef Clock::time_point Time;
auto &last = trajectory[trajectory.size() - 1];
auto &prev = trajectory[0];
auto& last = trajectory[trajectory.size() - 1];
auto& prev = trajectory[0];
Time t0 = Clock::now();
Time latest = t0;
std::array<double, 6> positions;
for (auto const &point : trajectory)
for (auto const& point : trajectory)
{
// skip t0
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))
{
@@ -128,13 +128,13 @@ bool parse_args(ProgArgs &args)
return true;
}
std::string getLocalIPAccessibleFromHost(std::string &host)
std::string getLocalIPAccessibleFromHost(std::string& host)
{
URStream stream(host, UR_RT_PORT);
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");
@@ -151,7 +151,7 @@ int main(int argc, char **argv)
std::string local_ip(getLocalIPAccessibleFromHost(args.host));
URFactory factory(args.host);
vector<Service *> services;
vector<Service*> services;
// RT packets
auto rt_parser = factory.getRTParser();
@@ -159,15 +159,15 @@ int main(int argc, char **argv)
URProducer<RTPacket> rt_prod(rt_stream, *rt_parser);
RTPublisher rt_pub(args.prefix, args.base_frame, args.tool_frame, args.use_ros_control);
auto rt_commander = factory.getCommander(rt_stream);
vector<IConsumer<RTPacket> *> rt_vec{ &rt_pub };
vector<IConsumer<RTPacket>*> rt_vec{ &rt_pub };
INotifier *notifier(nullptr);
ROSController *controller(nullptr);
ActionServer *action_server(nullptr);
INotifier* notifier(nullptr);
ROSController* controller(nullptr);
ActionServer* action_server(nullptr);
if (args.use_ros_control)
{
LOG_INFO("ROS control enabled");
TrajectoryFollower *traj_follower =
TrajectoryFollower* traj_follower =
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);
rt_vec.push_back(controller);
@@ -176,7 +176,7 @@ int main(int argc, char **argv)
else
{
LOG_INFO("ActionServer enabled");
ActionTrajectoryFollowerInterface *traj_follower(nullptr);
ActionTrajectoryFollowerInterface* traj_follower(nullptr);
if (args.use_lowbandwidth_trajectory_follower)
{
LOG_INFO("Use low bandwidth trajectory follower");
@@ -217,7 +217,7 @@ int main(int argc, char **argv)
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);
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));
}
bool TCPSocket::setup(std::string &host, int port)
bool TCPSocket::setup(std::string& host, int port)
{
if (state_ == SocketState::Connected)
return false;
@@ -52,7 +52,7 @@ bool TCPSocket::setup(std::string &host, int port)
// gethostbyname() is deprecated so use getadderinfo() as described in:
// 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);
struct addrinfo hints, *result;
std::memset(&hints, 0, sizeof(hints));
@@ -69,7 +69,7 @@ bool TCPSocket::setup(std::string &host, int port)
bool connected = false;
// 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);
@@ -118,7 +118,7 @@ std::string TCPSocket::getIP()
{
sockaddr_in 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)
{
@@ -131,16 +131,16 @@ std::string TCPSocket::getIP()
return std::string(buf);
}
bool TCPSocket::read(char *character)
bool TCPSocket::read(char* character)
{
size_t read_chars;
// It's inefficient, but in our case we read very small messages
// and the overhead connected with reading character by character is
// 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;
@@ -161,7 +161,7 @@ bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read)
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;

16
src/ur/commander.cpp
View File

@@ -19,18 +19,18 @@
#include "ur_rtde_driver/ur/commander.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();
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;
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("[");
for (auto const &val : values)
for (auto const& val : values)
{
out << mod << val;
mod = ",";
@@ -38,7 +38,7 @@ void URCommander::formatArray(std::ostringstream &out, std::array<double, 6> &va
out << "]";
}
bool URCommander::uploadProg(const std::string &s)
bool URCommander::uploadProg(const std::string& s)
{
LOG_DEBUG("Sending program [%s]", s.c_str());
return write(s);
@@ -78,7 +78,7 @@ bool URCommander::stopj(double a)
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;
out << std::fixed << std::setprecision(5);
@@ -147,7 +147,7 @@ bool URCommander_V3_X::setDigitalOut(uint8_t pin, bool value)
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;
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);
}
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;
out << std::fixed << std::setprecision(5);