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,7 +12,6 @@ private:
   Clock::time_point events_[250];
   size_t idx_ = 0;
 
-
   Clock::time_point last_;
 
 public:

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);
+  ROSController(URCommander& commander, TrajectoryFollower& follower, std::vector<std::string>& joint_names,
-  virtual ~ROSController() { }
+                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 start()
-  virtual void stop() {}
+  {
-  virtual void reset() {}
+  }
+  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;
@@ -61,7 +68,8 @@ private:
   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

@@ -51,13 +51,11 @@ private:
     return (resp.success = commander_.setPayload(req.payload));
   }
 
-
 public:
   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

@@ -30,8 +30,7 @@ private:
   void publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data);
 
 public:
-  MBPublisher()
+  MBPublisher() : io_pub_(nh_.advertise<ur_msgs::IOStates>("ur_driver/io_states", 1))
-    : io_pub_(nh_.advertise<ur_msgs::IOStates>("ur_driver/io_states", 1))
   {
   }
 

include/ur_modern_driver/ros/service_stopper.h

@@ -18,7 +18,8 @@ public:
   virtual void onRobotStateChange(RobotState state) = 0;
 };
 
-class ServiceStopper : public URStatePacketConsumer {
+class ServiceStopper : public URStatePacketConsumer
+{
 private:
   ros::NodeHandle nh_;
   ros::ServiceServer enable_service_;
@@ -46,7 +47,16 @@ public:
   }
 
   // unused
-  virtual bool consume(MasterBoardData_V1_X& data) { return true; }
+  virtual bool consume(MasterBoardData_V1_X& data)
-  virtual bool consume(MasterBoardData_V3_0__1& data) { return true; }
+  {
-  virtual bool consume(MasterBoardData_V3_2& data) { return true; }
+    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,9 +23,7 @@ struct TrajectoryPoint
   }
 
   TrajectoryPoint(std::array<double, 6> &pos, std::array<double, 6> &vel, std::chrono::microseconds tfs)
-    : positions(pos)
+    : positions(pos), velocities(vel), time_from_start(tfs)
-    , velocities(vel)
-    , time_from_start(tfs)
   {
   }
 };

include/ur_modern_driver/tcp_socket.h

@@ -2,8 +2,8 @@
 #include <netdb.h>
 #include <sys/socket.h>
 #include <sys/types.h>
-#include <mutex>
 #include <atomic>
+#include <mutex>
 #include <string>
 
 enum class SocketState
@@ -27,16 +27,21 @@ protected:
   }
   virtual void setOptions(int socket_fd);
 
-  
   bool setup(std::string &host, int port);
 
 public:
   TCPSocket();
   virtual ~TCPSocket();
 
-  SocketState getState() { return state_; }
+  SocketState getState()
+  {
+    return state_;
+  }
 
-  int getSocketFD() { return socket_fd_; }
+  int getSocketFD()
+  {
+    return socket_fd_;
+  }
   bool setSocketFD(int socket_fd);
 
   std::string getIP();

include/ur_modern_driver/ur/commander.h

@@ -1,7 +1,7 @@
 #pragma once
 #include <array>
-#include <sstream>
 #include <iomanip>
+#include <sstream>
 #include "ur_modern_driver/ur/stream.h"
 
 class URCommander
@@ -42,7 +42,6 @@ public:
   virtual bool setAnalogOut(uint8_t pin, double value);
 };
 
-
 class URCommander_V3_X : public URCommander
 {
 public:

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

@@ -59,7 +59,6 @@ public:
         timeout_ = next;
     }
 
-
     BinParser bp(buf, read);
     return parser_.parse(bp, products);
   }

include/ur_modern_driver/ur/robot_mode.h

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

include/ur_modern_driver/ur/state.h

@@ -32,8 +32,12 @@ class URStatePacketConsumer;
 class StatePacket
 {
 public:
-  StatePacket() {}
+  StatePacket()
-  virtual ~StatePacket() {}
+  {
+  }
+  virtual ~StatePacket()
+  {
+  }
   virtual bool parseWith(BinParser& bp) = 0;
   virtual bool consumeWith(URStatePacketConsumer& consumer) = 0;
 };

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"
@@ -36,7 +36,10 @@ 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);

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_(
+  : as_(nh_, "follow_joint_trajectory", boost::bind(&ActionServer::onGoal, this, _1),
-      nh_, 
+        boost::bind(&ActionServer::onCancel, this, _1), false)
-      "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)
@@ -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;
@@ -304,7 +296,9 @@ 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;

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");
 
@@ -70,7 +72,6 @@ void ROSController::read(RTShared& packet)
   wrench_interface_.update(packet);
 }
 
-
 bool ROSController::update(RTShared& state)
 {
   auto time = ros::Time::now();

src/ros/hardware_interface.cpp

@@ -26,7 +26,8 @@ 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++)
@@ -56,8 +57,9 @@ void VelocityInterface::reset()
   }
 }
 
-
+PositionInterface::PositionInterface(TrajectoryFollower &follower,
-PositionInterface::  PositionInterface(TrajectoryFollower& follower, hardware_interface::JointStateInterface &js_interface, std::vector<std::string> &joint_names)
+                                     hardware_interface::JointStateInterface &js_interface,
+                                     std::vector<std::string> &joint_names)
   : follower_(follower)
 {
   for (size_t i = 0; i < 6; i++)

src/ros/service_stopper.cpp

@@ -8,7 +8,6 @@ ServiceStopper::ServiceStopper(std::vector<Service*> services)
   // enable_all();
 }
 
-
 bool ServiceStopper::enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp)
 {
   notify_all(RobotState::Running);

src/ros/trajectory_follower.cpp

@@ -1,7 +1,6 @@
-#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}}");
@@ -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,7 +77,6 @@ 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_));
 
@@ -128,7 +127,8 @@ bool TrajectoryFollower::execute(std::array<double, 6> &positions, bool keep_ali
   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;
 
@@ -209,14 +209,8 @@ bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::
       double elapsed_s = duration_cast<double_seconds>(elapsed - prev.time_from_start).count();
       for (size_t j = 0; j < positions.size(); j++)
       {
-        positions[j] = interpolate(
+        positions[j] =
-          elapsed_s, 
+            interpolate(elapsed_s, d_s, prev.positions[j], point.positions[j], prev.velocities[j], point.velocities[j]);
-          d_s, 
-          prev.positions[j], 
-          point.positions[j],
-          prev.velocities[j],
-          point.velocities[j]
-        );
       }
 
       if (!execute(positions, true))

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 = {
+static const std::vector<std::string> DEFAULT_JOINTS = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
-  "shoulder_pan_joint",
+                                                         "wrist_1_joint",      "wrist_2_joint",       "wrist_3_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;

src/tcp_socket.cpp

@@ -1,15 +1,13 @@
+#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()
+TCPSocket::TCPSocket() : socket_fd_(-1), state_(SocketState::Invalid)
-  : socket_fd_(-1)
-  , state_(SocketState::Invalid)
 {
 }
 TCPSocket::~TCPSocket()

src/ur/commander.cpp

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

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)
+URServer::URServer(int port) : port_(port)
-  : port_(port)
 {
 }