Big code dump

CMakeLists.txt

@@ -158,10 +158,15 @@ target_link_libraries(ur_hardware_interface
 
 ## Declare a C++ executable
 set(${PROJECT_NAME}_SOURCES
-    src/ros/rt_publisher.cpp
+    src/ros/action_server.cpp
+    src/ros/controller.cpp
+    src/ros/hardware_interface.cpp
     src/ros/mb_publisher.cpp
-    src/ros/robot_hardware.cpp
+    src/ros/rt_publisher.cpp
+    src/ros/service_stopper.cpp
+    src/ros/trajectory_follower.cpp
     src/ur/stream.cpp
+    src/ur/server.cpp
     src/ur/commander.cpp
     src/ur/robot_mode.cpp
     src/ur/master_board.cpp

include/ur_modern_driver/ros/action_server.h

@@ -0,0 +1,69 @@
+#pragma once
+
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <mutex>
+#include <set>
+#include <ros/ros.h>
+#include <actionlib/server/action_server.h>
+#include <actionlib/server/server_goal_handle.h>
+#include <control_msgs/FollowJointTrajectoryAction.h>
+#include "ur_modern_driver/log.h"
+#include "ur_modern_driver/ur/consumer.h"
+#include "ur_modern_driver/ur/master_board.h"
+#include "ur_modern_driver/ur/state.h"
+#include "ur_modern_driver/ros/service_stopper.h"
+#include "ur_modern_driver/ros/trajectory_follower.h"
+
+
+class ActionServer : public URRTPacketConsumer, public Service
+{
+private:
+  typedef control_msgs::FollowJointTrajectoryAction Action;
+  typedef control_msgs::FollowJointTrajectoryResult Result;
+  typedef actionlib::ServerGoalHandle<Action> GoalHandle;
+  typedef actionlib::ActionServer<Action> Server;
+  
+  ros::NodeHandle nh_;
+  Server as_;
+
+  std::vector<std::string> joint_names_;
+  std::set<std::string> joint_set_;
+  double max_velocity_;
+  RobotState state_;
+
+  
+  GoalHandle curr_gh_;
+  std::atomic<bool> has_goal_, running_;
+  std::mutex tj_mutex_;
+  std::condition_variable tj_cv_;
+
+  TrajectoryFollower& follower_;
+
+  void onGoal(GoalHandle gh);
+  void onCancel(GoalHandle gh);
+
+  bool validate(GoalHandle& gh, Result& res);
+  bool validateState(GoalHandle& gh, Result& res);  
+  bool validateJoints(GoalHandle& gh, Result& res);
+  bool validateTrajectory(GoalHandle& gh, Result& res);
+
+  bool try_execute(GoalHandle& gh, Result& res);
+
+  std::vector<size_t> reorderMap(std::vector<std::string> goal_joints);
+  double interp_cubic(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel);
+
+  void trajectoryThread();
+
+  template <typename U>
+  double toSec(U const& u)
+  {
+      return std::chrono::duration_cast<std::chrono::duration<double>>(u).count();
+  }
+
+public:
+  ActionServer(TrajectoryFollower& follower, std::vector<std::string>& joint_names, double max_velocity);
+
+  virtual void onRobotStateChange(RobotState state);
+};

include/ur_modern_driver/ros/controller.h

@@ -0,0 +1,80 @@
+#pragma once
+#include <atomic>
+#include <ros/ros.h>
+#include <controller_manager/controller_manager.h>
+#include <hardware_interface/force_torque_sensor_interface.h>
+#include <hardware_interface/internal/demangle_symbol.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/joint_state_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include "ur_modern_driver/log.h"
+#include "ur_modern_driver/ur/commander.h"
+#include "ur_modern_driver/ur/consumer.h"
+#include "ur_modern_driver/ur/rt_state.h"
+#include "ur_modern_driver/ros/hardware_interface.h"
+#include "ur_modern_driver/ros/service_stopper.h"
+
+class ROSController : private hardware_interface::RobotHW, public URRTPacketConsumer, public Service
+{
+private:
+  ros::NodeHandle nh_;
+  ros::Time lastUpdate_;
+  controller_manager::ControllerManager controller_;
+
+  // state interfaces
+  JointInterface joint_interface_;
+  WrenchInterface wrench_interface_;
+  // controller interfaces
+  PositionInterface position_interface_;
+  VelocityInterface velocity_interface_;
+
+  // currently activated controller
+  HardwareInterface* active_interface_;
+  // map of switchable controllers
+  std::map<std::string, HardwareInterface*> available_interfaces_;
+
+  std::atomic<bool> service_enabled_;
+
+  // helper functions to map interfaces
+  template <typename T>
+  void registerInterface(T* interface)
+  {
+    RobotHW::registerInterface<typename T::parent_type>(interface);
+  }
+  template <typename T>
+  void registerControllereInterface(T* interface)
+  {
+    registerInterface(interface);
+    available_interfaces_[hardware_interface::internal::demangledTypeName<typename T::parent_type>()] = interface;
+  }
+
+  void read(RTShared& state);
+  bool update(RTShared& state);
+  bool write();
+
+public:
+  ROSController(URCommander& commander,  std::vector<std::string>& joint_names, double max_vel_change);
+  virtual ~ROSController() { }
+  // from RobotHW
+  void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list, const std::list<hardware_interface::ControllerInfo>& stop_list);
+  // from URRTPacketConsumer
+  virtual void setupConsumer();
+  virtual bool consume(RTState_V1_6__7& state)
+  {
+    return update(state);
+  }
+  virtual bool consume(RTState_V1_8& state)
+  {
+    return update(state);
+  }
+  virtual bool consume(RTState_V3_0__1& state)
+  {
+    return update(state);
+  }
+  virtual bool consume(RTState_V3_2__3& state)
+  {
+    return update(state);
+  }
+
+  virtual void onRobotStateChange(RobotState state);
+};

include/ur_modern_driver/ros/hardware_interface.h

@@ -10,39 +10,22 @@
 class HardwareInterface
 {
 public:
-  virtual void write() = 0;
-  virtual void start()
-  {
-  }
-  virtual void stop()
-  {
-  }
+  virtual bool write() = 0;
+  virtual void start() {}
+  virtual void stop() {}
 };
 
 using hardware_interface::JointHandle;
-using hardware_interface::JointStateHandle;
-using hardware_interface::JointStateInterface;
 
-class JointInterface : public JointStateInterface
+class JointInterface : public hardware_interface::JointStateInterface
 {
 private:
   std::array<double, 6> velocities_, positions_, efforts_;
 
 public:
-  JointInterface(std::vector<std::string>& joint_names)
-  {
-    for (size_t i = 0; i < 6; i++)
-    {
-      registerHandle(JointStateHandle(joint_names[i], &positions_[i], &velocities_[i], &efforts_[i]));
-    }
-  }
-
-  void update(RTShared& packet)
-  {
-    positions_ = packet.q_actual;
-    velocities_ = packet.qd_actual;
-    efforts_ = packet.i_actual;
-  }
+  JointInterface(std::vector<std::string> &joint_names);
+  void update(RTShared &packet);
+  typedef hardware_interface::JointStateInterface parent_type;
 };
 
 class WrenchInterface : public hardware_interface::ForceTorqueSensorInterface
@@ -50,124 +33,35 @@ class WrenchInterface : public hardware_interface::ForceTorqueSensorInterface
   std::array<double, 6> tcp_;
 
 public:
-  WrenchInterface()
-  {
-    registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin() + 3));
-  }
-
-  void update(RTShared& packet)
-  {
-    tcp_ = packet.tcp_force;
-  }
+  WrenchInterface();
+  void update(RTShared &packet);
+  typedef hardware_interface::ForceTorqueSensorInterface parent_type;
 };
 
 class VelocityInterface : public HardwareInterface, public hardware_interface::VelocityJointInterface
 {
 private:
-  URCommander& commander_;
+  URCommander &commander_;
   std::array<double, 6> velocity_cmd_, prev_velocity_cmd_;
-  double max_vel_change_ = 0.12 / 125;
+  double max_vel_change_;
 
 public:
-  VelocityInterface(URCommander& commander, 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]), &velocity_cmd_[i]));
-    }
-  }
-
-  virtual void 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));
-    }
-
-    // times 125???
-    commander_.speedj(prev_velocity_cmd_, max_vel_change_ * 125);
-  }
-
+  VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector<std::string> &joint_names, double max_vel_change);
+  virtual bool write();
   typedef hardware_interface::VelocityJointInterface parent_type;
 };
 
