Big code dump

src/ur/commander.cpp

@@ -9,6 +9,10 @@ bool URCommander::write(std::string& s)
     return res > 0 && static_cast<size_t>(res) == len;
 }
 
+bool URCommander::uploadProg(std::string &s)
+{
+  return write(s);
+}
 
 bool URCommander::speedj(std::array<double, 6> &speeds, double acceleration)
 {
@@ -52,11 +56,17 @@ bool URCommander::setToolVoltage(uint8_t voltage)
 }
 
 
-bool URCommander::setFlag(bool value)
+bool URCommander::setFlag(uint8_t pin, bool value)
 {
-
+    std::ostringstream out;
+    out << "set_flag(" << (int)pin << "," << (value ? "True" : "False") << ")\n";
+    std::string s(out.str());
+    return write(s);
 }
 bool URCommander::setPayload(double value)
 {
-
+    std::ostringstream out;
+    out << "set_payload(" << std::fixed << std::setprecision(4) << value << ")\n";
+    std::string s(out.str());
+    return write(s);
 }

src/ur/robot_mode.cpp

@@ -8,6 +8,9 @@ bool SharedRobotModeData::parseWith(BinParser& bp)
   bp.parse(real_robot_enabled);
   bp.parse(robot_power_on);
   bp.parse(emergency_stopped);
+  bp.parse(protective_stopped);
+  bp.parse(program_running);
+  bp.parse(program_paused);
   return true;
 }
 
@@ -18,9 +21,6 @@ bool RobotModeData_V1_X::parseWith(BinParser& bp)
 
   SharedRobotModeData::parseWith(bp);
 
-  bp.parse(security_stopped);
-  bp.parse(program_running);
-  bp.parse(program_paused);
   bp.parse(robot_mode);
   bp.parse(speed_fraction);
 
@@ -34,9 +34,6 @@ bool RobotModeData_V3_0__1::parseWith(BinParser& bp)
 
   SharedRobotModeData::parseWith(bp);
 
-  bp.parse(protective_stopped);
-  bp.parse(program_running);
-  bp.parse(program_paused);
   bp.parse(robot_mode);
   bp.parse(control_mode);
   bp.parse(target_speed_fraction);

src/ur/server.cpp

@@ -0,0 +1,51 @@
+#include <cstring>
+#include <netinet/tcp.h>
+#include <unistd.h>
+#include "ur_modern_driver/log.h"
+#include "ur_modern_driver/ur/server.h"
+
+URServer::URServer(int port)
+{
+  std::string service = std::to_string(port);
+  struct addrinfo hints, *result;
+  std::memset(&hints, 0, sizeof(hints));
+
+  hints.ai_family = AF_UNSPEC;
+  hints.ai_socktype = SOCK_STREAM;
+  hints.ai_flags = AI_PASSIVE;
+
+  if (getaddrinfo(nullptr, service.c_str(), &hints, &result) != 0)
+  {
+    LOG_ERROR("Failed to setup recieving server");
+    return;
+  }
+
+    // loop through the list of addresses untill we find one that's connectable
+  for (struct addrinfo* p = result; p != nullptr; p = p->ai_next)
+  {
+    socket_fd_ = socket(p->ai_family, p->ai_socktype, p->ai_protocol);
+
+    if (socket_fd_ == -1)  // socket error?
+      continue;
+
+    if (bind(socket_fd_, p->ai_addr, p->ai_addrlen) != 0)
+      continue;
+
+    // disable Nagle's algorithm to ensure we sent packets as fast as possible
+    int flag = 1;
+    setsockopt(socket_fd_, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag));
+    LOG_INFO("Server awaiting connection");
+    return;
+  }
+  
+  LOG_ERROR("Failed to setup recieving server");
+  std::exit(EXIT_FAILURE);
+}
+
+URStream URServer::accept()
+{
+  struct sockaddr addr; 
+  socklen_t addr_len;
+  int client_fd = ::accept(socket_fd_, &addr, &addr_len);
+  return URStream(client_fd);
+}