diff --git a/include/ur_modern_driver/bin_parser.h b/include/ur_modern_driver/bin_parser.h
index 2cfce45..5bac374 100644
--- a/include/ur_modern_driver/bin_parser.h
+++ b/include/ur_modern_driver/bin_parser.h
@@ -3,11 +3,11 @@
 #include <assert.h>
 #include <endian.h>
 #include <inttypes.h>
+#include <array>
+#include <bitset>
 #include <cstddef>
 #include <cstring>
 #include <string>
-#include <array>
-#include <bitset>
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/types.h"
 
diff --git a/include/ur_modern_driver/event_counter.h b/include/ur_modern_driver/event_counter.h
index 2c8ff31..566ffb2 100644
--- a/include/ur_modern_driver/event_counter.h
+++ b/include/ur_modern_driver/event_counter.h
@@ -1,11 +1,10 @@
 #pragma once
 
-#include <cstdlib>
 #include <chrono>
+#include <cstdlib>
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/ur/consumer.h"
 
-
 class EventCounter : public URRTPacketConsumer
 {
 private:
@@ -13,32 +12,31 @@ private:
   Clock::time_point events_[250];
   size_t idx_ = 0;
 
-
   Clock::time_point last_;
 
 public:
   void trigger()
   {
-    //auto now = Clock::now();
-    //LOG_INFO("Time diff: %d ms", std::chrono::duration_cast<std::chrono::microseconds>(now - last_));
-    //last_ = now;
-    //return;
+    // auto now = Clock::now();
+    // LOG_INFO("Time diff: %d ms", std::chrono::duration_cast<std::chrono::microseconds>(now - last_));
+    // last_ = now;
+    // return;
 
     events_[idx_] = Clock::now();
     idx_ += 1;
 
-    if(idx_ > 250)
+    if (idx_ > 250)
     {
       std::chrono::time_point<std::chrono::high_resolution_clock> t_min =
-          std::chrono::time_point<std::chrono::high_resolution_clock>::max();     
+          std::chrono::time_point<std::chrono::high_resolution_clock>::max();
       std::chrono::time_point<std::chrono::high_resolution_clock> t_max =
           std::chrono::time_point<std::chrono::high_resolution_clock>::min();
-          
-      for(auto const& e : events_)
+
+      for (auto const& e : events_)
       {
-        if(e < t_min)
+        if (e < t_min)
           t_min = e;
-        if(e > t_max)
+        if (e > t_max)
           t_max = e;
       }
 
@@ -46,7 +44,7 @@ public:
       auto secs = std::chrono::duration_cast<std::chrono::seconds>(diff).count();
       auto ms = std::chrono::duration_cast<std::chrono::microseconds>(diff).count();
       std::chrono::duration<double> test(t_max - t_min);
-      LOG_INFO("Recieved 250 messages at %f Hz", (250.0/test.count()));
+      LOG_INFO("Recieved 250 messages at %f Hz", (250.0 / test.count()));
       idx_ = 0;
     }
   }
diff --git a/include/ur_modern_driver/pipeline.h b/include/ur_modern_driver/pipeline.h
index 0e5cfdb..e902ed3 100644
--- a/include/ur_modern_driver/pipeline.h
+++ b/include/ur_modern_driver/pipeline.h
@@ -43,28 +43,28 @@ public:
 
   virtual void setupConsumer()
   {
-    for(auto &con : consumers_)
+    for (auto& con : consumers_)
     {
       con->setupConsumer();
     }
   }
   virtual void teardownConsumer()
   {
-    for(auto &con : consumers_)
+    for (auto& con : consumers_)
     {
       con->teardownConsumer();
     }
   }
   virtual void stopConsumer()
   {
-    for(auto &con : consumers_)
+    for (auto& con : consumers_)
     {
       con->stopConsumer();
     }
   }
   virtual void onTimeout()
   {
-    for(auto &con : consumers_)
+    for (auto& con : consumers_)
     {
       con->onTimeout();
     }
@@ -73,9 +73,9 @@ public:
   bool consume(shared_ptr<T> product)
   {
     bool res = true;
-    for(auto &con : consumers_)
+    for (auto& con : consumers_)
     {
-      if(!con->consume(product))
+      if (!con->consume(product))
         res = false;
     }
     return res;
@@ -153,7 +153,7 @@ private:
         Time now = Clock::now();
         auto pkg_diff = now - last_pkg;
         auto warn_diff = now - last_warn;
-        if(pkg_diff > std::chrono::seconds(1) && warn_diff > std::chrono::seconds(1))
+        if (pkg_diff > std::chrono::seconds(1) && warn_diff > std::chrono::seconds(1))
         {
           last_warn = now;
           consumer_.onTimeout();
diff --git a/include/ur_modern_driver/ros/controller.h b/include/ur_modern_driver/ros/controller.h
index b2780d5..ea4989b 100644
--- a/include/ur_modern_driver/ros/controller.h
+++ b/include/ur_modern_driver/ros/controller.h
@@ -1,18 +1,18 @@
 #pragma once
-#include <atomic>
-#include <ros/ros.h>
 #include <controller_manager/controller_manager.h>
 #include <hardware_interface/force_torque_sensor_interface.h>
 #include <hardware_interface/internal/demangle_symbol.h>
 #include <hardware_interface/joint_command_interface.h>
 #include <hardware_interface/joint_state_interface.h>
 #include <hardware_interface/robot_hw.h>
+#include <ros/ros.h>
+#include <atomic>
 #include "ur_modern_driver/log.h"
+#include "ur_modern_driver/ros/hardware_interface.h"
+#include "ur_modern_driver/ros/service_stopper.h"
 #include "ur_modern_driver/ur/commander.h"
 #include "ur_modern_driver/ur/consumer.h"
 #include "ur_modern_driver/ur/rt_state.h"
-#include "ur_modern_driver/ros/hardware_interface.h"
-#include "ur_modern_driver/ros/service_stopper.h"
 
 class ROSController : private hardware_interface::RobotHW, public URRTPacketConsumer, public Service
 {
@@ -55,10 +55,14 @@ private:
   void reset();
 
 public:
-  ROSController(URCommander& commander, TrajectoryFollower& follower, std::vector<std::string>& joint_names, double max_vel_change);
-  virtual ~ROSController() { }
+  ROSController(URCommander& commander, TrajectoryFollower& follower, std::vector<std::string>& joint_names,
+                double max_vel_change);
+  virtual ~ROSController()
+  {
+  }
   // from RobotHW
-  void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list, const std::list<hardware_interface::ControllerInfo>& stop_list);
+  void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
+                const std::list<hardware_interface::ControllerInfo>& stop_list);
   // from URRTPacketConsumer
   virtual void setupConsumer();
   virtual bool consume(RTState_V1_6__7& state)
diff --git a/include/ur_modern_driver/ros/hardware_interface.h b/include/ur_modern_driver/ros/hardware_interface.h
index 93bd0c7..5dd95c1 100644
--- a/include/ur_modern_driver/ros/hardware_interface.h
+++ b/include/ur_modern_driver/ros/hardware_interface.h
@@ -4,17 +4,23 @@
 #include <hardware_interface/joint_command_interface.h>
 #include <hardware_interface/joint_state_interface.h>
 #include <algorithm>
+#include "ur_modern_driver/ros/trajectory_follower.h"
 #include "ur_modern_driver/ur/commander.h"
 #include "ur_modern_driver/ur/rt_state.h"
-#include "ur_modern_driver/ros/trajectory_follower.h"
 
 class HardwareInterface
 {
 public:
   virtual bool write() = 0;
-  virtual void start() {}
-  virtual void stop() {}
-  virtual void reset() {}
+  virtual void start()
+  {
+  }
+  virtual void stop()
+  {
+  }
+  virtual void reset()
+  {
+  }
 };
 
 using hardware_interface::JointHandle;
@@ -48,7 +54,8 @@ private:
   double max_vel_change_;
 
 public:
-  VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector<std::string> &joint_names, double max_vel_change);
+  VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface,
+                    std::vector<std::string> &joint_names, double max_vel_change);
   virtual bool write();
   virtual void reset();
   typedef hardware_interface::VelocityJointInterface parent_type;
@@ -57,11 +64,12 @@ public:
 class PositionInterface : public HardwareInterface, public hardware_interface::PositionJointInterface
 {
 private:
-  TrajectoryFollower& follower_;
+  TrajectoryFollower &follower_;
   std::array<double, 6> position_cmd_;
 
 public:
-  PositionInterface(TrajectoryFollower& follower, hardware_interface::JointStateInterface &js_interface, std::vector<std::string> &joint_names);
+  PositionInterface(TrajectoryFollower &follower, hardware_interface::JointStateInterface &js_interface,
+                    std::vector<std::string> &joint_names);
   virtual bool write();
   virtual void start();
   virtual void stop();
diff --git a/include/ur_modern_driver/ros/io_service.h b/include/ur_modern_driver/ros/io_service.h
index 03d9935..4f02bb2 100644
--- a/include/ur_modern_driver/ros/io_service.h
+++ b/include/ur_modern_driver/ros/io_service.h
@@ -10,11 +10,11 @@
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/ur/commander.h"
 
-class IOService 
+class IOService
 {
 private:
   ros::NodeHandle nh_;
-  URCommander &commander_;
+  URCommander& commander_;
   ros::ServiceServer io_service_;
   ros::ServiceServer payload_service_;
 
@@ -23,20 +23,20 @@ private:
     LOG_INFO("setIO called with [%d, %d]", req.fun, req.pin);
     bool res = false;
     bool flag = req.state > 0.0 ? true : false;
-    switch(req.fun)
+    switch (req.fun)
     {
       case ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT:
         res = commander_.setDigitalOut(req.pin, flag);
-      break;
+        break;
       case ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT:
         res = commander_.setAnalogOut(req.pin, req.state);
-      break;
+        break;
       case ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE:
         res = commander_.setToolVoltage(static_cast<uint8_t>(req.state));
-      break;
+        break;
       case ur_msgs::SetIO::Request::FUN_SET_FLAG:
         res = commander_.setFlag(req.pin, flag);
-      break;
+        break;
       default:
         LOG_WARN("Invalid setIO function called (%d)", req.fun);
     }
@@ -47,17 +47,15 @@ private:
   bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& resp)
   {
     LOG_INFO("setPayload called");
-    //TODO check min and max payload?
+    // TODO check min and max payload?
     return (resp.success = commander_.setPayload(req.payload));
   }
 
-
 public:
-  IOService(URCommander &commander) 
+  IOService(URCommander& commander)
     : commander_(commander)
     , io_service_(nh_.advertiseService("ur_driver/set_io", &IOService::setIO, this))
     , payload_service_(nh_.advertiseService("ur_driver/set_payload", &IOService::setPayload, this))
   {
-
   }
 };
\ No newline at end of file
diff --git a/include/ur_modern_driver/ros/mb_publisher.h b/include/ur_modern_driver/ros/mb_publisher.h
index 11c4d8b..692ecfd 100644
--- a/include/ur_modern_driver/ros/mb_publisher.h
+++ b/include/ur_modern_driver/ros/mb_publisher.h
@@ -16,22 +16,21 @@ private:
   Publisher io_pub_;
 
   template <size_t N>
-  inline void appendDigital(std::vector<ur_msgs::Digital> &vec, std::bitset<N> bits)
+  inline void appendDigital(std::vector<ur_msgs::Digital>& vec, std::bitset<N> bits)
   {
-    for(size_t i = 0; i < N; i++)
+    for (size_t i = 0; i < N; i++)
     {
-        ur_msgs::Digital digi;
-        digi.pin = static_cast<uint8_t>(i);
-        digi.state = bits.test(i);
-        vec.push_back(digi);
+      ur_msgs::Digital digi;
+      digi.pin = static_cast<uint8_t>(i);
+      digi.state = bits.test(i);
+      vec.push_back(digi);
     }
   }
 
-  void publish(ur_msgs::IOStates &io_msg, SharedMasterBoardData& data);
+  void publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data);
 
 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))
   {
   }
 
