Implemented ros control

include/ur_modern_driver/ros/hardware_interface.h

@@ -0,0 +1,168 @@
+#pragma once
+#include <controller_manager/controller_manager.h>
+#include <hardware_interface/force_torque_sensor_interface.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/joint_state_interface.h>
+#include <algorithm>
+#include "ur_modern_driver/ur/commander.h"
+#include "ur_modern_driver/ur/rt_state.h"
+
+class HardwareInterface
+{
+public:
+  virtual void write() = 0;
+  virtual void start()
+  {
+  }
+  virtual void stop()
+  {
+  }
+};
+
+using hardware_interface::JointHandle;
+using hardware_interface::JointStateHandle;
+using hardware_interface::JointStateInterface;
+
+class JointInterface : public 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;
+  }
+};
+
+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;
+  }
+};
+
+class VelocityInterface : public HardwareInterface, public hardware_interface::VelocityJointInterface
+{
+private:
+  URCommander& commander_;
+  std::array<double, 6> velocity_cmd_, prev_velocity_cmd_;
+  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);
+  }
+};
+
+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_;
+  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()
+  {
+  }
+};

include/ur_modern_driver/ros/io_service.h

@@ -0,0 +1,61 @@
+#pragma once
+
+#include <ros/ros.h>
+#include <ur_msgs/SetIO.h>
+#include <ur_msgs/SetIORequest.h>
+#include <ur_msgs/SetIOResponse.h>
+#include <ur_msgs/SetPayload.h>
+#include <ur_msgs/SetPayloadRequest.h>
+#include <ur_msgs/SetPayloadResponse.h>
+#include "ur_modern_driver/log.h"
+#include "ur_modern_driver/ur/commander.h"
+
+class IOService 
+{
+private:
+  ros::NodeHandle nh_;
+  URCommander &commander_;
+  ros::ServiceServer io_service_;
+  ros::ServiceServer payload_service_;
+
+  bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp)
+  {
+    LOG_INFO("setIO called with [%d, %d]", req.fun, req.pin);
+    bool res = false;
+    bool flag = req.state > 0.0 ? true : false;
+    switch(req.fun)
+    {
+      case ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT:
+        res = commander_.setDigitalOut(req.pin, flag);
+      break;
+      case ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT:
+        res = commander_.setAnalogOut(req.pin, req.state);
+      break;
+      case ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE:
+        res = commander_.setToolVoltage(static_cast<uint8_t>(req.state));
+      break;
+      case ur_msgs::SetIO::Request::FUN_SET_FLAG:
+        res = commander_.setFlag(flag);
+      break;
+    }
+
+    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);
+  }
+
+
+public:
+  IOService(URCommander &commander) 
+    : commander_(commander)
+    , io_service_(nh_.advertiseService("ur_driver/set_io", &IOService::setIO, this))
+    , payload_service_(nh_.advertiseService("ur_driver/set_payload", &IOService::setPayload, this))
+  {
+
+  }
+};

include/ur_modern_driver/ros/mb_publisher.h

@@ -0,0 +1,55 @@
+#pragma once
+
+#include <ros/ros.h>
+#include <ur_msgs/Analog.h>
+#include <ur_msgs/Digital.h>
+#include <ur_msgs/IOStates.h>
+
+#include "ur_modern_driver/ur/consumer.h"
+
+using namespace ros;
+
+class MBPublisher : public URStatePacketConsumer
+{
+private:
+  NodeHandle nh_;
+  Publisher io_pub_;
+
+  template <size_t N>
+  inline void appendDigital(std::vector<ur_msgs::Digital> &vec, std::bitset<N> bits)
+  {
+    for(size_t i = 0; i < N; i++)
+    {
+        ur_msgs::Digital digi;
+        digi.pin = static_cast<uint8_t>(i);
+        digi.state = bits.test(i);
+        vec.push_back(digi);
+    }
+  }
+
+  void publish(ur_msgs::IOStates &io_msg, SharedMasterBoardData& data);
+
+public:
+  MBPublisher()
+    : io_pub_(nh_.advertise<ur_msgs::IOStates>("ur_driver/io_states", 1))
+  {
+  }
+
+  virtual bool consume(MasterBoardData_V1_X& data);
+  virtual bool consume(MasterBoardData_V3_0__1& data);
+  virtual bool consume(MasterBoardData_V3_2& data);
+
+  virtual bool consume(RobotModeData_V1_X& data);
+  virtual bool consume(RobotModeData_V3_0__1& data);
+  virtual bool consume(RobotModeData_V3_2& data);
+
+  virtual void setupConsumer()
+  {
+  }
+  virtual void teardownConsumer()
+  {
+  }
+  virtual void stopConsumer()
+  {
+  }
+};

include/ur_modern_driver/ros/robot_hardware.h

