Format sources with clang-format

include/ur_modern_driver/pipeline.h

@@ -110,7 +110,6 @@ public:
   }
 };
 
-
 template <typename T>
 class Pipeline
 {

include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h

@@ -9,9 +9,9 @@
 #include <thread>
 #include <vector>
 #include "ur_modern_driver/log.h"
+#include "ur_modern_driver/ros/action_trajectory_follower_interface.h"
 #include "ur_modern_driver/ur/commander.h"
 #include "ur_modern_driver/ur/server.h"
-#include "ur_modern_driver/ros/action_trajectory_follower_interface.h"
 
 class LowBandwidthTrajectoryFollower : public ActionTrajectoryFollowerInterface
 {
@@ -21,14 +21,13 @@ private:
   URCommander &commander_;
   URServer server_;
 
-  double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, \
-         servoj_gain_, servoj_lookahead_time_, max_joint_difference_;
+  double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, servoj_gain_, servoj_lookahead_time_,
+      max_joint_difference_;
 
   std::string program_;
 
-  bool execute(const std::array<double, 6> &positions,
-               const std::array<double, 6> &velocities,
-               double sample_number, double time_in_seconds);
+  bool execute(const std::array<double, 6> &positions, const std::array<double, 6> &velocities, double sample_number,
+               double time_in_seconds);
 
 public:
   LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3);

include/ur_modern_driver/ros/mb_publisher.h

@@ -35,7 +35,8 @@ private:
   void publishRobotStatus(const RobotModeData_V3_0__1& data) const;
 
 public:
-  MBPublisher() : io_pub_(nh_.advertise<ur_msgs::IOStates>("ur_driver/io_states", 1))
+  MBPublisher()
+    : io_pub_(nh_.advertise<ur_msgs::IOStates>("ur_driver/io_states", 1))
     , status_pub_(nh_.advertise<industrial_msgs::RobotStatus>("ur_driver/robot_status", 1))
   {
   }

include/ur_modern_driver/ros/trajectory_follower.h

@@ -9,9 +9,9 @@
 #include <thread>
 #include <vector>
 #include "ur_modern_driver/log.h"
+#include "ur_modern_driver/ros/action_trajectory_follower_interface.h"
 #include "ur_modern_driver/ur/commander.h"
 #include "ur_modern_driver/ur/server.h"
-#include "ur_modern_driver/ros/action_trajectory_follower_interface.h"
 
 class TrajectoryFollower : public ActionTrajectoryFollowerInterface
 {

include/ur_modern_driver/ros/urscript_handler.h

@@ -11,7 +11,6 @@
 class URScriptHandler : public Service
 {
 private:
-
   ros::NodeHandle nh_;
   URCommander &commander_;
   ros::Subscriber urscript_sub_;

include/ur_modern_driver/test/random_data.h

@@ -1,11 +1,11 @@
 #pragma once
 #include <inttypes.h>
 #include <algorithm>
+#include <bitset>
 #include <climits>
 #include <cstddef>
 #include <functional>
 #include <random>
-#include <bitset>
 #include "ur_modern_driver/bin_parser.h"
 
 class RandomDataTest

src/ros/action_server.cpp

@@ -1,7 +1,8 @@
 #include "ur_modern_driver/ros/action_server.h"
 #include <cmath>
 
-ActionServer::ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names, double max_velocity)
+ActionServer::ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names,
+                           double max_velocity)
   : as_(nh_, "follow_joint_trajectory", boost::bind(&ActionServer::onGoal, this, _1),
         boost::bind(&ActionServer::onCancel, this, _1), false)
   , joint_names_(joint_names)
@@ -147,7 +148,8 @@ bool ActionServer::validateJoints(GoalHandle& gh, Result& res)
   res.error_code = Result::INVALID_JOINTS;
   res.error_string = "Invalid joint names for goal\n";
   res.error_string += "Expected: ";
-  std::for_each(goal_joints.begin(), goal_joints.end(), [&res](std::string joint){res.error_string += joint + ", ";});
+  std::for_each(goal_joints.begin(), goal_joints.end(),
+                [&res](std::string joint) { res.error_string += joint + ", "; });
   res.error_string += "\nFound: ";
   std::for_each(joint_set_.begin(), joint_set_.end(), [&res](std::string joint) { res.error_string += joint + ", "; });
   return false;
@@ -187,7 +189,8 @@ bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res)
       }
       if (std::fabs(velocity) > max_velocity_)
       {
-        res.error_string = "Received a goal with velocities that are higher than max_velocity_ " + std::to_string(max_velocity_);
+        res.error_string =
+            "Received a goal with velocities that are higher than max_velocity_ " + std::to_string(max_velocity_);
         return false;
       }
     }

src/ros/lowbandwidth_trajectory_follower.cpp

@@ -1,7 +1,7 @@
 #include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h"
 #include <endian.h>
-#include <cmath>
 #include <ros/ros.h>
+#include <cmath>
 
 static const std::array<double, 6> EMPTY_VALUES = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
 
@@ -233,8 +233,8 @@ end
 
 )";
 
-LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port,
-                                       bool version_3)
+LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip,
+                                                               int reverse_port, bool version_3)
   : running_(false)
   , commander_(commander)
   , server_(reverse_port)
@@ -256,7 +256,8 @@ LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &comm
 
   std::string res(POSITION_PROGRAM);
   std::ostringstream out;
-  if (!version_3) {
+  if (!version_3)
+  {
     LOG_ERROR("Low Bandwidth Trajectory Follower only works for interface version > 3");
     std::exit(-1);
   }
@@ -307,8 +308,8 @@ bool LowBandwidthTrajectoryFollower::start()
 }
 
 bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6> &positions,
-                                     const std::array<double, 6> &velocities,
-                                     double sample_number, double time_in_seconds)
+                                             const std::array<double, 6> &velocities, double sample_number,
+                                             double time_in_seconds)
 {
   if (!running_)
     return false;
@@ -362,9 +363,10 @@ bool LowBandwidthTrajectoryFollower::execute(std::vector<TrajectoryPoint> &traje
     LOG_DEBUG("Received request %i", message_num);
     if (message_num < trajectory.size())
     {
-        res = execute(trajectory[message_num].positions, trajectory[message_num].velocities,
-                message_num, trajectory[message_num].time_from_start.count() / 1e6);
-     } else
+      res = execute(trajectory[message_num].positions, trajectory[message_num].velocities, message_num,
+                    trajectory[message_num].time_from_start.count() / 1e6);
+    }
+    else
     {
       res = execute(EMPTY_VALUES, EMPTY_VALUES, message_num, 0.0);
     }

src/ros/mb_publisher.cpp

@@ -54,8 +54,8 @@ void MBPublisher::publishRobotStatus(const RobotModeData_V1_X& data) const
     msg.mode.val = industrial_msgs::RobotMode::AUTO;
 
   // todo: verify that this correct, there is also ROBOT_READY_MODE
-  msg.motion_possible.val = (data.robot_mode == robot_mode_V1_X::ROBOT_RUNNING_MODE)
-                             ? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF;
+  msg.motion_possible.val = (data.robot_mode == robot_mode_V1_X::ROBOT_RUNNING_MODE) ? industrial_msgs::TriState::ON :
+                                                                                       industrial_msgs::TriState::OFF;
 
   publishRobotStatus(msg, data);
 }
@@ -64,8 +64,8 @@ void MBPublisher::publishRobotStatus(const RobotModeData_V3_0__1& data) const
 {
   industrial_msgs::RobotStatus msg;
 
-  msg.motion_possible.val = (data.robot_mode == robot_mode_V3_X::RUNNING)
-                             ? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF;
+  msg.motion_possible.val =
+      (data.robot_mode == robot_mode_V3_X::RUNNING) ? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF;
 
   if (data.control_mode == robot_control_mode_V3_X::TEACH)
     msg.mode.val = industrial_msgs::RobotMode::MANUAL;

src/ros/service_stopper.cpp

@@ -20,7 +20,8 @@ ServiceStopper::ServiceStopper(std::vector<Service*> services)
   {
     if (mode != "Never")
     {
-      LOG_WARN("Found invalid value for param require_activation: '%s'\nShould be one of Never, OnStartup, Always", mode.c_str());
+      LOG_WARN("Found invalid value for param require_activation: '%s'\nShould be one of Never, OnStartup, Always",
+               mode.c_str());
       mode = "Never";
     }
     notify_all(RobotState::Running);

src/ros/trajectory_follower.cpp

@@ -1,7 +1,7 @@
 #include "ur_modern_driver/ros/trajectory_follower.h"
 #include <endian.h>
-#include <cmath>
 #include <ros/ros.h>
+#include <cmath>
 
 static const int32_t MULT_JOINTSTATE_ = 1000000;
 static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}");

src/ros/urscript_handler.cpp

@@ -1,9 +1,7 @@
 #include "ur_modern_driver/ros/urscript_handler.h"
 #include "ur_modern_driver/log.h"
 
-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");
   urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, &URScriptHandler::urscriptInterface, this);

src/ros_main.cpp

@@ -9,11 +9,11 @@
 #include "ur_modern_driver/ros/action_server.h"
 #include "ur_modern_driver/ros/controller.h"
 #include "ur_modern_driver/ros/io_service.h"
+#include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h"
 #include "ur_modern_driver/ros/mb_publisher.h"
 #include "ur_modern_driver/ros/rt_publisher.h"
 #include "ur_modern_driver/ros/service_stopper.h"
 #include "ur_modern_driver/ros/trajectory_follower.h"
-#include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h"
 #include "ur_modern_driver/ros/urscript_handler.h"
 #include "ur_modern_driver/ur/commander.h"
 #include "ur_modern_driver/ur/factory.h"