diff --git a/include/ur_modern_driver/ros/service_stopper.h b/include/ur_modern_driver/ros/service_stopper.h
index af09dfa..ffbe106 100644
--- a/include/ur_modern_driver/ros/service_stopper.h
+++ b/include/ur_modern_driver/ros/service_stopper.h
@@ -4,7 +4,7 @@
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/ur/consumer.h"
 
-enum class RobotState 
+enum class RobotState
 {
   Running,
   Error,
@@ -18,15 +18,16 @@ public:
   virtual void onRobotStateChange(RobotState state) = 0;
 };
 
-class ServiceStopper : public URStatePacketConsumer {
+class ServiceStopper : public URStatePacketConsumer
+{
 private:
   ros::NodeHandle nh_;
   ros::ServiceServer enable_service_;
   std::vector<Service*> services_;
   RobotState last_state_;
-  
+
   void notify_all(RobotState state);
-  bool handle(SharedRobotModeData &data, bool error);
+  bool handle(SharedRobotModeData& data, bool error);
   bool enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp);
 
 public:
@@ -34,19 +35,28 @@ public:
 
   virtual bool consume(RobotModeData_V1_X& data)
   {
-      return handle(data, data.robot_mode != robot_mode_V1_X::ROBOT_RUNNING_MODE);
+    return handle(data, data.robot_mode != robot_mode_V1_X::ROBOT_RUNNING_MODE);
   }
   virtual bool consume(RobotModeData_V3_0__1& data)
   {
-      return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING);
+    return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING);
   }
   virtual bool consume(RobotModeData_V3_2& data)
   {
-      return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING);
+    return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING);
   }
 
-  //unused
-  virtual bool consume(MasterBoardData_V1_X& data) { return true; }
-  virtual bool consume(MasterBoardData_V3_0__1& data) { return true; }
-  virtual bool consume(MasterBoardData_V3_2& data) { return true; }
+  // unused
+  virtual bool consume(MasterBoardData_V1_X& data)
+  {
+    return true;
+  }
+  virtual bool consume(MasterBoardData_V3_0__1& data)
+  {
+    return true;
+  }
+  virtual bool consume(MasterBoardData_V3_2& data)
+  {
+    return true;
+  }
 };
\ No newline at end of file
diff --git a/include/ur_modern_driver/ros/trajectory_follower.h b/include/ur_modern_driver/ros/trajectory_follower.h
index da01004..d6c4789 100644
--- a/include/ur_modern_driver/ros/trajectory_follower.h
+++ b/include/ur_modern_driver/ros/trajectory_follower.h
@@ -1,13 +1,13 @@
 #pragma once
 
+#include <inttypes.h>
 #include <array>
-#include <vector>
 #include <atomic>
 #include <cstddef>
 #include <cstring>
 #include <string>
 #include <thread>
-#include <inttypes.h>
+#include <vector>
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/ur/commander.h"
 #include "ur_modern_driver/ur/server.h"
@@ -23,14 +23,12 @@ struct TrajectoryPoint
   }
 
   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)
   {
   }
 };
 
-class TrajectoryFollower 
+class TrajectoryFollower
 {
 private:
   std::atomic<bool> running_;
@@ -49,11 +47,11 @@ private:
     return s;
   }
 
-  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); 
+  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);
 
 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 execute(std::array<double, 6> &positions);
diff --git a/include/ur_modern_driver/tcp_socket.h b/include/ur_modern_driver/tcp_socket.h
index 4230ac0..99427b9 100644
--- a/include/ur_modern_driver/tcp_socket.h
+++ b/include/ur_modern_driver/tcp_socket.h
@@ -1,9 +1,9 @@
-#pragma once 
+#pragma once
 #include <netdb.h>
 #include <sys/socket.h>
 #include <sys/types.h>
