WIP

include/ur_modern_driver/bin_parser.h

@@ -68,7 +68,7 @@ public:
   template <typename T>
   T peek()
   {
-    assert(buf_pos_ <= buf_end_);
+    assert(buf_pos_ + sizeof(T) <= buf_end_);
     T val;
     std::memcpy(&val, buf_pos_, sizeof(T));
     return decode(val);

include/ur_modern_driver/ros/trajectory_follower.h

@@ -49,12 +49,11 @@ private:
     return s;
   }
 
-  std::string buildProgram();
   bool execute(std::array<double, 6> &positions, bool keep_alive);  
   double interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel); 
 
 public:
-  TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3);
+  TrajectoryFollower(URCommander &commander, std::string& reverse_ip, int reverse_port, bool version_3);
 
   bool start();
   bool execute(std::array<double, 6> &positions);

include/ur_modern_driver/tcp_socket.h

@@ -29,8 +29,6 @@ protected:
   
   
   bool setup(std::string &host, int port);
-  void close();
-
 
 public:
   TCPSocket();
@@ -41,6 +39,10 @@ public:
   int getSocketFD() { return socket_fd_; }
   bool setSocketFD(int socket_fd);
 
+  std::string getIP();
+
   bool read(uint8_t* buf, size_t buf_len, size_t &read);
   bool write(const uint8_t* buf, size_t buf_len, size_t &written);
+
+  void close();  
 };

include/ur_modern_driver/ur/producer.h

@@ -50,6 +50,10 @@ public:
 
       LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count());
       std::this_thread::sleep_for(timeout_);
+
+      if(stream_.connect())
+        continue;
+
       auto next = timeout_ * 2;
       if(next <= std::chrono::seconds(120))
         timeout_ = next;

include/ur_modern_driver/ur/server.h

@@ -12,7 +12,6 @@ class URServer : private TCPSocket
 {
 private:
   int port_;
-  SocketState state_;
   TCPSocket client_;
 
 protected:
@@ -28,5 +27,6 @@ public:
   std::string getIP();
   bool bind();
   bool accept();
+  void disconnectClient();
   bool write(const uint8_t* buf, size_t buf_len, size_t &written);
 };

include/ur_modern_driver/ur/state_parser.h

@@ -34,7 +34,7 @@ public:
     bp.parse(type);
 
     //quietly ignore the intial version message
-    if (type == message_type::ROBOT_MESSAGE)
+    if (type == message_type::ROBOT_MESSAGE || ((int)type) == 5)
     {
       bp.consume();
       return true;
@@ -42,7 +42,7 @@ public:
 
     if (type != message_type::ROBOT_STATE)
     {
-      LOG_WARN("Invalid message type recieved: %u", static_cast<uint8_t>(type));
+      LOG_WARN("Invalid state message type recieved: %u", static_cast<uint8_t>(type));
       return false;
     }
 
@@ -60,7 +60,7 @@ public:
         return false;
       }
 
-      LOG_DEBUG("sub-packet size: %" PRIu32, sub_size);
+      //LOG_DEBUG("sub-packet size: %" PRIu32, sub_size);
 
       // deconstruction of a sub parser will increment the position of the parent parser
       BinParser sbp(bp, sub_size);
@@ -73,7 +73,7 @@ public:
       if (packet == nullptr)
       {
         sbp.consume();
-        LOG_DEBUG("Skipping sub-packet of type %d", type);
+        //LOG_DEBUG("Skipping sub-packet of type %d", type);
         continue;
       }
 

include/ur_modern_driver/ur/stream.h

@@ -8,7 +8,7 @@
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/tcp_socket.h"
 
-class URStream : private TCPSocket
+class URStream : public TCPSocket
 {
 private:
   std::string host_;

src/ros/action_server.cpp

@@ -51,12 +51,12 @@ void ActionServer::onRobotStateChange(RobotState state)
   }
 
   interrupt_traj_ = true;
-  //wait for goal to be interrupted
+  //wait for goal to be interrupted and automagically unlock when going out of scope
   std::lock_guard<std::mutex> lock(tj_mutex_);
 
   Result res;
   res.error_code = -100;
-  res.error_string = "Received another trajectory";
+  res.error_string = "Robot safety stop";
   curr_gh_.setAborted(res, res.error_string);
 }
 
@@ -212,8 +212,8 @@ bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res)
 
 inline std::chrono::microseconds convert(const ros::Duration &dur)
 {
-  return std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::seconds(dur.sec)) 
-    + std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::nanoseconds(dur.nsec));
+  return std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::seconds(dur.sec) +
+    std::chrono::nanoseconds(dur.nsec));
 }
 
 bool ActionServer::try_execute(GoalHandle& gh, Result& res)