-static const std::string POSITION_PROGRAM = R"(
-def driverProg():
-    MULT_jointstate = XXXXX
-
-    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, YYYYYYYY)
-            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
-)";
-
 class PositionInterface : public HardwareInterface, public hardware_interface::PositionJointInterface
 {
 private:
-  URCommander& commander_;
+  URCommander &commander_;
   std::array<double, 6> position_cmd_;
 
 public:
-  PositionInterface(URCommander& commander, 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]));
-    }
-  }
-
-  virtual void write()
-  {
-  }
+  PositionInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector<std::string> &joint_names);
+  virtual bool write();
+  virtual void start();
+  virtual void stop();
 
   typedef hardware_interface::PositionJointInterface parent_type;
 };

include/ur_modern_driver/ros/io_service.h

@@ -35,18 +35,18 @@ private:
         res = commander_.setToolVoltage(static_cast<uint8_t>(req.state));
       break;
       case ur_msgs::SetIO::Request::FUN_SET_FLAG:
-        res = commander_.setFlag(flag);
+        res = commander_.setFlag(req.pin, flag);
       break;
     }
 
-    return resp.success = res;
+    return (resp.success = res);
   }
 
   bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& resp)
   {
     LOG_INFO("setPayload called");
     //TODO check min and max payload?
-    return resp.success = commander_.setPayload(req.payload);
+    return (resp.success = commander_.setPayload(req.payload));
   }
 
 

