Big code dump

src/ros/action_server.cpp

@@ -0,0 +1,231 @@
+#include <cmath>
+#include "ur_modern_driver/ros/action_server.h"
+
+ActionServer::ActionServer(TrajectoryFollower& follower, std::vector<std::string>& joint_names, double max_velocity)
+  : as_(
+      nh_, 
+      "follow_joint_trajectory",
+      boost::bind(&ActionServer::onGoal, this, _1),
+      boost::bind(&ActionServer::onCancel, this, _1), 
+      false
+    )
+  , joint_names_(joint_names)
+  , joint_set_(joint_names.begin(), joint_names.end())
+  , max_velocity_(max_velocity)
+  , state_(RobotState::Error)
+  , follower_(follower)
+{
+  
+}
+
+void ActionServer::onRobotStateChange(RobotState state)
+{
+  state_ = state;
+}
+
+void ActionServer::onGoal(GoalHandle gh)
+{
+  Result res;
+  res.error_code = -100;
+
+  if(!validate(gh, res) || !try_execute(gh, res))
+    gh.setRejected(res, res.error_string);
+}
+
+void ActionServer::onCancel(GoalHandle gh)
+{
+
+}
+
+bool ActionServer::validate(GoalHandle& gh, Result& res)
+{ 
+  return !validateState(gh, res) || !validateJoints(gh, res) || !validateTrajectory(gh, res);
+}
+
+bool ActionServer::validateState(GoalHandle& gh, Result& res)
+{
+  switch(state_)
+  {
+    case RobotState::EmergencyStopped:
+      res.error_string = "Robot is emergency stopped";
+      return false;
+      
+    case RobotState::ProtectiveStopped:
+      res.error_string = "Robot is protective stopped";
+      return false;
+      
+    case RobotState::Error:
+      res.error_string = "Robot is not ready, check robot_mode";
+      return false;
+
+    case RobotState::Running:
+      return true;
+
+    default:
+      res.error_string = "Undefined state";
+      return false;
+  }
+}
+
+bool ActionServer::validateJoints(GoalHandle& gh, Result& res)
+{
+  auto goal = gh.getGoal();
+  auto const& joints = goal->trajectory.joint_names;
+  std::set<std::string> goal_joints(joints.begin(), joints.end());
+
+  if(goal_joints == joint_set_)
+    return true;
+
+  res.error_code = Result::INVALID_JOINTS;
+  res.error_string = "Invalid joint names for goal";
+  return false;
+}
+
+bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res)
+{
+  auto goal = gh.getGoal();
+  res.error_code = Result::INVALID_GOAL;
+
+  for(auto const& point : goal->trajectory.points)
+  {
+    if(point.velocities.size() != joint_names_.size())
+    {
+      res.error_code = Result::INVALID_GOAL;
+      res.error_string = "Received a goal with an invalid number of velocities";
+      return false;
+    }
+    
+    if(point.positions.size() != joint_names_.size())
+    {
+      res.error_code = Result::INVALID_GOAL;
+      res.error_string = "Received a goal with an invalid number of positions";
+      return false;
+    }
+    
+    for(auto const& velocity : point.velocities)
+    {
+      if(!std::isfinite(velocity))
+      {
+        res.error_string = "Received a goal with infinities or NaNs in velocity";
+        return false;
+      }
+      if(std::fabs(velocity) > max_velocity_)
+      {
+        res.error_string = "Received a goal with velocities that are higher than " + std::to_string(max_velocity_);
+        return false;
+      }
+    }
+    for(auto const& position : point.positions)
+    {
+      if(!std::isfinite(position))
+      {
+        res.error_string = "Received a goal with infinities or NaNs in positions";
+        return false;
+      }
+    }
+  }
+
+  return true;
+}
+
+bool ActionServer::try_execute(GoalHandle& gh, Result& res)
+{
+  if(!running_)
+  {
+    res.error_string = "Internal error";
+    return false;
+  }
+  if(!tj_mutex_.try_lock())
+  {
+    has_goal_ = false;
+    //stop_trajectory();
+    res.error_string = "Received another trajectory";
+    curr_gh_.setAborted(res, res.error_string);
+    tj_mutex_.lock();
+  }
+  //locked here
+  curr_gh_ = gh;
+  has_goal_ = true;
+  tj_mutex_.unlock();
+  tj_cv_.notify_one();
+}
+
+inline double ActionServer::interp_cubic(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel)
+{
+  using std::pow;
+  double a = p0_pos;
+  double b = p0_vel;
+  double c = (-3 * a + 3 * p1_pos - 2 * T * b - T * p1_vel) / pow(T, 2);
+  double d = (2 * a - 2 * p1_pos + T * b + T * p1_vel) / pow(T, 3);
+  return a + b * t + c * pow(t, 2) + d * pow(t, 3);
+}
+
+std::vector<size_t> ActionServer::reorderMap(std::vector<std::string> goal_joints)
+{
+  std::vector<size_t> indecies;
+  for(auto const& aj : joint_names_)
+  {
+    size_t j = 0;
+    for(auto const& gj : goal_joints)
+    {
+      if(aj == gj)
+        break;
+      j++;
+    }
+    indecies.push_back(j);
+  }
+  return indecies;
+}
+
+void ActionServer::trajectoryThread()
+{
+  while(running_)
+  {
+    std::unique_lock<std::mutex> lk(tj_mutex_);
+    if(!tj_cv_.wait_for(lk, std::chrono::milliseconds(100), [&]{return running_ && has_goal_;}))
+      continue;
+    
+    auto g = curr_gh_.getGoal();
+    auto const& traj = g->trajectory;
+    auto const& points = traj.points;
+    size_t len = points.size();
+    auto const& last_point = points[points.size() - 1];
+    double end_time = last_point.time_from_start.toSec();
+    
+    auto mapping = reorderMap(traj.joint_names);
+    std::chrono::high_resolution_clock::time_point t0, t;
+    t = t0 = std::chrono::high_resolution_clock::now();
+
+    size_t i = 0;
+    while(end_time >= toSec(t - t0) && has_goal_)
+    {
+      while(points[i].time_from_start.toSec() <= toSec(t - t0) && i < len)
+        i++;
+      
+      auto const& pp = points[i-1];
+      auto const& p = points[i];
+
+      auto pp_t = pp.time_from_start.toSec();
+      auto p_t =p.time_from_start.toSec();
+
+      std::array<double, 6> pos;
+      for(size_t j = 0; j < pos.size(); j++)
+      {
+        pos[i] = interp_cubic(
+          toSec(t - t0) - pp_t,
+          p_t - pp_t,
+          pp.positions[j],
+          p.positions[j],
+          pp.velocities[j],
+          p.velocities[j]
+        );
+      }
+
+      follower_.execute(pos);
+      //std::this_thread::sleep_for(std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.))); 
+      t = std::chrono::high_resolution_clock::now();
+    }
+
+    has_goal_ = false;
+  }
+}