@@ -0,0 +1,46 @@
+#pragma once
+#include <controller_manager/controller_manager.h>
+#include <hardware_interface/force_torque_sensor_interface.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/ros/hardware_interface.h"
+
+using hardware_interface::RobotHW;
+using hardware_interface::ControllerInfo;
+
+class RobotHardware : public RobotHW
+{
+private:
+  JointInterface joint_interface_;
+  WrenchInterface wrench_interface_;
+  PositionInterface position_interface_;
+  VelocityInterface velocity_interface_;
+  hardware_interface::ForceTorqueSensorInterface force_torque_interface_;
+  HardwareInterface* active_interface_;
+  std::vector<void*> available_interfaces_;
+
+public:
+  RobotHardware(URCommander& commander, std::vector<std::string>& joint_names)
+    : joint_interface_(joint_names)
+    , wrench_interface_()
+    , position_interface_(commander, joint_interface_, joint_names)
+    , velocity_interface_(commander, joint_interface_, joint_names)
+    , available_interfaces_{&position_interface_, &velocity_interface_}
+  {
+    registerInterface<hardware_interface::JointStateInterface>(&joint_interface_);
+    registerInterface<hardware_interface::ForceTorqueSensorInterface>(&wrench_interface_);
+    registerInterface<hardware_interface::PositionJointInterface>(&position_interface_);
+    registerInterface<hardware_interface::VelocityJointInterface>(&velocity_interface_);
+  }
+
+  //bool canSwitch(const std::list<ControllerInfo>& start_list, const std::list<ControllerInfo>& stop_list) const;
+  void doSwitch(const std::list<ControllerInfo>& start_list, const std::list<ControllerInfo>& stop_list);
+
+
+  /// \brief Read the state from the robot hardware.
+  virtual void read(RTShared& packet);
+
+  /// \brief write the command to the robot hardware.
+  virtual void write();
+};

include/ur_modern_driver/ros/ros_controller.h

@@ -0,0 +1,60 @@
+#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);
+  }
+};

src/ros/mb_publisher.cpp

@@ -0,0 +1,54 @@
+#include "ur_modern_driver/ros/mb_publisher.h"
+
+inline void appendAnalog(std::vector<ur_msgs::Analog> &vec, double val, uint8_t pin)
+{
+  ur_msgs::Analog ana;
+  ana.pin = pin;
+  ana.state = val;
+  vec.push_back(ana);
+}
+
+void MBPublisher::publish(ur_msgs::IOStates &io_msg, SharedMasterBoardData& data)
+{
+  appendAnalog(io_msg.analog_in_states, data.analog_input0, 0);
+  appendAnalog(io_msg.analog_in_states, data.analog_input1, 1);
+  appendAnalog(io_msg.analog_out_states, data.analog_output0, 0);
+  appendAnalog(io_msg.analog_out_states, data.analog_output1, 1);
+
+  io_pub_.publish(io_msg);
+}
+
+bool MBPublisher::consume(MasterBoardData_V1_X& data)
+{
+  ur_msgs::IOStates io_msg;
+  appendDigital(io_msg.digital_in_states, data.digital_input_bits);
+  appendDigital(io_msg.digital_out_states, data.digital_output_bits);
+  publish(io_msg, data);
+  return true;
+}
+bool MBPublisher::consume(MasterBoardData_V3_0__1& data)
+{
+  ur_msgs::IOStates io_msg;
+  appendDigital(io_msg.digital_in_states, data.digital_input_bits);
+  appendDigital(io_msg.digital_out_states, data.digital_output_bits);
+  publish(io_msg, data);
+  return true;
+}
+bool MBPublisher::consume(MasterBoardData_V3_2& data)
+{
+  consume(static_cast<MasterBoardData_V3_0__1&>(data));
+  return true;
+}
+
+bool MBPublisher::consume(RobotModeData_V1_X& data)
+{
+  return true;
+}
+bool MBPublisher::consume(RobotModeData_V3_0__1& data)
+{
+  return true;
+}
+bool MBPublisher::consume(RobotModeData_V3_2& data)
+{
+  return true;
+}

src/ros/robot_hardware.cpp

@@ -0,0 +1,76 @@
+#include "ur_modern_driver/ros/robot_hardware.h"
+
+/*
+bool RobotHardware::canSwitch(const std::list<ControllerInfo>& start_list,
+                              const std::list<ControllerInfo>& stop_list) const
+{
+
+  bool running = active_interface_ != nullptr;
+  size_t start_size = start_list.size();
+  size_t stop_size = stop_list.size();
+
+
+  for (auto const& ci : stop_list)
+  {
+    auto it = interfaces_.find(ci.hardware_interface);
+    if(it == interfaces_.end() || it->second != active_interface_)
+      return false;
+  }
+  
+  for (auto const& ci : start_list)
+  {
+    auto it = interfaces_.find(ci.hardware_interface);
+    //we can only start a controller that's already running if we stop it first
+    if(it == interfaces_.end() || (it->second == active_interface_ && stop_size == 0))
+      return false;
+  }
+
+  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)
+  {
+    active_interface_->stop();
+    active_interface_ = nullptr;
+  }
+
+  for(auto const& ci : start_list)
+  {
+    auto it = interfaces_.find(ci.hardware_interface);
+    if(it == interfaces_.end())
+      continue;
+    
+    //we can only switch to one of the available interfaces
+    if(std::find(available_interfaces_.begin(), available_interfaces_.end(), it->second) == available_interfaces_.end())
+      continue;
+
+    auto new_interface = static_cast<HardwareInterface*>(it->second);
+
+    if(new_interface == nullptr)
+      continue;
+    
+    LOG_INFO("Starting %s", ci.hardware_interface.c_str());
+    active_interface_ = new_interface;
+    new_interface->start();
+    
+    return;
+  }
+}
+
+void RobotHardware::write()
+{
+  if(active_interface_ == nullptr)
+    return;
+  
+  active_interface_->write();
+}
+
+void RobotHardware::read(RTShared& packet)
+{
+  joint_interface_.update(packet);
+  wrench_interface_.update(packet);
+}