include/ur_modern_driver/ros/robot_hardware.h

@@ -29,11 +29,11 @@ private:
   }
 
 public:
-  RobotHardware(URCommander& commander, std::vector<std::string>& joint_names)
+  RobotHardware(URCommander& commander, std::vector<std::string>& joint_names, double max_vel_change)
     : joint_interface_(joint_names)
     , wrench_interface_()
     , position_interface_(commander, joint_interface_, joint_names)
-    , velocity_interface_(commander, joint_interface_, joint_names)
+    , velocity_interface_(commander, joint_interface_, joint_names, max_vel_change)
   {
     registerInterface<hardware_interface::JointStateInterface>(&joint_interface_);
     registerInterface<hardware_interface::ForceTorqueSensorInterface>(&wrench_interface_);

include/ur_modern_driver/ros/ros_controller.h

@@ -1,60 +0,0 @@
-#pragma once
-#include <ros/ros.h>
-#include <controller_manager/controller_manager.h>
-#include "ur_modern_driver/log.h"
-#include "ur_modern_driver/pipeline.h"
-#include "ur_modern_driver/ur/rt_state.h"
-#include "ur_modern_driver/ur/commander.h"
-#include "ur_modern_driver/ros/robot_hardware.h"
-
-class ROSController : public URRTPacketConsumer
-{
-private:
-  ros::NodeHandle nh_;
-  ros::Time lastUpdate_;
-  RobotHardware robot_;
-  controller_manager::ControllerManager controller_;
-
-public:
-  ROSController(URCommander& commander,  std::vector<std::string>& joint_names)
-    : robot_(commander, joint_names)
-    , controller_(&robot_, nh_)
-  {
-  }
-
-  virtual void setupConsumer()
-  {
-    lastUpdate_ = ros::Time::now();
-  }
-
-  bool handle(RTShared& state)
-  {
-    auto time = ros::Time::now();
-    auto diff = time - lastUpdate_;
-    lastUpdate_ = time;
-
-    robot_.read(state);
-    controller_.update(time, diff);
-    robot_.write();
-    //todo: return result of write
-    return true;
-  }
-
-
-  virtual bool consume(RTState_V1_6__7& state)
-  {
-    return handle(state);
-  }
-  virtual bool consume(RTState_V1_8& state)
-  {
-    return handle(state);
-  }
-  virtual bool consume(RTState_V3_0__1& state)
-  {
-    return handle(state);
-  }
-  virtual bool consume(RTState_V3_2__3& state)
-  {
-    return handle(state);
-  }
-};

include/ur_modern_driver/ros/service_stopper.h

@@ -0,0 +1,52 @@
+#pragma once
+#include <ros/ros.h>
+#include <std_srvs/Empty.h>
+#include "ur_modern_driver/log.h"
+#include "ur_modern_driver/ur/consumer.h"
+
+enum class RobotState 
+{
+  Running,
+  Error,
+  EmergencyStopped,
+  ProtectiveStopped
+};
+
+class Service
+{
+public:
+  virtual void onRobotStateChange(RobotState state) = 0;
+};
+
+class ServiceStopper : public URStatePacketConsumer {
+private:
+  ros::NodeHandle nh_;
+  ros::ServiceServer enable_service_;
+  std::vector<Service*> services_;
+  RobotState last_state_;
+  
+  void notify_all(RobotState state);
+  bool handle(SharedRobotModeData &data, bool error);
+  bool enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp);
+
+public:
+  ServiceStopper(std::vector<Service*> services);
+
+  virtual bool consume(RobotModeData_V1_X& data)
+  {
+      return handle(data, data.robot_mode != robot_mode_V1_X::ROBOT_RUNNING_MODE);
+  }
+  virtual bool consume(RobotModeData_V3_0__1& data)
+  {
+      return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING);
+  }
+  virtual bool consume(RobotModeData_V3_2& data)
+  {
+      return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING);
+  }
+
+  //unused
+  virtual bool consume(MasterBoardData_V1_X& data) { return true; }
+  virtual bool consume(MasterBoardData_V3_0__1& data) { return true; }
+  virtual bool consume(MasterBoardData_V3_2& data) { return true; }
+};

