From 1e4934a1992b31c10c3604af162221057afcd9ee Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 21 Jul 2017 17:32:13 +0200 Subject: [PATCH 1/7] Adds missing param max_velocity. --- src/ros_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/ros_main.cpp b/src/ros_main.cpp index 4f26634..b680ff6 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -60,6 +60,7 @@ bool parse_args(ProgArgs &args) } 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_velocity, 10.0); 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"); From 54a852305cf99e7ca730d06d7f7550bb55b78b4f Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 21 Jul 2017 17:33:28 +0200 Subject: [PATCH 2/7] Adds prefix to all joint names. --- src/ros_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/ros_main.cpp b/src/ros_main.cpp index b680ff6..8d74e3e 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -85,6 +85,10 @@ 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;}); + std::string local_ip(getLocalIPAccessibleFromHost(args.host)); URFactory factory(args.host); @@ -156,4 +160,4 @@ int main(int argc, char **argv) LOG_INFO("Pipelines shutdown complete"); return EXIT_SUCCESS; -} \ No newline at end of file +} From 63691e038e393abb531b0fd2e1b90b9ddd889c97 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 21 Jul 2017 17:34:58 +0200 Subject: [PATCH 3/7] Extend error messages Extends error message to print invalid joint names. Adds max_velocity as parameter descriptor in error message. --- src/ros/action_server.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/ros/action_server.cpp b/src/ros/action_server.cpp index 380acd3..494d932 100644 --- a/src/ros/action_server.cpp +++ b/src/ros/action_server.cpp @@ -145,7 +145,11 @@ bool ActionServer::validateJoints(GoalHandle& gh, Result& res) return true; res.error_code = Result::INVALID_JOINTS; - res.error_string = "Invalid joint names for goal"; + 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 + ", ";}); + res.error_string += "\nFound: "; + std::for_each(joint_set_.begin(), joint_set_.end(), [&res](std::string joint){res.error_string += joint + ", ";}); return false; } @@ -183,7 +187,7 @@ 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 " + 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; } } @@ -338,4 +342,4 @@ void ActionServer::trajectoryThread() has_goal_ = false; lk.unlock(); } -} \ No newline at end of file +} From 4375ffecc6b36379e0591e5b40a9294069be9f58 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 21 Jul 2017 17:37:07 +0200 Subject: [PATCH 4/7] Adds multiple retries to accept reverse connection We observed that the accept failed nondeterministically in rare cases. --- src/ur/server.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/ur/server.cpp b/src/ur/server.cpp index be19ae0..4da9b2c 100644 --- a/src/ur/server.cpp +++ b/src/ur/server.cpp @@ -60,10 +60,14 @@ bool URServer::accept() struct sockaddr addr; socklen_t addr_len; - int client_fd = ::accept(getSocketFD(), &addr, &addr_len); + int client_fd = -1; - if (client_fd <= 0) - return false; + int retry = 0; + while((client_fd = ::accept(getSocketFD(), &addr, &addr_len)) == -1){ + ROS_ERROR_STREAM("Accepting socket connection failed. (errno: " << errno << ")"); + if(retry++ >= 5) + return false; + } setOptions(client_fd); @@ -81,4 +85,4 @@ void URServer::disconnectClient() 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 +} From d3637e9633146c090107b943b652d5fe2ffad038 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 21 Jul 2017 17:45:18 +0200 Subject: [PATCH 5/7] Fixing some typos. --- src/ros/trajectory_follower.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/ros/trajectory_follower.cpp b/src/ros/trajectory_follower.cpp index 18ca691..bec6db7 100644 --- a/src/ros/trajectory_follower.cpp +++ b/src/ros/trajectory_follower.cpp @@ -111,11 +111,11 @@ bool TrajectoryFollower::start() return false; } - LOG_DEBUG("Awaiting incomming robot connection"); + LOG_DEBUG("Awaiting incoming robot connection"); if (!server_.accept()) { - LOG_ERROR("Failed to accept incomming robot connection"); + LOG_ERROR("Failed to accept incoming robot connection"); return false; } @@ -240,4 +240,4 @@ void TrajectoryFollower::stop() server_.disconnectClient(); running_ = false; -} \ No newline at end of file +} From 75c3733eb75789761e6383d587c7913b2ad7865e Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 21 Jul 2017 18:02:48 +0200 Subject: [PATCH 6/7] Fix type warning in log output --- include/ur_modern_driver/ur/state_parser.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/ur_modern_driver/ur/state_parser.h b/include/ur_modern_driver/ur/state_parser.h index 48eda1d..146d93c 100644 --- a/include/ur_modern_driver/ur/state_parser.h +++ b/include/ur_modern_driver/ur/state_parser.h @@ -75,7 +75,7 @@ public: if (!packet->parseWith(sbp)) { - LOG_ERROR("Sub-package parsing of type %d failed!", type); + LOG_ERROR("Sub-package parsing of type %d failed!", static_cast(type)); return false; } @@ -83,7 +83,7 @@ public: if (!sbp.empty()) { - LOG_ERROR("Sub-package of type %d was not parsed completely!", type); + LOG_ERROR("Sub-package of type %d was not parsed completely!", static_cast(type)); sbp.debug(); return false; } @@ -95,4 +95,4 @@ public: typedef URStateParser URStateParser_V1_X; typedef URStateParser URStateParser_V3_0__1; -typedef URStateParser URStateParser_V3_2; \ No newline at end of file +typedef URStateParser URStateParser_V3_2; From 45480881ffe32ecf5c89358b30443ad766ca9aea Mon Sep 17 00:00:00 2001 From: Henning <1kayser@informatik.uni-hamburg.de> Date: Mon, 24 Jul 2017 12:20:45 +0200 Subject: [PATCH 7/7] Change ROS_ERROR_STREAM to LOG_ERROR. --- src/ur/server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ur/server.cpp b/src/ur/server.cpp index 4da9b2c..cbdaa5b 100644 --- a/src/ur/server.cpp +++ b/src/ur/server.cpp @@ -64,7 +64,7 @@ bool URServer::accept() int retry = 0; while((client_fd = ::accept(getSocketFD(), &addr, &addr_len)) == -1){ - ROS_ERROR_STREAM("Accepting socket connection failed. (errno: " << errno << ")"); + LOG_ERROR("Accepting socket connection failed. (errno: %d)", errno); if(retry++ >= 5) return false; }