@@ -261,7 +261,7 @@ std::vector<size_t> ActionServer::reorderMap(std::vector<std::string> goal_joint
 void ActionServer::trajectoryThread()
 {
   LOG_INFO("Trajectory thread started");
-  follower_.start(); //todo check error
+  
   while(running_)
   {
     std::unique_lock<std::mutex> lk(tj_mutex_);
@@ -272,7 +272,8 @@ void ActionServer::trajectoryThread()
     curr_gh_.setAccepted();
 
     auto goal = curr_gh_.getGoal();
-    std::vector<TrajectoryPoint> trajectory(goal->trajectory.points.size());
+    std::vector<TrajectoryPoint> trajectory;
+    trajectory.reserve(goal->trajectory.points.size()+1);
 
     //joint names of the goal might have a different ordering compared
     //to what URScript expects so need to map between the two
@@ -281,7 +282,7 @@ void ActionServer::trajectoryThread()
     LOG_INFO("Translating trajectory");
 
     auto const& fp = goal->trajectory.points[0];
-    auto fpt = convert(fp.time_from_start)
+    auto fpt = convert(fp.time_from_start);
 
     //make sure we have a proper t0 position
     if(fpt > std::chrono::microseconds(0))
@@ -290,6 +291,7 @@ void ActionServer::trajectoryThread()
       trajectory.push_back(TrajectoryPoint(q_actual_, qd_actual_, std::chrono::microseconds(0)));
     }
 
+    int j = 0;
     for(auto const& point : goal->trajectory.points)
     {
       std::array<double, 6> pos, vel;
@@ -299,13 +301,18 @@ void ActionServer::trajectoryThread()
         pos[idx] = point.positions[i];
         vel[idx] = point.velocities[i];
       }
-      trajectory.push_back(TrajectoryPoint(pos, vel, convert(point.time_from_start)));
+      auto t = convert(point.time_from_start);
+      LOG_DEBUG("P[%d] = %ds, %dns -> %dus", j++, point.time_from_start.sec, point.time_from_start.nsec, t.count());
+      trajectory.push_back(TrajectoryPoint(pos, vel, t));
     }
 
     double t = std::chrono::duration_cast<std::chrono::duration<double>>(trajectory[trajectory.size()-1].time_from_start).count();
-    LOG_INFO("Trajectory with %d points and duration of %f constructed, executing now", trajectory.size(), t);
+    LOG_INFO("Executing trajectory with %d points and duration of %4.3fs", trajectory.size(), t);
 
     Result res;
+
+    if(follower_.start())
+    {
       if(follower_.execute(trajectory, interrupt_traj_))
       {
         //interrupted goals must be handled by interrupt trigger
@@ -326,8 +333,17 @@ void ActionServer::trajectoryThread()
         curr_gh_.setAborted(res, res.error_string);
       }
 
+      follower_.stop();
+    }
+    else
+    {
+      LOG_ERROR("Failed to start trajectory follower!");
+      res.error_code = -100;
+      res.error_string = "Robot connection could not be established";      
+      curr_gh_.setAborted(res, res.error_string);
+    }
+
     has_goal_ = false;
     lk.unlock();
   }
-  follower_.stop();
 }

src/ros/trajectory_follower.cpp

@@ -65,7 +65,7 @@ def driverProg():
 end
 )";
 
-TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3)
+TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reverse_ip, int reverse_port, bool version_3)
   : running_(false)
   , commander_(commander)
   , reverse_port_(reverse_port)
@@ -75,7 +75,6 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port,
   , server_(reverse_port)
 {
   std::string res(POSITION_PROGRAM);
-
   res.replace(res.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_));
 
   std::ostringstream out;
@@ -85,17 +84,16 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port,
 
   res.replace(res.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
 
-  program_ = res;
+  if(!server_.bind())
+  {
+    LOG_ERROR("Failed to bind server, the port %d is likely already in use", reverse_port);
+    std::exit(-1);
   }  
 
-std::string TrajectoryFollower::buildProgram()
-{
-  std::string res(program_);
-  std::string IP(server_.getIP());
-  LOG_INFO("Local IP: %s  ", IP.c_str());
-  res.replace(res.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), "127.0.0.1");
+  res.replace(res.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), reverse_ip);
   res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port_));
-  return res;
+
+  program_ = res;
 }
 
 bool TrajectoryFollower::start()
@@ -103,23 +101,15 @@ bool TrajectoryFollower::start()
   if(running_)
     return true; //not sure
 
-  if(!server_.bind())
-  {
-    LOG_ERROR("Failed to bind server");
-    return false;
-  }
-
   LOG_INFO("Uploading trajectory program to robot");
   
-  std::string prog(buildProgram());
-  //std::string prog = "socket_open(\"127.0.0.1\", 50001)\n";
-  if(!commander_.uploadProg(prog))
+  if(!commander_.uploadProg(program_))
   {
     LOG_ERROR("Program upload failed!");
     return false;
   }
 
-  LOG_INFO("Awaiting incomming robot connection");
+  LOG_DEBUG("Awaiting incomming robot connection");
 
   if(!server_.accept())
   {
@@ -127,7 +117,7 @@ bool TrajectoryFollower::start()
     return false;
   }
   
-  LOG_INFO("Robot successfully connected");
+  LOG_DEBUG("Robot successfully connected");
   return (running_ = true);
 }
 
