Clang-format run

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"
 

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;
     }
   }

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();

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)

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();

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))
   {
-
   }
 };

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))
   {
   }
 

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;
+  }
 };

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);

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();
 };

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)
   {
   }
 

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()

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;
 

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);
   }

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;
 

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);
 };

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;
 };

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));
       }

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);
 };