Fix runtime issues

include/ur_modern_driver/ur/state_parser.h

@@ -75,15 +75,15 @@ class URStateParser : public URParser<StatePacket>
 
       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<int>(type));
         return false;
       }
 
       results.push_back(std::move(packet));
 
       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<int>(type));
         sbp.debug();
         return false;
       }
@@ -95,4 +95,4 @@ class URStateParser : public URParser<StatePacket>
 
 typedef URStateParser<RobotModeData_V1_X, MasterBoardData_V1_X> URStateParser_V1_X;
 typedef URStateParser<RobotModeData_V3_0__1, MasterBoardData_V3_0__1> URStateParser_V3_0__1;
-typedef URStateParser<RobotModeData_V3_2, MasterBoardData_V3_2> URStateParser_V3_2;
+typedef URStateParser<RobotModeData_V3_2, MasterBoardData_V3_2> URStateParser_V3_2;

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

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

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");
@@ -84,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);
@@ -155,4 +160,4 @@ int main(int argc, char **argv)
   LOG_INFO("Pipelines shutdown complete");
 
   return EXIT_SUCCESS;
-}
+}

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){
+    LOG_ERROR("Accepting socket connection failed. (errno: %d)", 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);
-}
+}