-#include <mutex>
 #include <atomic>
+#include <mutex>
 #include <string>
 
 enum class SocketState
@@ -26,23 +26,28 @@ protected:
     return false;
   }
   virtual void setOptions(int socket_fd);
-  
-  
+
   bool setup(std::string &host, int port);
 
 public:
   TCPSocket();
   virtual ~TCPSocket();
 
-  SocketState getState() { return state_; }
-  
-  int getSocketFD() { return socket_fd_; }
+  SocketState getState()
+  {
+    return state_;
+  }
+
+  int getSocketFD()
+  {
+    return socket_fd_;
+  }
   bool setSocketFD(int socket_fd);
 
   std::string getIP();
 
-  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 read(uint8_t *buf, size_t buf_len, size_t &read);
+  bool write(const uint8_t *buf, size_t buf_len, size_t &written);
 
-  void close();  
+  void close();
 };
diff --git a/include/ur_modern_driver/ur/commander.h b/include/ur_modern_driver/ur/commander.h
index 5d81698..8fdffac 100644
--- a/include/ur_modern_driver/ur/commander.h
+++ b/include/ur_modern_driver/ur/commander.h
@@ -1,20 +1,20 @@
 #pragma once
 #include <array>
-#include <sstream>
 #include <iomanip>
+#include <sstream>
 #include "ur_modern_driver/ur/stream.h"
 
 class URCommander
 {
 private:
-  URStream& stream_;
+  URStream &stream_;
 
 protected:
-  bool write(std::string& s);
+  bool write(std::string &s);
   void formatArray(std::ostringstream &out, std::array<double, 6> &values);
 
 public:
-  URCommander(URStream& stream) : stream_(stream)
+  URCommander(URStream &stream) : stream_(stream)
   {
   }
 
@@ -22,7 +22,7 @@ public:
   virtual bool setDigitalOut(uint8_t pin, bool value) = 0;
   virtual bool setAnalogOut(uint8_t pin, double value) = 0;
 
-  //shared
+  // shared
   bool uploadProg(std::string &s);
   bool stopj(double a = 10.0);
   bool setToolVoltage(uint8_t voltage);
@@ -33,7 +33,7 @@ public:
 class URCommander_V1_X : public URCommander
 {
 public:
-  URCommander_V1_X(URStream& stream) : URCommander(stream)
+  URCommander_V1_X(URStream &stream) : URCommander(stream)
   {
   }
 
@@ -42,11 +42,10 @@ public:
   virtual bool setAnalogOut(uint8_t pin, double value);
 };
 
-
 class URCommander_V3_X : public URCommander
 {
 public:
-  URCommander_V3_X(URStream& stream) : URCommander(stream)
+  URCommander_V3_X(URStream &stream) : URCommander(stream)
   {
   }
 
diff --git a/include/ur_modern_driver/ur/factory.h b/include/ur_modern_driver/ur/factory.h
index 0b4e6ca..ecf3bed 100644
--- a/include/ur_modern_driver/ur/factory.h
+++ b/include/ur_modern_driver/ur/factory.h
@@ -76,12 +76,12 @@ public:
     return major_version_ == 3;
   }
 
-  std::unique_ptr<URCommander> getCommander(URStream &stream)
+  std::unique_ptr<URCommander> getCommander(URStream& stream)
   {
-    if(major_version_ == 1)
+    if (major_version_ == 1)
       return std::unique_ptr<URCommander>(new URCommander_V1_X(stream));
     else
-      return std::unique_ptr<URCommander>(new URCommander_V3_X(stream));    
+      return std::unique_ptr<URCommander>(new URCommander_V3_X(stream));
   }
 
   std::unique_ptr<URParser<StatePacket>> getStateParser()
diff --git a/include/ur_modern_driver/ur/master_board.h b/include/ur_modern_driver/ur/master_board.h
index c625cbf..e3163dc 100644
--- a/include/ur_modern_driver/ur/master_board.h
+++ b/include/ur_modern_driver/ur/master_board.h
@@ -1,8 +1,8 @@
 #pragma once
 
 #include <inttypes.h>
-#include <cstddef>
 #include <bitset>
+#include <cstddef>
 #include "ur_modern_driver/bin_parser.h"
 #include "ur_modern_driver/types.h"
 #include "ur_modern_driver/ur/state.h"
@@ -42,7 +42,6 @@ public:
   virtual bool parseWith(BinParser& bp);
   virtual bool consumeWith(URStatePacketConsumer& consumer);
 
-  
   std::bitset<10> digital_input_bits;
   std::bitset<10> digital_output_bits;
 
diff --git a/include/ur_modern_driver/ur/producer.h b/include/ur_modern_driver/ur/producer.h
index df4a61a..6f08ab6 100644
--- a/include/ur_modern_driver/ur/producer.h
+++ b/include/ur_modern_driver/ur/producer.h
@@ -35,31 +35,30 @@ public:
     // 4KB should be enough to hold any packet received from UR
     uint8_t buf[4096];
     size_t read = 0;
-    //expoential backoff reconnects
-    while(true)
+    // expoential backoff reconnects
+    while (true)
     {
-      if(stream_.read(buf, sizeof(buf), read))
+      if (stream_.read(buf, sizeof(buf), read))
       {
-        //reset sleep amount
+        // reset sleep amount
         timeout_ = std::chrono::seconds(1);
         break;
       }
 
-      if(stream_.closed())
+      if (stream_.closed())
         return false;
 
       LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count());
       std::this_thread::sleep_for(timeout_);
 
-      if(stream_.connect())
+      if (stream_.connect())
         continue;
 
       auto next = timeout_ * 2;
-      if(next <= std::chrono::seconds(120))
+      if (next <= std::chrono::seconds(120))
         timeout_ = next;
     }
 
-
     BinParser bp(buf, read);
     return parser_.parse(bp, products);
   }
diff --git a/include/ur_modern_driver/ur/robot_mode.h b/include/ur_modern_driver/ur/robot_mode.h
index 6c7c641..80aa145 100644
--- a/include/ur_modern_driver/ur/robot_mode.h
+++ b/include/ur_modern_driver/ur/robot_mode.h
@@ -16,7 +16,7 @@ public:
   bool real_robot_enabled;
   bool robot_power_on;
   bool emergency_stopped;
-  bool protective_stopped; //AKA security_stopped
+  bool protective_stopped;  // AKA security_stopped
   bool program_running;
   bool program_paused;
 
@@ -79,7 +79,6 @@ public:
   virtual bool parseWith(BinParser& bp);
   virtual bool consumeWith(URStatePacketConsumer& consumer);
 
-
   robot_mode_V3_X robot_mode;
   robot_control_mode_V3_X control_mode;
 
diff --git a/include/ur_modern_driver/ur/server.h b/include/ur_modern_driver/ur/server.h
index 027e278..f236b96 100644
--- a/include/ur_modern_driver/ur/server.h
+++ b/include/ur_modern_driver/ur/server.h
@@ -2,9 +2,9 @@
 #include <netdb.h>
 #include <sys/socket.h>
 #include <sys/types.h>
+#include <atomic>
 #include <cstdlib>
 #include <mutex>
-#include <atomic>
 #include <string>
 #include "ur_modern_driver/tcp_socket.h"
 
@@ -21,7 +21,6 @@ protected:
   }
   virtual void setOptions(int socket_fd);
 
-
 public:
   URServer(int port);
   ~URServer();
@@ -29,5 +28,5 @@ public:
   bool bind();
   bool accept();
   void disconnectClient();
-  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);
 };