@@ -182,7 +172,7 @@ bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::
   typedef high_resolution_clock Clock;
   typedef Clock::time_point Time;
 
-  auto const& last = trajectory[trajectory.size()-1];
+  auto& last = trajectory[trajectory.size()-1];
   auto& prev = trajectory[0];
 
   Time t0 = Clock::now();
@@ -202,8 +192,6 @@ bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::
     auto duration = point.time_from_start - prev.time_from_start;
     double d_s = duration_cast<double_seconds>(duration).count();
 
-    LOG_INFO("Point duration: %f", d_s);
-
     //interpolation loop
     while(!interrupt)
     {
@@ -243,15 +231,9 @@ bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::
   //the interpolation loop above but rather some position between
   //t[N-1] and t[N] where N is the number of trajectory points.
   //To make sure this does not happen the last position is sent 
-  //if not interrupted.
-  if(!interrupt)
-  {
   return execute(last.positions, true);
 }
 
-  return true;
-}
-
 void TrajectoryFollower::stop()
 {
   if(!running_)
@@ -260,6 +242,6 @@ void TrajectoryFollower::stop()
   std::array<double, 6> empty;
   execute(empty, false);
 
-  //server_.disconnect();
+  server_.disconnectClient();
   running_ = false;
 }

src/ros_main.cpp

@@ -74,7 +74,11 @@ bool parse_args(ProgArgs &args)
   return true;
 }
 
-#include "ur_modern_driver/ur/server.h"
+std::string getLocalIPAccessibleFromHost(std::string& host)
+{
+  URStream stream(host, UR_RT_PORT);
+  return stream.connect() ? stream.getIP() : std::string();
+}
 
 int main(int argc, char **argv)
 {
@@ -86,9 +90,9 @@ int main(int argc, char **argv)
     return EXIT_FAILURE;
   }
 
+  std::string local_ip(getLocalIPAccessibleFromHost(args.host));
   
   URFactory factory(args.host);
-
   vector<Service*> services;
 
   // RT packets
@@ -99,7 +103,7 @@ int main(int argc, char **argv)
   auto rt_commander = factory.getCommander(rt_stream);
   vector<IConsumer<RTPacket> *> rt_vec{&rt_pub};
 
-  TrajectoryFollower traj_follower(*rt_commander, args.reverse_port, factory.isVersion3());
+  TrajectoryFollower traj_follower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
 
   ROSController *controller(nullptr);
   ActionServer *action_server(nullptr);

src/tcp_socket.cpp

@@ -1,5 +1,6 @@
 #include <endian.h>
 #include <netinet/tcp.h>
+#include <arpa/inet.h>
 #include <unistd.h>
 #include <cstring>
 
@@ -94,6 +95,23 @@ void TCPSocket::close()
   socket_fd_ = -1;
 }
 
+std::string TCPSocket::getIP()
+{
+  sockaddr_in name;
+  socklen_t len = sizeof(name);
+  int res = ::getsockname(socket_fd_, (sockaddr*)&name, &len);
+
+  if(res < 0)
+  {
+    LOG_ERROR("Could not get local IP");
+    return std::string();
+  }
+
+  char buf[128];
+  inet_ntop(AF_INET, &name.sin_addr, buf, sizeof(buf));
+  return std::string(buf);
+}
+
 bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read)
 {
   read = 0;

src/ur/server.cpp

@@ -1,5 +1,6 @@
 #include <cstring>
 #include <netinet/tcp.h>
+#include <arpa/inet.h>
 #include <unistd.h>
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/ur/server.h"
@@ -19,8 +20,18 @@ void URServer::setOptions(int socket_fd)
 
 std::string URServer::getIP()
 {
+  sockaddr_in name;
+  socklen_t len = sizeof(name);
+  int res = ::getsockname(getSocketFD(), (sockaddr*)&name, &len);
+
+  if(res < 0)
+  {
+    LOG_ERROR("Could not get local IP");
+    return std::string();
+  }
+
   char buf[128];
-  int res = ::gethostname(buf, sizeof(buf));
+  inet_ntop(AF_INET, &name.sin_addr, buf, sizeof(buf));
   return std::string(buf);
 }
 
@@ -28,7 +39,6 @@ bool URServer::bind()
 {
   std::string empty;
   bool res = TCPSocket::setup(empty, port_);
-  state_ = TCPSocket::getState();
   
   if(!res)
     return false;
@@ -41,7 +51,7 @@ bool URServer::bind()
 
 bool URServer::accept()
 {
-  if(state_ != SocketState::Connected || client_.getSocketFD() > 0)
+  if(TCPSocket::getState() != SocketState::Connected || client_.getSocketFD() > 0)
     return false;
 
   struct sockaddr addr; 
@@ -54,6 +64,14 @@ bool URServer::accept()
   return client_.setSocketFD(client_fd);
 }
 
+void URServer::disconnectClient()
+{
+  if(client_.getState() != SocketState::Connected)
+    return;
+  
+  client_.close();
+}
+
 bool URServer::write(const uint8_t* buf, size_t buf_len, size_t &written)
 {
   return client_.write(buf, buf_len, written);