@@ -62,10 +62,12 @@ public:
 class IgnorePipelineStoppedNotifier : public INotifier
 {
 public:
-    void started(std::string name){
+  void started(std::string name)
+  {
     LOG_INFO("Starting pipeline %s", name.c_str());
   }
-    void stopped(std::string name){
+  void stopped(std::string name)
+  {
     LOG_INFO("Stopping pipeline %s", name.c_str());
   }
 };
@@ -73,17 +75,18 @@ public:
 class ShutdownOnPipelineStoppedNotifier : public INotifier
 {
 public:
-    void started(std::string name){
+  void started(std::string name)
+  {
     LOG_INFO("Starting pipeline %s", name.c_str());
   }
-    void stopped(std::string name){
+  void stopped(std::string name)
+  {
     LOG_INFO("Shutting down on stopped pipeline %s", name.c_str());
     ros::shutdown();
     exit(1);
   }
 };
 
-
 bool parse_args(ProgArgs &args)
 {
   if (!ros::param::get(IP_ADDR_ARG, args.host))
@@ -144,8 +147,8 @@ int main(int argc, char **argv)
   if (args.use_ros_control)
   {
     LOG_INFO("ROS control enabled");
-    TrajectoryFollower *traj_follower = new TrajectoryFollower(
-        *rt_commander, local_ip, args.reverse_port, factory.isVersion3());
+    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);
     services.push_back(controller);
@@ -157,8 +160,8 @@ int main(int argc, char **argv)
     if (args.use_lowbandwidth_trajectory_follower)
     {
       LOG_INFO("Use low bandwidth trajectory follower");
-      traj_follower = new LowBandwidthTrajectoryFollower(*rt_commander,
-           local_ip, args.reverse_port,factory.isVersion3());
+      traj_follower =
+          new LowBandwidthTrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
     }
     else
     {

src/tcp_socket.cpp

@@ -120,7 +120,6 @@ bool TCPSocket::read(char *character)
   return read((uint8_t *)character, 1, read_chars);
 }
 
-
 bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read)
 {
   read = 0;

src/ur/commander.cpp

@@ -73,7 +73,9 @@ bool URCommander_V1_X::speedj(std::array<double, 6> &speeds, double acceleration
 bool URCommander_V1_X::setAnalogOut(uint8_t pin, double value)
 {
   std::ostringstream out;
-  out << "sec io_fun():\n" << "set_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(4) << value << ")\n" << "end\n";
+  out << "sec io_fun():\n"
+      << "set_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(4) << value << ")\n"
+      << "end\n";
   std::string s(out.str());
   return write(s);
 }
@@ -81,7 +83,9 @@ bool URCommander_V1_X::setAnalogOut(uint8_t pin, double value)
 bool URCommander_V1_X::setDigitalOut(uint8_t pin, bool value)
 {
   std::ostringstream out;
-  out << "sec io_fun():\n" << "set_digital_out(" << (int)pin << "," << (value ? "True" : "False") << ")\n" << "end\n";
+  out << "sec io_fun():\n"
+      << "set_digital_out(" << (int)pin << "," << (value ? "True" : "False") << ")\n"
+      << "end\n";
   std::string s(out.str());
   return write(s);
 }
@@ -89,7 +93,9 @@ bool URCommander_V1_X::setDigitalOut(uint8_t pin, bool value)
 bool URCommander_V3_X::setAnalogOut(uint8_t pin, double value)
 {
   std::ostringstream out;
-  out << "sec io_fun():\n" << "set_standard_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(5) << value << ")\n" << "end\n";
+  out << "sec io_fun():\n"
+      << "set_standard_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(5) << value << ")\n"
+      << "end\n";
   std::string s(out.str());
   return write(s);
 }
@@ -116,7 +122,9 @@ bool URCommander_V3_X::setDigitalOut(uint8_t pin, bool value)
   else
     return false;
 
-  out << "sec io_fun():\n" << func << "(" << (int)pin << "," << (value ? "True" : "False") << ")\n" << "end\n";
+  out << "sec io_fun():\n"
+      << func << "(" << (int)pin << "," << (value ? "True" : "False") << ")\n"
+      << "end\n";
   std::string s(out.str());
   return write(s);
 }

src/ur/server.cpp

@@ -31,7 +31,6 @@ std::string URServer::getIP()
   return std::string(buf);
 }
 
-
 bool URServer::open(int socket_fd, struct sockaddr* address, size_t address_len)
 {
   int flag = 1;
@@ -63,7 +62,8 @@ bool URServer::accept()
   int client_fd = -1;
 
   int retry = 0;
-  while((client_fd = ::accept(getSocketFD(), &addr, &addr_len)) == -1){
+  while ((client_fd = ::accept(getSocketFD(), &addr, &addr_len)) == -1)
+  {
     LOG_ERROR("Accepting socket connection failed. (errno: %d)", errno);
     if (retry++ >= 5)
       return false;
@@ -93,12 +93,14 @@ bool URServer::readLine(char* buffer, size_t buf_len)
   char ch;
   size_t total_read;
 
-    if (buf_len <= 0 || buffer == NULL) {
+  if (buf_len <= 0 || buffer == NULL)
+  {
     return false;
   }
 
   total_read = 0;
-    for (;;) {
+  for (;;)
+  {
     if (client_.read(&ch))
     {
       if (total_read < buf_len - 1)  // just in case ...