include/ur_modern_driver/ros/trajectory_follower.h

@@ -0,0 +1,44 @@
+#pragma once
+
+#include <array>
+#include <atomic>
+#include <cstddef>
+#include <cstring>
+#include <string>
+#include <inttypes.h>
+#include "ur_modern_driver/ur/commander.h"
+#include "ur_modern_driver/ur/server.h"
+#include "ur_modern_driver/ur/stream.h"
+
+class TrajectoryFollower 
+{
+private:
+  const int32_t MULT_JOINTSTATE_ = 1000000;
+  double servoj_time_, servoj_lookahead_time_, servoj_gain_;
+  std::atomic<bool> running_;
+  std::array<double, 6> last_positions_;
+  URCommander &commander_;
+  URServer server_;
+  URStream stream_;
+  std::string program_;
+
+  template <typename T>
+  size_t append(uint8_t *buffer, T &val)
+  {
+    size_t s = sizeof(T);
+    std::memcpy(buffer, &val, s);
+    return s;
+  }
+
+  bool execute(std::array<double, 6> &positions, bool keep_alive);  
+
+public:
+  TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3);
+
+  std::string buildProgram(bool version_3);
+
+  bool start();
+  bool execute(std::array<double, 6> &positions);
+  void stop();
+  void halt(); //maybe
+};