\ No newline at end of file
diff --git a/include/ur_modern_driver/ur/state.h b/include/ur_modern_driver/ur/state.h
index 4aeddba..d48a876 100644
--- a/include/ur_modern_driver/ur/state.h
+++ b/include/ur_modern_driver/ur/state.h
@@ -32,8 +32,12 @@ class URStatePacketConsumer;
 class StatePacket
 {
 public:
-  StatePacket() {}
-  virtual ~StatePacket() {}
+  StatePacket()
+  {
+  }
+  virtual ~StatePacket()
+  {
+  }
   virtual bool parseWith(BinParser& bp) = 0;
   virtual bool consumeWith(URStatePacketConsumer& consumer) = 0;
 };
diff --git a/include/ur_modern_driver/ur/state_parser.h b/include/ur_modern_driver/ur/state_parser.h
index fc80242..48eda1d 100644
--- a/include/ur_modern_driver/ur/state_parser.h
+++ b/include/ur_modern_driver/ur/state_parser.h
@@ -35,8 +35,8 @@ public:
 
     if (type != message_type::ROBOT_STATE)
     {
-      //quietly ignore the intial version message
-      if(type != message_type::ROBOT_MESSAGE)
+      // quietly ignore the intial version message
+      if (type != message_type::ROBOT_MESSAGE)
       {
         LOG_WARN("Invalid state message type recieved: %u", static_cast<uint8_t>(type));
       }
diff --git a/include/ur_modern_driver/ur/stream.h b/include/ur_modern_driver/ur/stream.h
index 3d5bc81..913f665 100644
--- a/include/ur_modern_driver/ur/stream.h
+++ b/include/ur_modern_driver/ur/stream.h
@@ -2,8 +2,8 @@
 #include <netdb.h>
 #include <sys/socket.h>
 #include <sys/types.h>
-#include <mutex>
 #include <atomic>
+#include <mutex>
 #include <string>
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/tcp_socket.h"
@@ -16,7 +16,7 @@ private:
   std::mutex write_mutex_, read_mutex_;
 
 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 ::connect(socket_fd, address, address_len) == 0;
   }
@@ -36,8 +36,11 @@ public:
     TCPSocket::close();
   }
 
-  bool closed() { return getState() == SocketState::Closed; }
+  bool closed()
+  {
+    return getState() == SocketState::Closed;
+  }
 
-  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 read(uint8_t* buf, size_t buf_len, size_t& read);
+  bool write(const uint8_t* buf, size_t buf_len, size_t& written);
 };
\ No newline at end of file
diff --git a/src/ros/action_server.cpp b/src/ros/action_server.cpp
index 102df31..380acd3 100644
--- a/src/ros/action_server.cpp
+++ b/src/ros/action_server.cpp
@@ -1,14 +1,9 @@
-#include <cmath>
 #include "ur_modern_driver/ros/action_server.h"
+#include <cmath>
 
 ActionServer::ActionServer(TrajectoryFollower& 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
-    )
+  : as_(nh_, "follow_joint_trajectory", boost::bind(&ActionServer::onGoal, this, _1),
+        boost::bind(&ActionServer::onCancel, this, _1), false)
   , joint_names_(joint_names)
   , joint_set_(joint_names.begin(), joint_names.end())
   , max_velocity_(max_velocity)