src/ros/controller.cpp

@@ -0,0 +1,84 @@
+#include "ur_modern_driver/ros/controller.h"
+
+ROSController::ROSController(URCommander &commander, std::vector<std::string> &joint_names, double max_vel_change)
+  : controller_(this, nh_)
+  , joint_interface_(joint_names)
+  , wrench_interface_()
+  , position_interface_(commander, joint_interface_, joint_names)
+  , velocity_interface_(commander, joint_interface_, joint_names, max_vel_change)
+{
+  registerInterface(&joint_interface_);
+  registerInterface(&wrench_interface_);
+  registerControllereInterface(&position_interface_);
+  registerControllereInterface(&velocity_interface_);
+}
+
+void ROSController::setupConsumer()
+{
+  lastUpdate_ = ros::Time::now();
+}
+
+void ROSController::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list, const std::list<hardware_interface::ControllerInfo>& stop_list)
+{
+  if (active_interface_ != nullptr && stop_list.size() > 0)
+  {
+    LOG_INFO("Stopping active interface");
+    active_interface_->stop();
+    active_interface_ = nullptr;
+  }
+
+  for (auto const& ci : start_list)
+  {
+    auto ait = available_interfaces_.find(ci.hardware_interface);
+
+    if (ait == available_interfaces_.end())
+      continue;
+
+    auto new_interface = ait->second;
+
+    LOG_INFO("Starting %s", ci.hardware_interface.c_str());
+    active_interface_ = new_interface;
+    new_interface->start();
+
+    return;
+  }
+
+  LOG_WARN("Failed to start interface!");
+}
+
+bool ROSController::write()
+{
+  if (active_interface_ == nullptr)
+    return true;
+
+  return active_interface_->write();
+}
+
+void ROSController::read(RTShared& packet)
+{
+  joint_interface_.update(packet);
+  wrench_interface_.update(packet);
+}
+
+
+bool ROSController::update(RTShared& state)
+{
+  auto time = ros::Time::now();
+  auto diff = time - lastUpdate_;
+  lastUpdate_ = time;
+
+  read(state);
+  controller_.update(time, diff);
+
+  //emergency stop and such should not kill the pipeline
+  //but still prevent writes
+  if(!service_enabled_)
+    return true;
+
+  return write();
+}
+
+void ROSController::onRobotStateChange(RobotState state)
+{
+  service_enabled_ = (state == RobotState::Running);
+}

