Format sources with clang-format

src/ros/action_server.cpp

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

src/ros/controller.cpp

@@ -58,7 +58,7 @@ void ROSController::doSwitch(const std::list<hardware_interface::ControllerInfo>
     return;
   }
 
-  if(start_list.size() > 0)
+  if (start_list.size() > 0)
     LOG_WARN("Failed to start interface!");
 }
 

src/ros/lowbandwidth_trajectory_follower.cpp

@@ -1,9 +1,9 @@
 #include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h"
 #include <endian.h>
-#include <cmath>
 #include <ros/ros.h>
+#include <cmath>
 
-static const std::array<double, 6> EMPTY_VALUES = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+static const std::array<double, 6> EMPTY_VALUES = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
 
 static const std::string TIME_INTERVAL("{{TIME_INTERVAL}}");
 static const std::string SERVOJ_TIME("{{SERVOJ_TIME}}");
@@ -233,8 +233,8 @@ end
 
 )";
 
-LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port,
-                                       bool version_3)
+LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip,
+                                                               int reverse_port, bool version_3)
   : running_(false)
   , commander_(commander)
   , server_(reverse_port)
@@ -256,7 +256,8 @@ LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &comm
 
   std::string res(POSITION_PROGRAM);
   std::ostringstream out;
-  if (!version_3) {
+  if (!version_3)
+  {
     LOG_ERROR("Low Bandwidth Trajectory Follower only works for interface version > 3");
     std::exit(-1);
   }
@@ -307,8 +308,8 @@ bool LowBandwidthTrajectoryFollower::start()
 }
 
 bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6> &positions,
