From d7a59872b233b53e5d78608f07246dcd9b1146bc Mon Sep 17 00:00:00 2001 From: Miguel Prada Date: Tue, 20 Nov 2018 09:45:10 +0100 Subject: [PATCH 1/2] Enable clang-format checking in CI --- .travis.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.travis.yml b/.travis.yml index 9dbfa22..b02e2fb 100644 --- a/.travis.yml +++ b/.travis.yml @@ -14,6 +14,7 @@ env: - ROS_DISTRO="indigo" ROS_REPO=ros-shadow-fixed NOT_TEST_INSTALL=true - ROS_DISTRO="kinetic" ROS_REPO=ros NOT_TEST_INSTALL=true - ROS_DISTRO="kinetic" ROS_REPO=ros-shadow-fixed NOT_TEST_INSTALL=true + - ROS_DISTRO="kinetic" CLANG_FORMAT_CHECK=file install: - git clone --depth=1 https://github.com/ros-industrial/industrial_ci.git .ci_config From 9e276a336874c34875451645dabd94f23eb0d64d Mon Sep 17 00:00:00 2001 From: Miguel Prada Date: Tue, 20 Nov 2018 09:47:17 +0100 Subject: [PATCH 2/2] Format sources with clang-format --- include/ur_modern_driver/pipeline.h | 5 +- .../action_trajectory_follower_interface.h | 2 +- .../ros/lowbandwidth_trajectory_follower.h | 15 ++-- include/ur_modern_driver/ros/mb_publisher.h | 5 +- .../ros/trajectory_follower.h | 4 +- .../ur_modern_driver/ros/urscript_handler.h | 5 +- include/ur_modern_driver/test/random_data.h | 4 +- include/ur_modern_driver/ur/server.h | 2 +- src/ros/action_server.cpp | 11 ++- src/ros/controller.cpp | 2 +- src/ros/lowbandwidth_trajectory_follower.cpp | 64 ++++++++-------- src/ros/mb_publisher.cpp | 36 ++++----- src/ros/service_stopper.cpp | 3 +- src/ros/trajectory_follower.cpp | 2 +- src/ros/urscript_handler.cpp | 16 ++-- src/ros_main.cpp | 49 ++++++------ src/tcp_socket.cpp | 11 ++- src/ur/commander.cpp | 18 +++-- src/ur/server.cpp | 76 ++++++++++--------- tests/ur/master_board.cpp | 10 +-- 20 files changed, 177 insertions(+), 163 deletions(-) diff --git a/include/ur_modern_driver/pipeline.h b/include/ur_modern_driver/pipeline.h index ba71009..f64aa4b 100644 --- a/include/ur_modern_driver/pipeline.h +++ b/include/ur_modern_driver/pipeline.h @@ -110,7 +110,6 @@ public: } }; - template class Pipeline { @@ -140,7 +139,7 @@ private: { if (!queue_.try_enqueue(std::move(p))) { - LOG_ERROR("Pipeline producer overflowed! <%s>",name_.c_str()); + LOG_ERROR("Pipeline producer overflowed! <%s>", name_.c_str()); } } @@ -201,7 +200,7 @@ public: if (!running_) return; - LOG_DEBUG("Stopping pipeline! <%s>",name_.c_str()); + LOG_DEBUG("Stopping pipeline! <%s>", name_.c_str()); consumer_.stopConsumer(); producer_.stopProducer(); diff --git a/include/ur_modern_driver/ros/action_trajectory_follower_interface.h b/include/ur_modern_driver/ros/action_trajectory_follower_interface.h index 9b5427f..897c89c 100644 --- a/include/ur_modern_driver/ros/action_trajectory_follower_interface.h +++ b/include/ur_modern_driver/ros/action_trajectory_follower_interface.h @@ -28,5 +28,5 @@ public: virtual bool start() = 0; virtual bool execute(std::vector &trajectory, std::atomic &interrupt) = 0; virtual void stop() = 0; - virtual ~ActionTrajectoryFollowerInterface() {}; + virtual ~ActionTrajectoryFollowerInterface(){}; }; diff --git a/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h b/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h index 6442dcb..d1274d6 100644 --- a/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h +++ b/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h @@ -9,11 +9,11 @@ #include #include #include "ur_modern_driver/log.h" +#include "ur_modern_driver/ros/action_trajectory_follower_interface.h" #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/server.h" -#include "ur_modern_driver/ros/action_trajectory_follower_interface.h" -class LowBandwidthTrajectoryFollower: public ActionTrajectoryFollowerInterface +class LowBandwidthTrajectoryFollower : public ActionTrajectoryFollowerInterface { private: std::atomic running_; @@ -21,14 +21,13 @@ private: URCommander &commander_; URServer server_; - double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, \ - servoj_gain_, servoj_lookahead_time_, max_joint_difference_; + double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, servoj_gain_, servoj_lookahead_time_, + max_joint_difference_; std::string program_; - bool execute(const std::array &positions, - const std::array &velocities, - double sample_number, double time_in_seconds); + bool execute(const std::array &positions, const std::array &velocities, double sample_number, + double time_in_seconds); public: LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3); @@ -37,5 +36,5 @@ public: bool execute(std::vector &trajectory, std::atomic &interrupt); void stop(); - virtual ~LowBandwidthTrajectoryFollower() {}; + virtual ~LowBandwidthTrajectoryFollower(){}; }; diff --git a/include/ur_modern_driver/ros/mb_publisher.h b/include/ur_modern_driver/ros/mb_publisher.h index f4b1549..24a7ef4 100644 --- a/include/ur_modern_driver/ros/mb_publisher.h +++ b/include/ur_modern_driver/ros/mb_publisher.h @@ -35,8 +35,9 @@ private: void publishRobotStatus(const RobotModeData_V3_0__1& data) const; public: - MBPublisher() : io_pub_(nh_.advertise("ur_driver/io_states", 1)) - , status_pub_(nh_.advertise("ur_driver/robot_status", 1)) + MBPublisher() + : io_pub_(nh_.advertise("ur_driver/io_states", 1)) + , status_pub_(nh_.advertise("ur_driver/robot_status", 1)) { } diff --git a/include/ur_modern_driver/ros/trajectory_follower.h b/include/ur_modern_driver/ros/trajectory_follower.h index af37474..9eec6e3 100644 --- a/include/ur_modern_driver/ros/trajectory_follower.h +++ b/include/ur_modern_driver/ros/trajectory_follower.h @@ -9,9 +9,9 @@ #include #include #include "ur_modern_driver/log.h" +#include "ur_modern_driver/ros/action_trajectory_follower_interface.h" #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/server.h" -#include "ur_modern_driver/ros/action_trajectory_follower_interface.h" class TrajectoryFollower : public ActionTrajectoryFollowerInterface { @@ -43,5 +43,5 @@ public: bool execute(std::vector &trajectory, std::atomic &interrupt); void stop(); - virtual ~TrajectoryFollower() {}; + virtual ~TrajectoryFollower(){}; }; diff --git a/include/ur_modern_driver/ros/urscript_handler.h b/include/ur_modern_driver/ros/urscript_handler.h index f5190db..98297fc 100644 --- a/include/ur_modern_driver/ros/urscript_handler.h +++ b/include/ur_modern_driver/ros/urscript_handler.h @@ -8,10 +8,9 @@ #include "ur_modern_driver/ros/service_stopper.h" #include "ur_modern_driver/ur/commander.h" -class URScriptHandler: public Service +class URScriptHandler : public Service { private: - ros::NodeHandle nh_; URCommander &commander_; ros::Subscriber urscript_sub_; @@ -19,6 +18,6 @@ private: public: URScriptHandler(URCommander &commander); - void urscriptInterface(const std_msgs::String::ConstPtr& msg); + void urscriptInterface(const std_msgs::String::ConstPtr &msg); void onRobotStateChange(RobotState state); }; diff --git a/include/ur_modern_driver/test/random_data.h b/include/ur_modern_driver/test/random_data.h index 4f98386..6ccf9bc 100644 --- a/include/ur_modern_driver/test/random_data.h +++ b/include/ur_modern_driver/test/random_data.h @@ -1,11 +1,11 @@ #pragma once #include #include +#include #include #include #include #include -#include #include "ur_modern_driver/bin_parser.h" class RandomDataTest @@ -52,7 +52,7 @@ public: template void set(T data, size_t pos) { - std::memcpy(&data, buf_+pos, sizeof(T)); + std::memcpy(&data, buf_ + pos, sizeof(T)); } void skip(size_t n) diff --git a/include/ur_modern_driver/ur/server.h b/include/ur_modern_driver/ur/server.h index 1809237..5246840 100644 --- a/include/ur_modern_driver/ur/server.h +++ b/include/ur_modern_driver/ur/server.h @@ -26,6 +26,6 @@ public: bool bind(); bool accept(); void disconnectClient(); - bool readLine(char* buffer, size_t buf_len); + bool readLine(char *buffer, size_t buf_len); bool write(const uint8_t *buf, size_t buf_len, size_t &written); }; diff --git a/src/ros/action_server.cpp b/src/ros/action_server.cpp index 8616c35..b68fc62 100644 --- a/src/ros/action_server.cpp +++ b/src/ros/action_server.cpp @@ -1,7 +1,8 @@ #include "ur_modern_driver/ros/action_server.h" #include -ActionServer::ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector& joint_names, double max_velocity) +ActionServer::ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector& 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; } } diff --git a/src/ros/controller.cpp b/src/ros/controller.cpp index b73f437..08161a3 100644 --- a/src/ros/controller.cpp +++ b/src/ros/controller.cpp @@ -58,7 +58,7 @@ void ROSController::doSwitch(const std::list return; } - if(start_list.size() > 0) + if (start_list.size() > 0) LOG_WARN("Failed to start interface!"); } diff --git a/src/ros/lowbandwidth_trajectory_follower.cpp b/src/ros/lowbandwidth_trajectory_follower.cpp index 75c47f7..0371f7c 100644 --- a/src/ros/lowbandwidth_trajectory_follower.cpp +++ b/src/ros/lowbandwidth_trajectory_follower.cpp @@ -1,9 +1,9 @@ #include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h" #include -#include #include +#include -static const std::array EMPTY_VALUES = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; +static const std::array 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 &positions, - const std::array &velocities, - double sample_number, double time_in_seconds) + const std::array &velocities, double sample_number, + double time_in_seconds) { if (!running_) return false; @@ -317,11 +318,11 @@ bool LowBandwidthTrajectoryFollower::execute(const std::array &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 &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 &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; } diff --git a/src/ros/mb_publisher.cpp b/src/ros/mb_publisher.cpp index 9d9832f..71d03d2 100644 --- a/src/ros/mb_publisher.cpp +++ b/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); } diff --git a/src/ros/service_stopper.cpp b/src/ros/service_stopper.cpp index fcbf1a1..5a48d2e 100644 --- a/src/ros/service_stopper.cpp +++ b/src/ros/service_stopper.cpp @@ -20,7 +20,8 @@ ServiceStopper::ServiceStopper(std::vector 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); diff --git a/src/ros/trajectory_follower.cpp b/src/ros/trajectory_follower.cpp index bec6db7..10e645f 100644 --- a/src/ros/trajectory_follower.cpp +++ b/src/ros/trajectory_follower.cpp @@ -1,7 +1,7 @@ #include "ur_modern_driver/ros/trajectory_follower.h" #include -#include #include +#include static const int32_t MULT_JOINTSTATE_ = 1000000; static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}"); diff --git a/src/ros/urscript_handler.cpp b/src/ros/urscript_handler.cpp index 565bf92..7e50086 100644 --- a/src/ros/urscript_handler.cpp +++ b/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; } diff --git a/src/ros_main.cpp b/src/ros_main.cpp index 5343334..275a72a 100644 --- a/src/ros_main.cpp +++ b/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 { diff --git a/src/tcp_socket.cpp b/src/tcp_socket.cpp index 6a057bd..8fc96b7 100644 --- a/src/tcp_socket.cpp +++ b/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; diff --git a/src/ur/commander.cpp b/src/ur/commander.cpp index 44f5867..bece27f 100644 --- a/src/ur/commander.cpp +++ b/src/ur/commander.cpp @@ -22,7 +22,7 @@ void URCommander::formatArray(std::ostringstream &out, std::array &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 &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); } diff --git a/src/ur/server.cpp b/src/ur/server.cpp index 557ed5d..1257e59 100644 --- a/src/ur/server.cpp +++ b/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; } diff --git a/tests/ur/master_board.cpp b/tests/ur/master_board.cpp index 2af943f..64de13f 100644 --- a/tests/ur/master_board.cpp +++ b/tests/ur/master_board.cpp @@ -9,7 +9,7 @@ TEST(MasterBoardData_V1_X, testRandomDataParsing) { RandomDataTest rdt(71); - rdt.set(1, 58); //sets euromap67_interface_installed to true + rdt.set(1, 58); // sets euromap67_interface_installed to true BinParser bp = rdt.getParser(); MasterBoardData_V1_X state; @@ -36,14 +36,14 @@ TEST(MasterBoardData_V1_X, testRandomDataParsing) ASSERT_EQ(rdt.getNext(), state.euromap_output_bits); ASSERT_EQ(rdt.getNext(), state.euromap_voltage); ASSERT_EQ(rdt.getNext(), state.euromap_current); - + ASSERT_TRUE(bp.empty()) << "Did not consume all data"; } TEST(MasterBoardData_V3_0__1, testRandomDataParsing) { RandomDataTest rdt(83); - rdt.set(1, 62); //sets euromap67_interface_installed to true + rdt.set(1, 62); // sets euromap67_interface_installed to true BinParser bp = rdt.getParser(); MasterBoardData_V3_0__1 state; ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false"; @@ -69,7 +69,7 @@ TEST(MasterBoardData_V3_0__1, testRandomDataParsing) ASSERT_EQ(rdt.getNext(), state.euromap_output_bits); ASSERT_EQ(rdt.getNext(), state.euromap_voltage); ASSERT_EQ(rdt.getNext(), state.euromap_current); - + rdt.skip(sizeof(uint32_t)); ASSERT_TRUE(bp.empty()) << "Did not consume all data"; @@ -78,7 +78,7 @@ TEST(MasterBoardData_V3_0__1, testRandomDataParsing) TEST(MasterBoardData_V3_2, testRandomDataParsing) { RandomDataTest rdt(85); - rdt.set(1, 62); //sets euromap67_interface_installed to true + rdt.set(1, 62); // sets euromap67_interface_installed to true BinParser bp = rdt.getParser(); MasterBoardData_V3_2 state; ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false";