src/ros/hardware_interface.cpp

@@ -0,0 +1,74 @@
+#include "ur_modern_driver/ros/hardware_interface.h"
+
+JointInterface::JointInterface(std::vector<std::string> &joint_names)
+{
+    for (size_t i = 0; i < 6; i++)
+    {
+        registerHandle(hardware_interface::JointStateHandle(joint_names[i], &positions_[i], &velocities_[i], &efforts_[i]));
+    }
+}
+
+void JointInterface::update(RTShared &packet)
+{
+    positions_ = packet.q_actual;
+    velocities_ = packet.qd_actual;
+    efforts_ = packet.i_actual;
+}
+
+WrenchInterface::WrenchInterface()
+{
+    registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin() + 3));
+}
+
+void WrenchInterface::update(RTShared &packet)
+{
+    tcp_ = packet.tcp_force;
+}
+
+VelocityInterface::VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector<std::string> &joint_names, double max_vel_change)
+    : commander_(commander), max_vel_change_(max_vel_change)
+{
+    for (size_t i = 0; i < 6; i++)
+    {
+        registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &velocity_cmd_[i]));
+    }
+}
+
+bool VelocityInterface::write()
+{
+    for (size_t i = 0; i < 6; i++)
+    {
+        double prev = prev_velocity_cmd_[i];
+        double lo = prev - max_vel_change_;
+        double hi = prev + max_vel_change_;
+        // clamp value to ±max_vel_change
+        prev_velocity_cmd_[i] = std::max(lo, std::min(velocity_cmd_[i], hi));
+    }
+
+    return commander_.speedj(prev_velocity_cmd_, max_vel_change_);
+}
+
+
+PositionInterface::  PositionInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector<std::string> &joint_names)
+    : commander_(commander)
+{
+    for (size_t i = 0; i < 6; i++)
+    {
+        registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &position_cmd_[i]));
+    }
+}
+
+bool PositionInterface::write()
+{
+
+}
+
+void PositionInterface::start()
+{
+
+}
+
+void PositionInterface::stop()
+{
+
+}

src/ros/robot_hardware.cpp

@@ -28,45 +28,3 @@ bool RobotHardware::canSwitch(const std::list<ControllerInfo>& start_list,
   return true;
 }
 */
-
-void RobotHardware::doSwitch(const std::list<ControllerInfo>& start_list, const std::list<ControllerInfo>& stop_list)
-{
-  if (active_interface_ != nullptr && stop_list.size() > 0)
-  {
-    LOG_INFO("Stopping active interface");
-    active_interface_->stop();
-    active_interface_ = nullptr;
-  }
-
-  for (auto const& ci : start_list)
-  {
-    auto ait = available_interfaces_.find(ci.hardware_interface);
-
-    if (ait == available_interfaces_.end())
-      continue;
-
-    auto new_interface = ait->second;
-
-    LOG_INFO("Starting %s", ci.hardware_interface.c_str());
-    active_interface_ = new_interface;
-    new_interface->start();
-
-    return;
-  }
-
-  LOG_WARN("Failed to start interface!");
-}
-
-void RobotHardware::write()
-{
-  if (active_interface_ == nullptr)
-    return;
-
-  active_interface_->write();
-}
-
-void RobotHardware::read(RTShared& packet)
-{
-  joint_interface_.update(packet);
-  wrench_interface_.update(packet);
-}

src/ros/service_stopper.cpp

@@ -0,0 +1,47 @@
+#include "ur_modern_driver/ros/service_stopper.h"
+
+ServiceStopper::ServiceStopper(std::vector<Service*> services)
+  : enable_service_(nh_.advertiseService("ur_driver/robot_enable", &ServiceStopper::enableCallback, this))
+  , services_(services)
+  , last_state_(RobotState::Error)
+{
+  //enable_all();
+}
+
+
+bool ServiceStopper::enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp)
+{
+  notify_all(RobotState::Running);
+  return true;
+}
+
+void ServiceStopper::notify_all(RobotState state)
+{
+  if(last_state_ == state)
+    return;
+  
+  for(auto const service : services_)
+  {
+    service->onRobotStateChange(state);
+  }
+
+  last_state_ = state;
+}
+
+bool ServiceStopper::handle(SharedRobotModeData& data, bool error)
+{
+  if(data.emergency_stopped)
+  {
+    notify_all(RobotState::EmergencyStopped);
+  }
+  else if(data.protective_stopped)
+  {
+    notify_all(RobotState::ProtectiveStopped);
+  }
+  else if(error)
+  {
+    notify_all(RobotState::Error);
+  }
+
+  return true;
+}