@@ -22,7 +17,7 @@ ActionServer::ActionServer(TrajectoryFollower& follower, std::vector<std::string
 
 void ActionServer::start()
 {
-  if(running_)
+  if (running_)
     return;
 
   LOG_INFO("Starting ActionServer");
@@ -35,23 +30,23 @@ void ActionServer::onRobotStateChange(RobotState state)
 {
   state_ = state;
 
-  //don't interrupt if everything is fine
-  if(state == RobotState::Running)
+  // don't interrupt if everything is fine
+  if (state == RobotState::Running)
     return;
 
-  //don't retry interrupts
-  if(interrupt_traj_ || !has_goal_)
+  // don't retry interrupts
+  if (interrupt_traj_ || !has_goal_)
     return;
 
-  //on successful lock we're not executing a goal so don't interrupt
-  if(tj_mutex_.try_lock())
+  // on successful lock we're not executing a goal so don't interrupt
+  if (tj_mutex_.try_lock())
   {
     tj_mutex_.unlock();
     return;
   }
 
   interrupt_traj_ = true;
-  //wait for goal to be interrupted and automagically unlock when going out of scope
+  // wait for goal to be interrupted and automagically unlock when going out of scope
   std::lock_guard<std::mutex> lock(tj_mutex_);
 
   Result res;
@@ -60,7 +55,6 @@ void ActionServer::onRobotStateChange(RobotState state)
   curr_gh_.setAborted(res, res.error_string);
 }
 
-
 bool ActionServer::updateState(RTShared& data)
 {
   q_actual_ = data.q_actual;
@@ -68,7 +62,6 @@ bool ActionServer::updateState(RTShared& data)
   return true;
 }
 
-
 bool ActionServer::consume(RTState_V1_6__7& state)
 {
   return updateState(state);
@@ -86,7 +79,6 @@ bool ActionServer::consume(RTState_V3_2__3& state)
   return updateState(state);
 }
 
-
 void ActionServer::onGoal(GoalHandle gh)
 {
   Result res;
@@ -94,7 +86,7 @@ void ActionServer::onGoal(GoalHandle gh)
 
   LOG_INFO("Received new goal");
 
-  if(!validate(gh, res) || !try_execute(gh, res))
+  if (!validate(gh, res) || !try_execute(gh, res))
   {
     LOG_WARN("Goal error: %s", res.error_string.c_str());
     gh.setRejected(res, res.error_string);
@@ -104,7 +96,7 @@ void ActionServer::onGoal(GoalHandle gh)
 void ActionServer::onCancel(GoalHandle gh)
 {
   interrupt_traj_ = true;
-  //wait for goal to be interrupted
+  // wait for goal to be interrupted
   std::lock_guard<std::mutex> lock(tj_mutex_);
 
   Result res;
@@ -114,22 +106,22 @@ void ActionServer::onCancel(GoalHandle gh)
 }
 
 bool ActionServer::validate(GoalHandle& gh, Result& res)
-{ 
+{
   return validateState(gh, res) && validateJoints(gh, res) && validateTrajectory(gh, res);
 }
 
 bool ActionServer::validateState(GoalHandle& gh, Result& res)
 {
-  switch(state_)
+  switch (state_)
   {
     case RobotState::EmergencyStopped:
       res.error_string = "Robot is emergency stopped";
       return false;
-      
+
     case RobotState::ProtectiveStopped:
       res.error_string = "Robot is protective stopped";
       return false;
-      
+
     case RobotState::Error:
       res.error_string = "Robot is not ready, check robot_mode";
       return false;
@@ -149,7 +141,7 @@ bool ActionServer::validateJoints(GoalHandle& gh, Result& res)
   auto const& joints = goal->trajectory.joint_names;
   std::set<std::string> goal_joints(joints.begin(), joints.end());
 
-  if(goal_joints == joint_set_)
+  if (goal_joints == joint_set_)
     return true;
 
   res.error_code = Result::INVALID_JOINTS;
@@ -162,42 +154,42 @@ bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res)
   auto goal = gh.getGoal();
   res.error_code = Result::INVALID_GOAL;
 
-  //must at least have one point
-  if(goal->trajectory.points.size() < 1)
+  // must at least have one point
+  if (goal->trajectory.points.size() < 1)
     return false;
 
-  for(auto const& point : goal->trajectory.points)
+  for (auto const& point : goal->trajectory.points)
   {
-    if(point.velocities.size() != joint_names_.size())
+    if (point.velocities.size() != joint_names_.size())
     {
       res.error_code = Result::INVALID_GOAL;
       res.error_string = "Received a goal with an invalid number of velocities";
       return false;
     }
-    
-    if(point.positions.size() != joint_names_.size())
+
+    if (point.positions.size() != joint_names_.size())
     {
       res.error_code = Result::INVALID_GOAL;
       res.error_string = "Received a goal with an invalid number of positions";
       return false;
     }
-    
-    for(auto const& velocity : point.velocities)
+
+    for (auto const& velocity : point.velocities)
     {
-      if(!std::isfinite(velocity))
+      if (!std::isfinite(velocity))
       {
         res.error_string = "Received a goal with infinities or NaNs in velocity";
         return false;
       }
-      if(std::fabs(velocity) > max_velocity_)
+      if (std::fabs(velocity) > max_velocity_)
       {
         res.error_string = "Received a goal with velocities that are higher than " + std::to_string(max_velocity_);
         return false;
       }
     }
-    for(auto const& position : point.positions)
+    for (auto const& position : point.positions)
     {
-      if(!std::isfinite(position))
+      if (!std::isfinite(position))
       {
         res.error_string = "Received a goal with infinities or NaNs in positions";
         return false;
@@ -205,34 +197,34 @@ bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res)
     }
   }
 
-  //todo validate start position?
+  // todo validate start position?
 
   return true;
 }
 
-inline std::chrono::microseconds convert(const ros::Duration &dur)
+inline std::chrono::microseconds convert(const ros::Duration& dur)
 {
   return std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::seconds(dur.sec) +
-    std::chrono::nanoseconds(dur.nsec));
+                                                               std::chrono::nanoseconds(dur.nsec));
 }
 
 bool ActionServer::try_execute(GoalHandle& gh, Result& res)
 {
-  if(!running_)
+  if (!running_)
   {
     res.error_string = "Internal error";
     return false;
   }
-  if(!tj_mutex_.try_lock())
+  if (!tj_mutex_.try_lock())
   {
     interrupt_traj_ = true;
     res.error_string = "Received another trajectory";
     curr_gh_.setAborted(res, res.error_string);
     tj_mutex_.lock();
-    //todo: make configurable
+    // todo: make configurable
     std::this_thread::sleep_for(std::chrono::milliseconds(250));
   }
-  //locked here
+  // locked here
   curr_gh_ = gh;
   interrupt_traj_ = false;
   has_goal_ = true;
@@ -244,12 +236,12 @@ bool ActionServer::try_execute(GoalHandle& gh, Result& res)
 std::vector<size_t> ActionServer::reorderMap(std::vector<std::string> goal_joints)
 {
   std::vector<size_t> indecies;
-  for(auto const& aj : joint_names_)
+  for (auto const& aj : joint_names_)
   {
     size_t j = 0;
-    for(auto const& gj : goal_joints)
+    for (auto const& gj : goal_joints)
     {
-      if(aj == gj)
+      if (aj == gj)
         break;
       j++;
     }
@@ -261,40 +253,40 @@ std::vector<size_t> ActionServer::reorderMap(std::vector<std::string> goal_joint
 void ActionServer::trajectoryThread()
 {
   LOG_INFO("Trajectory thread started");
-  
-  while(running_)
+
+  while (running_)
   {
     std::unique_lock<std::mutex> lk(tj_mutex_);
-    if(!tj_cv_.wait_for(lk, std::chrono::milliseconds(100), [&]{return running_ && has_goal_;}))
+    if (!tj_cv_.wait_for(lk, std::chrono::milliseconds(100), [&] { return running_ && has_goal_; }))
       continue;
-    
+
     LOG_INFO("Trajectory received and accepted");
     curr_gh_.setAccepted();
 
     auto goal = curr_gh_.getGoal();
     std::vector<TrajectoryPoint> trajectory;
-    trajectory.reserve(goal->trajectory.points.size()+1);
+    trajectory.reserve(goal->trajectory.points.size() + 1);
 
-    //joint names of the goal might have a different ordering compared
-    //to what URScript expects so need to map between the two
+    // joint names of the goal might have a different ordering compared
+    // to what URScript expects so need to map between the two
     auto mapping = reorderMap(goal->trajectory.joint_names);
-    
+
     LOG_INFO("Translating trajectory");
 
     auto const& fp = goal->trajectory.points[0];
     auto fpt = convert(fp.time_from_start);
 
-    //make sure we have a proper t0 position
-    if(fpt > std::chrono::microseconds(0))
+    // make sure we have a proper t0 position
+    if (fpt > std::chrono::microseconds(0))
     {
       LOG_INFO("Trajectory without t0 recieved, inserting t0 at currrent position");
       trajectory.push_back(TrajectoryPoint(q_actual_, qd_actual_, std::chrono::microseconds(0)));
     }
 
-    for(auto const& point : goal->trajectory.points)
+    for (auto const& point : goal->trajectory.points)
     {
       std::array<double, 6> pos, vel;
-      for(size_t i = 0; i < 6; i++)
+      for (size_t i = 0; i < 6; i++)
       {
         size_t idx = mapping[i];
         pos[idx] = point.positions[i];
@@ -304,17 +296,19 @@ void ActionServer::trajectoryThread()
       trajectory.push_back(TrajectoryPoint(pos, vel, t));
     }
 
-    double t = std::chrono::duration_cast<std::chrono::duration<double>>(trajectory[trajectory.size()-1].time_from_start).count();
+    double t =
+        std::chrono::duration_cast<std::chrono::duration<double>>(trajectory[trajectory.size() - 1].time_from_start)
+            .count();
     LOG_INFO("Executing trajectory with %zu points and duration of %4.3fs", trajectory.size(), t);
 
     Result res;
 
-    if(follower_.start())
+    if (follower_.start())
     {
-      if(follower_.execute(trajectory, interrupt_traj_))
+      if (follower_.execute(trajectory, interrupt_traj_))
       {
-        //interrupted goals must be handled by interrupt trigger
-        if(!interrupt_traj_)
+        // interrupted goals must be handled by interrupt trigger
+        if (!interrupt_traj_)
         {
           LOG_INFO("Trajectory executed successfully");
           res.error_code = Result::SUCCESSFUL;
@@ -327,7 +321,7 @@ void ActionServer::trajectoryThread()
       {
         LOG_INFO("Trajectory failed");
         res.error_code = -100;
-        res.error_string = "Connection to robot was lost";      
+        res.error_string = "Connection to robot was lost";
         curr_gh_.setAborted(res, res.error_string);
       }
 
@@ -337,7 +331,7 @@ void ActionServer::trajectoryThread()
     {
       LOG_ERROR("Failed to start trajectory follower!");
       res.error_code = -100;
-      res.error_string = "Robot connection could not be established";      
+      res.error_string = "Robot connection could not be established";
       curr_gh_.setAborted(res, res.error_string);
     }
 
diff --git a/src/ros/controller.cpp b/src/ros/controller.cpp
index f8896d7..75aa9c3 100644
--- a/src/ros/controller.cpp
+++ b/src/ros/controller.cpp
@@ -1,6 +1,7 @@
 #include "ur_modern_driver/ros/controller.h"
 
-ROSController::ROSController(URCommander &commander, TrajectoryFollower& follower, std::vector<std::string> &joint_names, double max_vel_change)
+ROSController::ROSController(URCommander& commander, TrajectoryFollower& follower,
+                             std::vector<std::string>& joint_names, double max_vel_change)
   : controller_(this, nh_)
   , joint_interface_(joint_names)
   , wrench_interface_()
@@ -18,7 +19,8 @@ void ROSController::setupConsumer()
   lastUpdate_ = ros::Time::now();
 }
 
-void ROSController::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list, const std::list<hardware_interface::ControllerInfo>& stop_list)
+void ROSController::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
+                             const std::list<hardware_interface::ControllerInfo>& stop_list)
 {
   LOG_INFO("Switching hardware interface");
 
@@ -60,7 +62,7 @@ void ROSController::reset()
 {
   if (active_interface_ == nullptr)
     return;
-  
+
   active_interface_->reset();
 }
 
@@ -70,7 +72,6 @@ void ROSController::read(RTShared& packet)
   wrench_interface_.update(packet);
 }
 
-
 bool ROSController::update(RTShared& state)
 {
   auto time = ros::Time::now();
@@ -80,16 +81,16 @@ bool ROSController::update(RTShared& state)
   read(state);
   controller_.update(time, diff, !service_enabled_);
 
-  //emergency stop and such should not kill the pipeline
-  //but still prevent writes
-  if(!service_enabled_)
+  // emergency stop and such should not kill the pipeline
+  // but still prevent writes
+  if (!service_enabled_)
   {
     reset();
     return true;
   }
-  
-  //allow the controller to update x times before allowing writes again
-  if(service_cooldown_ > 0)
+
+  // allow the controller to update x times before allowing writes again
+  if (service_cooldown_ > 0)
   {
     service_cooldown_ -= 1;
     return true;
@@ -101,9 +102,9 @@ bool ROSController::update(RTShared& state)
 void ROSController::onRobotStateChange(RobotState state)
 {
   bool next = (state == RobotState::Running);
-  if(next == service_enabled_)
+  if (next == service_enabled_)
     return;
-  
+
   service_enabled_ = next;
-  service_cooldown_ = 125; 
+  service_cooldown_ = 125;
 }
\ No newline at end of file
diff --git a/src/ros/hardware_interface.cpp b/src/ros/hardware_interface.cpp
index 793769b..fde4e85 100644
--- a/src/ros/hardware_interface.cpp
+++ b/src/ros/hardware_interface.cpp
@@ -19,14 +19,15 @@ void JointInterface::update(RTShared &packet)
 WrenchInterface::WrenchInterface()
 {
   registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin() + 3));
-} 
+}
 
 void WrenchInterface::update(RTShared &packet)
 {
   tcp_ = packet.tcp_force;
 }
 
-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)
 {
   for (size_t i = 0; i < 6; i++)
@@ -50,14 +51,15 @@ bool VelocityInterface::write()
 
 void VelocityInterface::reset()
 {
-  for(auto &val : prev_velocity_cmd_)
+  for (auto &val : prev_velocity_cmd_)
   {
     val = 0;
   }
 }
 
-
-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++)
diff --git a/src/ros/mb_publisher.cpp b/src/ros/mb_publisher.cpp
index 1177be5..c0fdc23 100644
--- a/src/ros/mb_publisher.cpp
+++ b/src/ros/mb_publisher.cpp
@@ -1,6 +1,6 @@
 #include "ur_modern_driver/ros/mb_publisher.h"
 
-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;
   ana.pin = pin;
@@ -8,7 +8,7 @@ inline void appendAnalog(std::vector<ur_msgs::Analog> &vec, double val, uint8_t
   vec.push_back(ana);
 }
 
-void MBPublisher::publish(ur_msgs::IOStates &io_msg, SharedMasterBoardData& data)
+void MBPublisher::publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data)
 {
   appendAnalog(io_msg.analog_in_states, data.analog_input0, 0);
   appendAnalog(io_msg.analog_in_states, data.analog_input1, 1);
diff --git a/src/ros/service_stopper.cpp b/src/ros/service_stopper.cpp
index 53cb43c..2f46fd5 100644
--- a/src/ros/service_stopper.cpp
+++ b/src/ros/service_stopper.cpp
@@ -5,10 +5,9 @@ ServiceStopper::ServiceStopper(std::vector<Service*> services)
   , services_(services)
   , last_state_(RobotState::Error)
 {
-  //enable_all();
+  // enable_all();
 }
 
-
 bool ServiceStopper::enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp)
 {
   notify_all(RobotState::Running);
@@ -17,10 +16,10 @@ bool ServiceStopper::enableCallback(std_srvs::EmptyRequest& req, std_srvs::Empty
 
 void ServiceStopper::notify_all(RobotState state)
 {
-  if(last_state_ == state)
+  if (last_state_ == state)
     return;
-  
-  for(auto const service : services_)
+
+  for (auto const service : services_)
   {
     service->onRobotStateChange(state);
   }
@@ -30,15 +29,15 @@ void ServiceStopper::notify_all(RobotState state)
 
 bool ServiceStopper::handle(SharedRobotModeData& data, bool error)
 {
-  if(data.emergency_stopped)
+  if (data.emergency_stopped)
   {
     notify_all(RobotState::EmergencyStopped);
   }
-  else if(data.protective_stopped)
+  else if (data.protective_stopped)
   {
     notify_all(RobotState::ProtectiveStopped);
   }
-  else if(error)
+  else if (error)
   {
     notify_all(RobotState::Error);
   }
diff --git a/src/ros/trajectory_follower.cpp b/src/ros/trajectory_follower.cpp
index 52b79f2..a1f45d9 100644
--- a/src/ros/trajectory_follower.cpp
+++ b/src/ros/trajectory_follower.cpp
@@ -1,8 +1,7 @@
-#include <cmath>
-#include <endian.h>
 #include "ur_modern_driver/ros/trajectory_follower.h"
-  
-  
+#include <endian.h>
+#include <cmath>
+
 static const int32_t MULT_JOINTSTATE_ = 1000000;
 static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}");
 static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}");
@@ -65,7 +64,8 @@ def driverProg():
 end
 )";
 
-TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reverse_ip, int reverse_port, bool version_3)
+TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port,
+                                       bool version_3)
   : running_(false)
   , commander_(commander)
   , server_(reverse_port)
@@ -77,13 +77,12 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reve
   ros::param::get("~servoj_lookahead_time", servoj_lookahead_time_);
   ros::param::get("~servoj_gain", servoj_gain_);
 
-
   std::string res(POSITION_PROGRAM);
   res.replace(res.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_));
 
   std::ostringstream out;
   out << "t=" << std::fixed << std::setprecision(4) << servoj_time_;
-  if(version_3)
+  if (version_3)
     out << ", lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_;
 
   res.replace(res.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
@@ -91,21 +90,21 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reve
   res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port));
   program_ = res;
 
-  if(!server_.bind())
+  if (!server_.bind())
   {
     LOG_ERROR("Failed to bind server, the port %d is likely already in use", reverse_port);
     std::exit(-1);
-  }  
+  }
 }
 
 bool TrajectoryFollower::start()
 {
-  if(running_)
-    return true; //not sure
+  if (running_)
+    return true;  // not sure
 
   LOG_INFO("Uploading trajectory program to robot");
-  
-  if(!commander_.uploadProg(program_))
+
+  if (!commander_.uploadProg(program_))
   {
     LOG_ERROR("Program upload failed!");
     return false;
@@ -113,29 +112,30 @@ bool TrajectoryFollower::start()
 
   LOG_DEBUG("Awaiting incomming robot connection");
 
-  if(!server_.accept())
+  if (!server_.accept())
   {
     LOG_ERROR("Failed to accept incomming robot connection");
     return false;
   }
-  
+
   LOG_DEBUG("Robot successfully connected");
   return (running_ = true);
 }
 
 bool TrajectoryFollower::execute(std::array<double, 6> &positions, bool keep_alive)
 {
-  if(!running_)
+  if (!running_)
     return false;
 
-  //  LOG_INFO("servoj([%f,%f,%f,%f,%f,%f])", positions[0], positions[1], positions[2], positions[3], positions[4], positions[5]);
+  //  LOG_INFO("servoj([%f,%f,%f,%f,%f,%f])", positions[0], positions[1], positions[2], positions[3], positions[4],
+  //  positions[5]);
 
   last_positions_ = positions;
 
-  uint8_t buf[sizeof(uint32_t)*7];
+  uint8_t buf[sizeof(uint32_t) * 7];
   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);
@@ -145,7 +145,7 @@ bool TrajectoryFollower::execute(std::array<double, 6> &positions, bool keep_ali
   int32_t val = htobe32(static_cast<int32_t>(keep_alive));
   append(idx, val);
 
-  size_t written; 
+  size_t written;
   return server_.write(buf, sizeof(buf), written);
 }
 
@@ -166,60 +166,54 @@ bool TrajectoryFollower::execute(std::array<double, 6> &positions)
 
 bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt)
 {
-  if(!running_)
+  if (!running_)
     return false;
-  
+
   using namespace std::chrono;
   typedef duration<double> double_seconds;
   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)
+    // skip t0
+    if (&point == &prev)
       continue;
 
-    if(interrupt)
+    if (interrupt)
       break;
 
     auto duration = point.time_from_start - prev.time_from_start;
     double d_s = duration_cast<double_seconds>(duration).count();
 
-    //interpolation loop
-    while(!interrupt)
+    // interpolation loop
+    while (!interrupt)
     {
       latest = Clock::now();
       auto elapsed = latest - t0;
 
-      if(point.time_from_start <= elapsed)
+      if (point.time_from_start <= elapsed)
         break;
 
-      if(last.time_from_start <= elapsed)
+      if (last.time_from_start <= elapsed)
         return true;
 
       double elapsed_s = duration_cast<double_seconds>(elapsed - prev.time_from_start).count();
-      for(size_t j = 0; j < positions.size(); j++)
+      for (size_t j = 0; j < positions.size(); j++)
       {
-        positions[j] = interpolate(
-          elapsed_s, 
-          d_s, 
-          prev.positions[j], 
-          point.positions[j],
-          prev.velocities[j],
-          point.velocities[j]
-        );
+        positions[j] =
+            interpolate(elapsed_s, d_s, prev.positions[j], point.positions[j], prev.velocities[j], point.velocities[j]);
       }
 
-      if(!execute(positions, true))
+      if (!execute(positions, true))
         return false;
 
       std::this_thread::sleep_for(std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.)));
@@ -228,20 +222,20 @@ bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::
     prev = point;
   }
 
