Big code dump

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