src/ros/trajectory_follower.cpp

@@ -0,0 +1,137 @@
+#include <endian.h>
+#include "ur_modern_driver/ros/trajectory_follower.h"
+
+
+TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3)
+  : running_(false)
+  , commander_(commander)
+  , server_(reverse_port)
+  , program_(buildProgram(version_3))
+{
+}
+static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}");
+static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}");
+static const std::string POSITION_PROGRAM = R"(
+def driverProg():
+    MULT_jointstate = {{JOINT_STATE_REPLACE}}
+
+    SERVO_IDLE = 0
+    SERVO_RUNNING = 1
+    cmd_servo_state = SERVO_IDLE
+    cmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+    def set_servo_setpoint(q):
+        enter_critical
+        cmd_servo_state = SERVO_RUNNING
+        cmd_servo_q = q
+        exit_critical
+    end
+    thread servoThread():
+        state = SERVO_IDLE
+        while True:
+            enter_critical
+            q = cmd_servo_q
+            do_brake = False
+            if (state == SERVO_RUNNING) and (cmd_servo_state == SERVO_IDLE):
+                do_brake = True
+            end
+            state = cmd_servo_state
+            cmd_servo_state = SERVO_IDLE
+            exit_critical
+            if do_brake:
+                stopj(1.0)
+                sync()
+            elif state == SERVO_RUNNING:
+                servoj(q, {{SERVO_J_REPLACE}})
+            else:
+                sync()
+            end
+        end
+    end
+
+  thread_servo = run servoThread()
+  keepalive = 1
+  while keepalive > 0:
+      params_mult = socket_read_binary_integer(6+1)
+      if params_mult[0] > 0:
+          q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate]
+          keepalive = params_mult[7]
+          set_servo_setpoint(q)
+      end
+  end
+  sleep(.1)
+  socket_close()
+  kill thread_servo
+end
+)";
+std::string TrajectoryFollower::buildProgram(bool version_3)
+{
+  std::string res(POSITION_PROGRAM);
+  size_t js_idx = POSITION_PROGRAM.find(JOINT_STATE_REPLACE);
+  size_t sj_idx = POSITION_PROGRAM.find(SERVO_J_REPLACE);
+
+
+  std::ostringstream out;
+  out << "t=" << std::fixed << std::setprecision(4) << servoj_time_;
+  
+  if(version_3)
+    out << ", lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_;
+
+  res.replace(js_idx, JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_));
+  res.replace(sj_idx, SERVO_J_REPLACE.length(), out.str());
+  return res;
+}
+
+bool TrajectoryFollower::start()
+{
+  if(running_)
+    return true; //not sure
+
+  //TODO
+  std::string prog(""); // buildProg();
+  if(!commander_.uploadProg(prog))
+    return false;
+
+  stream_ = std::move(server_.accept()); //todo: pointer instead?
+  return (running_ = true);
+}
+
+bool TrajectoryFollower::execute(std::array<double, 6> &positions, bool keep_alive)
+{
+  if(!running_)
+    return false;
+
+  last_positions_ = positions;
+
+  uint8_t buf[sizeof(uint32_t)*7];
+  uint8_t *idx = buf;
+  
+  for(auto const& pos : positions)
+  {
+    int32_t val = static_cast<int32_t>(pos * MULT_JOINTSTATE_);
+    val = htobe32(val);
+    idx += append(idx, val);
+  }
+
+  int32_t val = htobe32(static_cast<int32_t>(keep_alive));
+  append(idx, val);
+
+  ssize_t res = stream_.send(buf, sizeof(buf));
+  return res > 0 && res == sizeof(buf);
+}
+
+bool TrajectoryFollower::execute(std::array<double, 6> &positions)
+{
+  return execute(positions, true);
+}
+
+void TrajectoryFollower::stop()
+{
+  if(!running_)
+    return;
+
+  std::array<double, 6> empty;
+  execute(empty, false);
+
+  stream_.disconnect();
+  running_ = false;
+}

src/ros_main.cpp