include/ur_modern_driver/ur/commander.h

@@ -17,11 +17,12 @@ public:
   {
   }
 
+  virtual bool uploadProg(std::string &s);
   virtual bool speedj(std::array<double, 6> &speeds, double acceleration);
   virtual bool stopj(double a = 10.0);
   virtual bool setDigitalOut(uint8_t pin, bool value);
   virtual bool setAnalogOut(uint8_t pin, double value);
   virtual bool setToolVoltage(uint8_t voltage);
-  virtual bool setFlag(bool value);
+  virtual bool setFlag(uint8_t pin, bool value);
   virtual bool setPayload(double value);
 };

include/ur_modern_driver/ur/robot_mode.h

@@ -16,6 +16,7 @@ public:
   bool real_robot_enabled;
   bool robot_power_on;
   bool emergency_stopped;
+  bool protective_stopped; //AKA security_stopped
   bool program_running;
   bool program_paused;
 
@@ -43,7 +44,6 @@ public:
   virtual bool parseWith(BinParser& bp);
   virtual bool consumeWith(URStatePacketConsumer& consumer);
 
-  bool security_stopped;
   robot_mode_V1_X robot_mode;
   double speed_fraction;
 
@@ -79,7 +79,6 @@ public:
   virtual bool parseWith(BinParser& bp);
   virtual bool consumeWith(URStatePacketConsumer& consumer);
 
-  bool protective_stopped;
 
   robot_mode_V3_X robot_mode;
   robot_control_mode_V3_X control_mode;

include/ur_modern_driver/ur/server.h

@@ -0,0 +1,19 @@
+#pragma once
+#include <netdb.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <cstdlib>
+#include <mutex>
+#include <atomic>
+#include <string>
+#include "ur_modern_driver/ur/stream.h"
+
+class URServer
+{
+private:
+  int socket_fd_ = -1;
+
+public:
+  URServer(int port);
+  URStream accept();
+};

include/ur_modern_driver/ur/stream.h

@@ -19,15 +19,38 @@ private:
   std::mutex send_mutex_, receive_mutex_;
 
 public:
+  URStream() 
+  {
+  }
+
   URStream(std::string& host, int port) : host_(host), port_(port), initialized_(false), stopping_(false)
   {
   }
 
+  URStream(int socket_fd) : socket_fd_(socket_fd), initialized_(true), stopping_(false)
+  {
+    
+  }
+
+  URStream(URStream&& other) noexcept : socket_fd_(other.socket_fd_), host_(other.host_), initialized_(other.initialized_.load()), stopping_(other.stopping_.load())
+  {
+    
+  }
+
   ~URStream()
   {
     disconnect();
   }
 
+  URStream& operator=(URStream&& other)
+  {
+    socket_fd_ = std::move(other.socket_fd_);
+    host_ = std::move(other.host_); 
+    initialized_ = std::move(other.initialized_.load());
+    stopping_ = std::move(other.stopping_.load());
+    return *this;
+  }
+
   bool connect();
   void disconnect();
   void reconnect();

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

tests/ur/robot_mode.cpp

@@ -18,7 +18,7 @@ TEST(RobotModeData_V1_X, testRandomDataParsing)
   ASSERT_EQ(rdt.getNext<bool>(), state.real_robot_enabled);
   ASSERT_EQ(rdt.getNext<bool>(), state.robot_power_on);
   ASSERT_EQ(rdt.getNext<bool>(), state.emergency_stopped);
-  ASSERT_EQ(rdt.getNext<bool>(), state.security_stopped);
+  ASSERT_EQ(rdt.getNext<bool>(), state.protective_stopped);
   ASSERT_EQ(rdt.getNext<bool>(), state.program_running);
   ASSERT_EQ(rdt.getNext<bool>(), state.program_paused);
   ASSERT_EQ(rdt.getNext<robot_mode_V1_X>(), state.robot_mode);