-  //In theory it's possible the last position won't be sent by
-  //the interpolation loop above but rather some position between
-  //t[N-1] and t[N] where N is the number of trajectory points.
-  //To make sure this does not happen the last position is sent 
+  // In theory it's possible the last position won't be sent by
+  // the interpolation loop above but rather some position between
+  // t[N-1] and t[N] where N is the number of trajectory points.
+  // To make sure this does not happen the last position is sent
   return execute(last.positions, true);
 }
 
 void TrajectoryFollower::stop()
 {
-  if(!running_)
+  if (!running_)
     return;
 
-  //std::array<double, 6> empty;
-  //execute(empty, false);
+  // std::array<double, 6> empty;
+  // execute(empty, false);
 
   server_.disconnectClient();
   running_ = false;
diff --git a/src/ros_main.cpp b/src/ros_main.cpp
index f069ac0..4f26634 100644
--- a/src/ros_main.cpp
+++ b/src/ros_main.cpp
@@ -11,8 +11,8 @@
 #include "ur_modern_driver/ros/io_service.h"
 #include "ur_modern_driver/ros/mb_publisher.h"
 #include "ur_modern_driver/ros/rt_publisher.h"
-#include "ur_modern_driver/ros/trajectory_follower.h"
 #include "ur_modern_driver/ros/service_stopper.h"
+#include "ur_modern_driver/ros/trajectory_follower.h"
 #include "ur_modern_driver/ur/commander.h"
 #include "ur_modern_driver/ur/factory.h"
 #include "ur_modern_driver/ur/messages.h"
@@ -30,14 +30,8 @@ static const std::string BASE_FRAME_ARG("~base_frame");
 static const std::string TOOL_FRAME_ARG("~tool_frame");
 static const std::string JOINT_NAMES_PARAM("hardware_interface/joints");
 
-static const std::vector<std::string> DEFAULT_JOINTS = {
-  "shoulder_pan_joint",
-  "shoulder_lift_joint",
-  "elbow_joint",
-  "wrist_1_joint",
-  "wrist_2_joint",
-  "wrist_3_joint"
-};
+static const std::vector<std::string> DEFAULT_JOINTS = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
+                                                         "wrist_1_joint",      "wrist_2_joint",       "wrist_3_joint" };
 
 static const int UR_SECONDARY_PORT = 30002;
 static const int UR_RT_PORT = 30003;
