Merge pull request #1 from TAMS-Group/fork_master

Fix runtime issues

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<int>(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<int>(type));
         sbp.debug();
         return false;
       }

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

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

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

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)
+  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;
+  }
 
   TCPSocket::setOptions(client_fd);