-                                     const std::array<double, 6> &velocities,
-                                     double sample_number, double time_in_seconds)
+                                             const std::array<double, 6> &velocities, double sample_number,
+                                             double time_in_seconds)
 {
   if (!running_)
     return false;
@@ -317,11 +318,11 @@ bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6> &positi
 
   out << "(";
   out << sample_number << ",";
-  for (auto const &pos: positions)
+  for (auto const &pos : positions)
   {
     out << pos << ",";
   }
-  for (auto const &vel: velocities)
+  for (auto const &vel : velocities)
   {
     out << vel << ",";
   }
@@ -331,7 +332,7 @@ bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6> &positi
   // We have only ASCII characters and we can cast char -> uint_8
   const std::string tmp = out.str();
   const char *formatted_message = tmp.c_str();
-  const uint8_t *buf = (uint8_t *) formatted_message;
+  const uint8_t *buf = (uint8_t *)formatted_message;
 
   size_t written;
   LOG_DEBUG("Sending message %s", formatted_message);
@@ -346,32 +347,33 @@ bool LowBandwidthTrajectoryFollower::execute(std::vector<TrajectoryPoint> &traje
 
   bool finished = false;
 
-  char* line[MAX_SERVER_BUF_LEN];
+  char *line[MAX_SERVER_BUF_LEN];
 
   bool res = true;
 
   while (!finished && !interrupt)
   {
-     if (!server_.readLine((char *)line, MAX_SERVER_BUF_LEN))
-     {
-        LOG_DEBUG("Connection closed. Finishing!");
-        finished = true;
-        break;
-     }
-     unsigned int message_num=atoi((const char *) line);
-     LOG_DEBUG("Received request %i", message_num);
-     if (message_num < trajectory.size())
-     {
-        res = execute(trajectory[message_num].positions, trajectory[message_num].velocities,
-                message_num, trajectory[message_num].time_from_start.count() / 1e6);
-     } else
-     {
-        res = execute(EMPTY_VALUES, EMPTY_VALUES, message_num, 0.0);
-     }
-     if (!res)
-     {
-        finished = true;
-     }
+    if (!server_.readLine((char *)line, MAX_SERVER_BUF_LEN))
+    {
+      LOG_DEBUG("Connection closed. Finishing!");
+      finished = true;
+      break;
+    }
+    unsigned int message_num = atoi((const char *)line);
+    LOG_DEBUG("Received request %i", message_num);
+    if (message_num < trajectory.size())
+    {
+      res = execute(trajectory[message_num].positions, trajectory[message_num].velocities, message_num,
+                    trajectory[message_num].time_from_start.count() / 1e6);
+    }
+    else
+    {
+      res = execute(EMPTY_VALUES, EMPTY_VALUES, message_num, 0.0);
+    }
+    if (!res)
+    {
+      finished = true;
+    }
   }
   return res;
 }

src/ros/mb_publisher.cpp

@@ -20,25 +20,25 @@ void MBPublisher::publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data
 
 void MBPublisher::publishRobotStatus(industrial_msgs::RobotStatus& status, const SharedRobotModeData& data) const
 {
-  //note that this is true as soon as the drives are powered,
-  //even if the breakes are still closed
-  //which is in slight contrast to the comments in the
-  //message definition
+  // note that this is true as soon as the drives are powered,
+  // even if the breakes are still closed
+  // which is in slight contrast to the comments in the
+  // message definition
   status.drives_powered.val = data.robot_power_on;
 
   status.e_stopped.val = data.emergency_stopped;
 
-  //I found no way to reliably get information if the robot is moving
-  //data.programm_running would be true when using this driver to move the robot
-  //but it would also be true when another programm is running that might not
-  //in fact contain any movement commands
+  // I found no way to reliably get information if the robot is moving
+  // data.programm_running would be true when using this driver to move the robot
+  // but it would also be true when another programm is running that might not
+  // in fact contain any movement commands
   status.in_motion.val = industrial_msgs::TriState::UNKNOWN;
 
-  //the error code, if any, is not transmitted by this protocol
-  //it can and should be fetched seperately
+  // the error code, if any, is not transmitted by this protocol
+  // it can and should be fetched seperately
   status.error_code = 0;
 
-  //note that e-stop is handled by a seperate variable
+  // note that e-stop is handled by a seperate variable
   status.in_error.val = data.protective_stopped;
 
   status_pub_.publish(status);
@@ -53,9 +53,9 @@ void MBPublisher::publishRobotStatus(const RobotModeData_V1_X& data) const
   else
     msg.mode.val = industrial_msgs::RobotMode::AUTO;
 
-  //todo: verify that this correct, there is also ROBOT_READY_MODE
-  msg.motion_possible.val = (data.robot_mode == robot_mode_V1_X::ROBOT_RUNNING_MODE)
-                             ? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF;
+  // todo: verify that this correct, there is also ROBOT_READY_MODE
+  msg.motion_possible.val = (data.robot_mode == robot_mode_V1_X::ROBOT_RUNNING_MODE) ? industrial_msgs::TriState::ON :
+                                                                                       industrial_msgs::TriState::OFF;
 
   publishRobotStatus(msg, data);
 }
@@ -64,13 +64,13 @@ void MBPublisher::publishRobotStatus(const RobotModeData_V3_0__1& data) const
 {
   industrial_msgs::RobotStatus msg;
 
-  msg.motion_possible.val = (data.robot_mode == robot_mode_V3_X::RUNNING)
-                             ? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF;
+  msg.motion_possible.val =
+      (data.robot_mode == robot_mode_V3_X::RUNNING) ? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF;
 
   if (data.control_mode == robot_control_mode_V3_X::TEACH)
-      msg.mode.val = industrial_msgs::RobotMode::MANUAL;
+    msg.mode.val = industrial_msgs::RobotMode::MANUAL;
   else
-      msg.mode.val = industrial_msgs::RobotMode::AUTO;
+    msg.mode.val = industrial_msgs::RobotMode::AUTO;
 
   publishRobotStatus(msg, data);
 }

src/ros/service_stopper.cpp

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

src/ros/trajectory_follower.cpp

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

src/ros/urscript_handler.cpp

@@ -1,13 +1,11 @@
 #include "ur_modern_driver/ros/urscript_handler.h"
 #include "ur_modern_driver/log.h"
 
-URScriptHandler::URScriptHandler(URCommander &commander)
-    : commander_(commander)
-    , state_(RobotState::Error)
+URScriptHandler::URScriptHandler(URCommander& commander) : commander_(commander), state_(RobotState::Error)
 {
-    LOG_INFO("Initializing ur_driver/URScript subscriber");
-    urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, &URScriptHandler::urscriptInterface, this);
-    LOG_INFO("The ur_driver/URScript initialized");
+  LOG_INFO("Initializing ur_driver/URScript subscriber");
+  urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, &URScriptHandler::urscriptInterface, this);
+  LOG_INFO("The ur_driver/URScript initialized");
 }
 
 void URScriptHandler::urscriptInterface(const std_msgs::String::ConstPtr& msg)