@@ -65,7 +59,7 @@ bool parse_args(ProgArgs &args)
     return false;
   }
   ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001));
-  ros::param::param(MAX_VEL_CHANGE_ARG, args.max_vel_change, 15.0); // rad/s
+  ros::param::param(MAX_VEL_CHANGE_ARG, args.max_vel_change, 15.0);  // rad/s
   ros::param::param(ROS_CONTROL_ARG, args.use_ros_control, false);
   ros::param::param(PREFIX_ARG, args.prefix, std::string());
   ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link");
@@ -74,7 +68,7 @@ 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();
@@ -91,9 +85,9 @@ 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();
@@ -101,7 +95,7 @@ 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 };
 
   TrajectoryFollower traj_follower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
 
@@ -130,10 +124,10 @@ int main(int argc, char **argv)
   URStream state_stream(args.host, UR_SECONDARY_PORT);
   URProducer<StatePacket> state_prod(state_stream, *state_parser);
   MBPublisher state_pub;
-  
+
   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);
 
@@ -145,7 +139,7 @@ int main(int argc, char **argv)
   auto state_commander = factory.getCommander(state_stream);
   IOService io_service(*state_commander);
 
-  if(action_server)
+  if (action_server)
     action_server->start();
 
   ros::spin();
@@ -154,8 +148,8 @@ int main(int argc, char **argv)
 
   rt_pl.stop();
   state_pl.stop();
-  
-  if(controller)
+
+  if (controller)
     delete controller;
 
   LOG_INFO("Pipelines shutdown complete");
diff --git a/src/tcp_socket.cpp b/src/tcp_socket.cpp
index c4f1839..0bd390a 100644
--- a/src/tcp_socket.cpp
+++ b/src/tcp_socket.cpp
@@ -1,16 +1,14 @@
+#include <arpa/inet.h>
 #include <endian.h>
 #include <netinet/tcp.h>
-#include <arpa/inet.h>
 #include <unistd.h>
 #include <cstring>
 
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/tcp_socket.h"
 
-TCPSocket::TCPSocket()
-  : socket_fd_(-1)
-  , state_(SocketState::Invalid)
-{ 
+TCPSocket::TCPSocket() : socket_fd_(-1), state_(SocketState::Invalid)
+{
 }
 TCPSocket::~TCPSocket()
 {
@@ -26,7 +24,7 @@ void TCPSocket::setOptions(int socket_fd)
 
 bool TCPSocket::setup(std::string &host, int port)
 {
-  if(state_ == SocketState::Connected)
+  if (state_ == SocketState::Connected)
     return false;
 
   LOG_INFO("Setting up connection: %s:%d", host.c_str(), port);
@@ -51,11 +49,11 @@ 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);
 
-    if (socket_fd_ != -1 && open(socket_fd_, p->ai_addr, p->ai_addrlen)) 
+    if (socket_fd_ != -1 && open(socket_fd_, p->ai_addr, p->ai_addrlen))
     {
       connected = true;
       break;
@@ -63,8 +61,8 @@ bool TCPSocket::setup(std::string &host, int port)
   }
 
   freeaddrinfo(result);
-  
-  if(!connected)
+
+  if (!connected)
   {
     state_ = SocketState::Invalid;
     LOG_ERROR("Connection setup failed for %s:%d", host.c_str(), port);
@@ -80,7 +78,7 @@ bool TCPSocket::setup(std::string &host, int port)
 
 bool TCPSocket::setSocketFD(int socket_fd)
 {
-  if(state_ == SocketState::Connected)
+  if (state_ == SocketState::Connected)
     return false;
   socket_fd_ = socket_fd;
   state_ = SocketState::Connected;
@@ -89,7 +87,7 @@ bool TCPSocket::setSocketFD(int socket_fd)
 
 void TCPSocket::close()
 {
-  if(state_ != SocketState::Connected)
+  if (state_ != SocketState::Connected)
     return;
   state_ = SocketState::Closed;
   ::shutdown(socket_fd_, SHUT_RDWR);
@@ -100,9 +98,9 @@ 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)
+  if (res < 0)
   {
     LOG_ERROR("Could not get local IP");
     return std::string();
@@ -117,17 +115,17 @@ bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read)
 {
   read = 0;
 
-  if(state_ != SocketState::Connected)
+  if (state_ != SocketState::Connected)
     return false;
-    
+
   ssize_t res = ::recv(socket_fd_, buf, buf_len, 0);
 
-  if(res == 0)
+  if (res == 0)
   {
     state_ = SocketState::Disconnected;
     return false;
   }
-  else if(res < 0)
+  else if (res < 0)
     return false;
 
   read = static_cast<size_t>(res);
@@ -137,8 +135,8 @@ bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read)
 bool TCPSocket::write(const uint8_t *buf, size_t buf_len, size_t &written)
 {
   written = 0;
-  
-  if(state_ != SocketState::Connected)
+
+  if (state_ != SocketState::Connected)
     return false;
 
   size_t remaining = buf_len;
@@ -150,10 +148,10 @@ bool TCPSocket::write(const uint8_t *buf, size_t buf_len, size_t &written)
 
     if (sent <= 0)
       return false;
-    
+
     written += sent;
     remaining -= sent;
   }
-  
+
   return true;
 }
\ No newline at end of file
diff --git a/src/ur/commander.cpp b/src/ur/commander.cpp
index 0fd358e..96fde7d 100644
--- a/src/ur/commander.cpp
+++ b/src/ur/commander.cpp
@@ -1,10 +1,10 @@
 #include "ur_modern_driver/ur/commander.h"
 #include "ur_modern_driver/log.h"
 
-bool URCommander::write(std::string& s)
+bool URCommander::write(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);
 }
@@ -12,10 +12,10 @@ bool URCommander::write(std::string& s)
 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 = ",";
+    out << mod << val;
+    mod = ",";
   }
   out << "]";
 }
