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