@@ -15,8 +13,8 @@ void URScriptHandler::urscriptInterface(const std_msgs::String::ConstPtr& msg)
   LOG_INFO("Message received");
   std::string str(msg->data);
   if (str.back() != '\n')
-      str.append("\n");
-  
+    str.append("\n");
+
   switch (state_)
   {
     case RobotState::Running:
@@ -42,5 +40,5 @@ void URScriptHandler::urscriptInterface(const std_msgs::String::ConstPtr& msg)
 
 void URScriptHandler::onRobotStateChange(RobotState state)
 {
-   state_ = state;
+  state_ = state;
 }

src/ros_main.cpp

@@ -9,11 +9,11 @@
 #include "ur_modern_driver/ros/action_server.h"
 #include "ur_modern_driver/ros/controller.h"
 #include "ur_modern_driver/ros/io_service.h"
+#include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h"
 #include "ur_modern_driver/ros/mb_publisher.h"
 #include "ur_modern_driver/ros/rt_publisher.h"
 #include "ur_modern_driver/ros/service_stopper.h"
 #include "ur_modern_driver/ros/trajectory_follower.h"
-#include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h"
 #include "ur_modern_driver/ros/urscript_handler.h"
 #include "ur_modern_driver/ur/commander.h"
 #include "ur_modern_driver/ur/factory.h"
@@ -62,28 +62,31 @@ public:
 class IgnorePipelineStoppedNotifier : public INotifier
 {
 public:
-    void started(std::string name){
-        LOG_INFO("Starting pipeline %s", name.c_str());
-    }
-    void stopped(std::string name){
-        LOG_INFO("Stopping pipeline %s", name.c_str());
-    }
+  void started(std::string name)
+  {
+    LOG_INFO("Starting pipeline %s", name.c_str());
+  }
+  void stopped(std::string name)
+  {
+    LOG_INFO("Stopping pipeline %s", name.c_str());
+  }
 };
 
 class ShutdownOnPipelineStoppedNotifier : public INotifier
 {
 public:
-    void started(std::string name){
-        LOG_INFO("Starting pipeline %s", name.c_str());
-    }
-    void stopped(std::string name){
-        LOG_INFO("Shutting down on stopped pipeline %s", name.c_str());
-        ros::shutdown();
-        exit(1);
-    }
+  void started(std::string name)
+  {
+    LOG_INFO("Starting pipeline %s", name.c_str());
+  }
+  void stopped(std::string name)
+  {
+    LOG_INFO("Shutting down on stopped pipeline %s", name.c_str());
+    ros::shutdown();
+    exit(1);
+  }
 };
 
-
 bool parse_args(ProgArgs &args)
 {
   if (!ros::param::get(IP_ADDR_ARG, args.host))
@@ -121,9 +124,9 @@ int main(int argc, char **argv)
     return EXIT_FAILURE;
   }
 
-  //Add prefix to joint names
-  std::transform (args.joint_names.begin(), args.joint_names.end(), args.joint_names.begin(),
-        [&args](std::string name){return args.prefix + name;});
+  // Add prefix to joint names
+  std::transform(args.joint_names.begin(), args.joint_names.end(), args.joint_names.begin(),
+                 [&args](std::string name) { return args.prefix + name; });
 
   std::string local_ip(getLocalIPAccessibleFromHost(args.host));
 
@@ -144,8 +147,8 @@ int main(int argc, char **argv)
   if (args.use_ros_control)
   {
     LOG_INFO("ROS control enabled");
-    TrajectoryFollower *traj_follower = new TrajectoryFollower(
-        *rt_commander, local_ip, args.reverse_port, factory.isVersion3());
+    TrajectoryFollower *traj_follower =
+        new TrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
     controller = new ROSController(*rt_commander, *traj_follower, args.joint_names, args.max_vel_change, args.tcp_link);
     rt_vec.push_back(controller);
     services.push_back(controller);
@@ -157,8 +160,8 @@ int main(int argc, char **argv)
     if (args.use_lowbandwidth_trajectory_follower)
     {
       LOG_INFO("Use low bandwidth trajectory follower");
-      traj_follower = new LowBandwidthTrajectoryFollower(*rt_commander,
-           local_ip, args.reverse_port,factory.isVersion3());
+      traj_follower =
+          new LowBandwidthTrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
     }
     else
     {

src/tcp_socket.cpp

@@ -113,14 +113,13 @@ std::string TCPSocket::getIP()
 
 bool TCPSocket::read(char *character)
 {
-    size_t read_chars;
-    // It's inefficient, but in our case we read very small messages
-    // and the overhead connected with reading character by character is
-    // negligible - adding buffering would complicate the code needlessly.
-    return read((uint8_t *) character, 1, read_chars);
+  size_t read_chars;
+  // It's inefficient, but in our case we read very small messages
+  // and the overhead connected with reading character by character is
+  // negligible - adding buffering would complicate the code needlessly.
+  return read((uint8_t *)character, 1, read_chars);
 }
 
-
 bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read)
 {
   read = 0;

src/ur/commander.cpp

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

src/ur/server.cpp

@@ -31,8 +31,7 @@ std::string URServer::getIP()
   return std::string(buf);
 }
 
-
-bool URServer::open(int socket_fd, struct sockaddr *address, size_t address_len)
+bool URServer::open(int socket_fd, struct sockaddr* address, size_t address_len)
 {
   int flag = 1;
   setsockopt(socket_fd, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int));
@@ -63,9 +62,10 @@ bool URServer::accept()
   int client_fd = -1;
 
   int retry = 0;
-  while((client_fd = ::accept(getSocketFD(), &addr, &addr_len)) == -1){
+  while ((client_fd = ::accept(getSocketFD(), &addr, &addr_len)) == -1)
+  {
     LOG_ERROR("Accepting socket connection failed. (errno: %d)", errno);
-    if(retry++ >= 5)
+    if (retry++ >= 5)
       return false;
   }
 
@@ -89,41 +89,43 @@ bool URServer::write(const uint8_t* buf, size_t buf_len, size_t& written)
 
 bool URServer::readLine(char* buffer, size_t buf_len)
 {
-    char *current_pointer = buffer;
-    char ch;
-    size_t total_read;
+  char* current_pointer = buffer;
+  char ch;
+  size_t total_read;
 
-    if (buf_len <= 0 || buffer == NULL) {
+  if (buf_len <= 0 || buffer == NULL)
+  {
+    return false;
+  }
+
+  total_read = 0;
+  for (;;)
+  {
+    if (client_.read(&ch))
+    {
+      if (total_read < buf_len - 1)  // just in case ...
+      {
+        total_read++;
+        *current_pointer++ = ch;
+      }
+      if (ch == '\n')
+      {
+        break;
+      }
+    }
+    else
+    {
+      if (total_read == 0)
+      {
         return false;
+      }
+      else
+      {
+        break;
+      }
     }
+  }
 
-    total_read = 0;
-    for (;;) {
-        if (client_.read(&ch))
-        {
-            if (total_read < buf_len - 1) // just in case ...
-            {
-                total_read ++;
-                *current_pointer++ = ch;
-            }
-            if (ch == '\n')
-            {
-                break;
-            }
-        }
-        else
-        {
-            if (total_read == 0)
-            {
-                return false;
-            }
-            else
-            {
-                break;
-            }
-        }
-    }
-
-    *current_pointer = '\0';
-    return true;
+  *current_pointer = '\0';
+  return true;
 }