@@ -27,7 +27,7 @@ bool URCommander::uploadProg(std::string &s)
 
 bool URCommander::setToolVoltage(uint8_t voltage)
 {
-  if(voltage != 0 || voltage != 12 || voltage != 24)
+  if (voltage != 0 || voltage != 12 || voltage != 24)
     return false;
 
   std::ostringstream out;
@@ -59,7 +59,6 @@ bool URCommander::stopj(double a)
   return write(s);
 }
 
-
 bool URCommander_V1_X::speedj(std::array<double, 6> &speeds, double acceleration)
 {
   std::ostringstream out;
@@ -86,7 +85,6 @@ bool URCommander_V1_X::setDigitalOut(uint8_t pin, bool value)
   return write(s);
 }
 
-
 bool URCommander_V3_X::speedj(std::array<double, 6> &speeds, double acceleration)
 {
   std::ostringstream out;
@@ -110,16 +108,16 @@ bool URCommander_V3_X::setDigitalOut(uint8_t pin, bool value)
   std::ostringstream out;
   std::string func;
 
-  if(pin < 8)
+  if (pin < 8)
   {
     func = "set_standard_digital_out";
   }
-  else if(pin < 16)
+  else if (pin < 16)
   {
     func = "set_configurable_digital_out";
     pin -= 8;
   }
-  else if(pin < 18)
+  else if (pin < 18)
   {
     func = "set_tool_digital_out";
     pin -= 16;
@@ -127,7 +125,6 @@ bool URCommander_V3_X::setDigitalOut(uint8_t pin, bool value)
   else
     return false;
 
-
   out << func << "(" << (int)pin << "," << (value ? "True" : "False") << ")\n";
   std::string s(out.str());
   return write(s);
diff --git a/src/ur/master_board.cpp b/src/ur/master_board.cpp
index 991b6ac..c6aa924 100644
--- a/src/ur/master_board.cpp
+++ b/src/ur/master_board.cpp
@@ -59,7 +59,7 @@ bool MasterBoardData_V3_0__1::parseWith(BinParser& bp)
   bp.parse(euromap67_interface_installed);
 
   if (euromap67_interface_installed)
-  {  
+  {
     if (!bp.checkSize(MasterBoardData_V3_0__1::EURO_SIZE))
       return false;
 
diff --git a/src/ur/server.cpp b/src/ur/server.cpp
index 5530be9..be19ae0 100644
--- a/src/ur/server.cpp
+++ b/src/ur/server.cpp
@@ -1,12 +1,11 @@
-#include <cstring>
-#include <netinet/tcp.h>
-#include <arpa/inet.h>
-#include <unistd.h>
-#include "ur_modern_driver/log.h"
 #include "ur_modern_driver/ur/server.h"
+#include <arpa/inet.h>
+#include <netinet/tcp.h>
+#include <unistd.h>
+#include <cstring>
+#include "ur_modern_driver/log.h"
 
-URServer::URServer(int port)
-  : port_(port)
+URServer::URServer(int port) : port_(port)
 {
 }
 
@@ -29,7 +28,7 @@ std::string URServer::getIP()
   socklen_t len = sizeof(name);
   int res = ::getsockname(getSocketFD(), (sockaddr*)&name, &len);
 
-  if(res < 0)
+  if (res < 0)
   {
     LOG_ERROR("Could not get local IP");
     return std::string();
@@ -44,11 +43,11 @@ bool URServer::bind()
 {
   std::string empty;
   bool res = TCPSocket::setup(empty, port_);
-  
-  if(!res)
+
+  if (!res)
     return false;
 
-  if(::listen(getSocketFD(), 1) < 0)
+  if (::listen(getSocketFD(), 1) < 0)
     return false;
 
   return true;
@@ -56,14 +55,14 @@ bool URServer::bind()
 
 bool URServer::accept()
 {
-  if(TCPSocket::getState() != SocketState::Connected || client_.getSocketFD() > 0)
+  if (TCPSocket::getState() != SocketState::Connected || client_.getSocketFD() > 0)
     return false;
 
-  struct sockaddr addr; 
+  struct sockaddr addr;
   socklen_t addr_len;
   int client_fd = ::accept(getSocketFD(), &addr, &addr_len);
 
-  if(client_fd <= 0)
+  if (client_fd <= 0)
     return false;
 
   setOptions(client_fd);
@@ -73,13 +72,13 @@ bool URServer::accept()
 
 void URServer::disconnectClient()
 {
-  if(client_.getState() != SocketState::Connected)
+  if (client_.getState() != SocketState::Connected)
     return;
-  
+
   client_.close();
 }
 
-bool URServer::write(const uint8_t* buf, size_t buf_len, size_t &written)
+bool URServer::write(const uint8_t* buf, size_t buf_len, size_t& written)
 {
   return client_.write(buf, buf_len, written);
 }
\ No newline at end of file
diff --git a/src/ur/stream.cpp b/src/ur/stream.cpp
index cdb20f2..b0609da 100644
--- a/src/ur/stream.cpp
+++ b/src/ur/stream.cpp
@@ -6,22 +6,22 @@
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/ur/stream.h"
 
-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_);
   return TCPSocket::write(buf, buf_len, written);
 }
 
-bool URStream::read(uint8_t* buf, size_t buf_len, size_t &total)
+bool URStream::read(uint8_t* buf, size_t buf_len, size_t& total)
 {
-  std::lock_guard<std::mutex> lock(read_mutex_);   
+  std::lock_guard<std::mutex> lock(read_mutex_);
 
   bool initial = true;
   uint8_t* buf_pos = buf;
   size_t remainder = sizeof(int32_t);
   size_t read = 0;
 
-  while(remainder > 0 && TCPSocket::read(buf_pos, remainder, read))
+  while (remainder > 0 && TCPSocket::read(buf_pos, remainder, read))
   {
     TCPSocket::setOptions(getSocketFD());
     if (initial)
@@ -39,6 +39,6 @@ bool URStream::read(uint8_t* buf, size_t buf_len, size_t &total)
     buf_pos += read;
     remainder -= read;
   }
-  
+
   return remainder == 0;
 }
\ No newline at end of file