@@ -8,8 +8,9 @@
 #include "ur_modern_driver/pipeline.h"
 #include "ur_modern_driver/ros/io_service.h"
 #include "ur_modern_driver/ros/mb_publisher.h"
-#include "ur_modern_driver/ros/ros_controller.h"
+#include "ur_modern_driver/ros/controller.h"
 #include "ur_modern_driver/ros/rt_publisher.h"
+#include "ur_modern_driver/ros/service_stopper.h"
 #include "ur_modern_driver/ur/commander.h"
 #include "ur_modern_driver/ur/factory.h"
 #include "ur_modern_driver/ur/messages.h"
@@ -20,8 +21,8 @@
 
 static const std::string IP_ADDR_ARG("~robot_ip_address");
 static const std::string REVERSE_PORT_ARG("~reverse_port");
-static const std::string SIM_TIME_ARG("~use_sim_time");
 static const std::string ROS_CONTROL_ARG("~use_ros_control");
+static const std::string MAX_VEL_CHANGE_ARG("~max_vel_change");
 static const std::string PREFIX_ARG("~prefix");
 static const std::string BASE_FRAME_ARG("~base_frame");
 static const std::string TOOL_FRAME_ARG("~tool_frame");
@@ -40,12 +41,12 @@ public:
   std::vector<std::string> joint_names;
   double max_acceleration;
   double max_velocity;
+  double max_vel_change;
   int32_t reverse_port;
-  bool use_sim_time;
   bool use_ros_control;
 };
 
-bool parse_args(ProgArgs& args)
+bool parse_args(ProgArgs &args)
 {
   if (!ros::param::get(IP_ADDR_ARG, args.host))
   {
@@ -53,7 +54,7 @@ bool parse_args(ProgArgs& args)
     return false;
   }
   ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001));
-  ros::param::param(SIM_TIME_ARG, args.use_sim_time, false);
+  ros::param::param(MAX_VEL_CHANGE_ARG, args.max_vel_change, 15.0); // rad/s
   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");
@@ -64,7 +65,7 @@ bool parse_args(ProgArgs& args)
 
 #include "ur_modern_driver/event_counter.h"
 
-int main(int argc, char** argv)
+int main(int argc, char **argv)
 {
   ros::init(argc, argv, "ur_driver");
 
@@ -75,18 +76,24 @@ int main(int argc, char** argv)
   }
 
   URFactory factory(args.host);
+
+  vector<Service*> services;
+
   // RT packets
   auto rt_parser = factory.getRTParser();
   URStream rt_stream(args.host, UR_RT_PORT);
   URProducer<RTPacket> rt_prod(rt_stream, *rt_parser);
   RTPublisher rt_pub(args.prefix, args.base_frame, args.tool_frame, args.use_ros_control);
   URCommander rt_commander(rt_stream);
-  vector<IConsumer<RTPacket>*> rt_vec{ &rt_pub };
+  vector<IConsumer<RTPacket> *> rt_vec{&rt_pub};
 
+  ROSController *controller(nullptr);
   if (args.use_ros_control)
   {
     LOG_INFO("ROS control enabled");
-    rt_vec.push_back(new ROSController(rt_commander, args.joint_names));
+    controller = new ROSController(rt_commander, args.joint_names, args.max_vel_change);
+    rt_vec.push_back(controller);
+    services.push_back(controller);
   }
 
   MultiConsumer<RTPacket> rt_cons(rt_vec);
@@ -97,7 +104,10 @@ int main(int argc, char** argv)
   URStream state_stream(args.host, UR_SECONDARY_PORT);
   URProducer<StatePacket> state_prod(state_stream, *state_parser);
   MBPublisher state_pub;
-  vector<IConsumer<StatePacket>*> state_vec{ &state_pub };
+  
+  ServiceStopper service_stopper(services);
+
+  vector<IConsumer<StatePacket> *> state_vec{&state_pub, &service_stopper};
   MultiConsumer<StatePacket> state_cons(state_vec);
   Pipeline<StatePacket> state_pl(state_prod, state_cons);
 
@@ -107,12 +117,19 @@ int main(int argc, char** argv)
   state_pl.run();
 
   URCommander state_commander(state_stream);
-  IOService service(state_commander);
+  IOService io_service(state_commander);
 
   ros::spin();
 
+  LOG_INFO("ROS stopping, shutting down pipelines");
+
   rt_pl.stop();
   state_pl.stop();
+  
+  if(controller)
+    delete controller;
+
+  LOG_INFO("Pipelines shutdown complete");
 
   return EXIT_SUCCESS;
 }

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