Major refactor

src/ros/action_server.cpp

@@ -15,7 +15,14 @@ ActionServer::ActionServer(TrajectoryFollower& follower, std::vector<std::string
   , state_(RobotState::Error)
   , follower_(follower)
 {
-  
+}
+
+void ActionServer::start()
+{
+  if(running_)
+    return;
+  running_ = true;
+  tj_thread_ = thread(&ActionServer::trajectoryThread, this);
 }
 
 void ActionServer::onRobotStateChange(RobotState state)
@@ -34,7 +41,14 @@ void ActionServer::onGoal(GoalHandle gh)
 
 void ActionServer::onCancel(GoalHandle gh)
 {
+  interrupt_traj_ = true;
+  //wait for goal to be interrupted
+  std::lock_guard<std::mutex> lock(tj_mutex_);
 
+  Result res;
+  res.error_code = -100;
+  res.error_string = "Goal cancelled by client";
+  gh.setCanceled(res);
 }
 
 bool ActionServer::validate(GoalHandle& gh, Result& res)
@@ -125,9 +139,17 @@ bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res)
     }
   }
 
+  //todo validate start position?
+
   return true;
 }
 
+inline std::chrono::microseconds convert(const ros::Duration &dur)
+{
+  return std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::seconds(dur.sec)) 
+    + std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::nanoseconds(dur.nsec));
+}
+
 bool ActionServer::try_execute(GoalHandle& gh, Result& res)
 {
   if(!running_)
@@ -137,27 +159,20 @@ bool ActionServer::try_execute(GoalHandle& gh, Result& res)
   }
   if(!tj_mutex_.try_lock())
   {
-    has_goal_ = false;
-    //stop_trajectory();
+    interrupt_traj_ = true;
     res.error_string = "Received another trajectory";
     curr_gh_.setAborted(res, res.error_string);
     tj_mutex_.lock();
+    //todo: make configurable
+    std::this_thread::sleep_for(std::chrono::milliseconds(250));
   }
   //locked here
   curr_gh_ = gh;
+  interrupt_traj_ = false;
   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);
+  return true;
 }
 
 std::vector<size_t> ActionServer::reorderMap(std::vector<std::string> goal_joints)
@@ -179,53 +194,56 @@ std::vector<size_t> ActionServer::reorderMap(std::vector<std::string> goal_joint
 
 void ActionServer::trajectoryThread()
 {
+  follower_.start(); //todo check error
+  //as_.start();
   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();
+    LOG_DEBUG("Trajectory received and accepted");
+    curr_gh_.setAccepted();
+
+    auto goal = curr_gh_.getGoal();
+    auto mapping = reorderMap(goal->trajectory.joint_names);
+    std::vector<TrajectoryPoint> trajectory(goal->trajectory.points.size());
     
-    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_)
+    for(auto const& point : goal->trajectory.points)
     {
-      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++)
+      std::array<double, 6> pos, vel;
+      for(size_t i = 0; i < 6; i++)
       {
-        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]
-        );
+        //joint names of the goal might have a different ordering compared
+        //to what URScript expects so need to map between the two
+        size_t idx = mapping[i];
+        pos[idx] = point.positions[i];
+        vel[idx] = point.velocities[i];
       }
+      trajectory.push_back(TrajectoryPoint(pos, vel, convert(point.time_from_start)));
+    }
 
-      follower_.execute(pos);
-      //std::this_thread::sleep_for(std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.))); 
-      t = std::chrono::high_resolution_clock::now();
+    Result res;
+    if(follower_.execute(trajectory, interrupt_traj_))
+    {
+      //interrupted goals must be handled by interrupt trigger
+      if(!interrupt_traj_)
+      {
+        LOG_DEBUG("Trajectory executed successfully");
+        res.error_code = Result::SUCCESSFUL;
+        curr_gh_.setSucceeded(res);
+      }
+    }
+    else
+    {
+      LOG_DEBUG("Trajectory failed");
+      res.error_code = -100;
+      res.error_string = "Connection to robot was lost";      
+      curr_gh_.setAborted(res, res.error_string);
     }
 
     has_goal_ = false;
+    lk.unlock();
   }
+  follower_.stop();
 }

src/ros/controller.cpp

@@ -20,6 +20,8 @@ void ROSController::setupConsumer()
 
 void ROSController::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list, const std::list<hardware_interface::ControllerInfo>& stop_list)
 {
+  LOG_INFO("Switching hardware interface");
+
   if (active_interface_ != nullptr && stop_list.size() > 0)
   {
     LOG_INFO("Stopping active interface");
@@ -54,6 +56,14 @@ bool ROSController::write()
   return active_interface_->write();
 }
 
+void ROSController::reset()
+{
+  if (active_interface_ == nullptr)
+    return;
+  
+  active_interface_->reset();
+}
+
 void ROSController::read(RTShared& packet)
 {
   joint_interface_.update(packet);
@@ -68,17 +78,32 @@ bool ROSController::update(RTShared& state)
   lastUpdate_ = time;
 
   read(state);
-  controller_.update(time, diff);
+  controller_.update(time, diff, !service_enabled_);
 
   //emergency stop and such should not kill the pipeline
   //but still prevent writes
   if(!service_enabled_)
+  {
+    reset();
     return true;
+  }
+  
+  //allow the controller to update x times before allowing writes again
+  if(service_cooldown_ > 0)
+  {
+    service_cooldown_ -= 1;
+    return true;
+  }
 
   return write();
 }
 
 void ROSController::onRobotStateChange(RobotState state)
 {
-  service_enabled_ = (state == RobotState::Running);
+  bool next = (state == RobotState::Running);
+  if(next == service_enabled_)
+    return;
+  
+  service_enabled_ = next;
+  service_cooldown_ = 125; 
 }

src/ros/hardware_interface.cpp

@@ -1,61 +1,69 @@
 #include "ur_modern_driver/ros/hardware_interface.h"
+#include "ur_modern_driver/log.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]));
-    }
+  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;
+  positions_ = packet.q_actual;
+  velocities_ = packet.qd_actual;
+  efforts_ = packet.i_actual;
 }
 
 WrenchInterface::WrenchInterface()
 {
-    registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin() + 3));
-}
+  registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin() + 3));
+} 
 
 void WrenchInterface::update(RTShared &packet)
 {
-    tcp_ = packet.tcp_force;
+  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)
+  : 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]));
-    }
+  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));
-    }
+  for (size_t i = 0; i < 6; i++)
+  {
+    // clamp value to ±max_vel_change
+    double prev = prev_velocity_cmd_[i];
+    double lo = prev - max_vel_change_;
+    double hi = prev + 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_);
+}
 
-    return commander_.speedj(prev_velocity_cmd_, max_vel_change_);
+void VelocityInterface::reset()
+{
+  for(auto &val : prev_velocity_cmd_)
+  {
+    val = 0;
+  }
 }
 
 
 PositionInterface::  PositionInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector<std::string> &joint_names)
-    : commander_(commander)
+  : commander_(commander)
 {
-    for (size_t i = 0; i < 6; i++)
-    {
-        registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &position_cmd_[i]));
-    }
+  for (size_t i = 0; i < 6; i++)
+  {
+    registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &position_cmd_[i]));
+  }
 }
 
 bool PositionInterface::write()

src/ros/trajectory_follower.cpp

@@ -1,83 +1,96 @@
 #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 int32_t MULT_JOINTSTATE_ = 1000000;
 static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}");
 static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}");
+static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}");
+static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}");
 static const std::string POSITION_PROGRAM = R"(
 def driverProg():
-    MULT_jointstate = {{JOINT_STATE_REPLACE}}
+	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
+	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
+
+  socket_open(\"{{SERVER_IP_REPLACE}}\", {{SERVER_PORT_REPLACE}})
 
   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
+	  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)
+
+TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3)
+  : running_(false)
+  , commander_(commander)
+  , reverse_port_(reverse_port)
+  , server_(reverse_port)
 {
   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);
 
+  res.replace(res.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_));
 
   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());
+  res.replace(res.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
+
+  program_ = res;
+}
+
+std::string TrajectoryFollower::buildProgram()
+{
+  std::string res(program_);
+  std::string IP(server_.getIP());
+  LOG_INFO("Local IP: %s  ", IP.c_str());
+  res.replace(res.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), "127.0.0.1");
+  res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port_));
   return res;
 }
 
@@ -86,12 +99,31 @@ bool TrajectoryFollower::start()
   if(running_)
     return true; //not sure
 
-  //TODO
-  std::string prog(""); // buildProg();
-  if(!commander_.uploadProg(prog))
+  if(!server_.bind())
+  {
+    LOG_ERROR("Failed to bind server");
     return false;
+  }
 
-  stream_ = std::move(server_.accept()); //todo: pointer instead?
+  LOG_INFO("Uploading trajectory program to robot");
+
+  std::string prog(buildProgram());
+  //std::string prog = "socket_open(\"127.0.0.1\", 50001)\n";
+  if(!commander_.uploadProg(prog))
+  {
+    LOG_ERROR("Program upload failed!");
+    return false;
+  }
+
+  LOG_INFO("Awaiting incomming robot connection");
+
+  if(!server_.accept())
+  {
+    LOG_ERROR("Failed to accept incomming robot connection");
+    return false;
+  }
+  
+  LOG_INFO("Robot successfully connected");
   return (running_ = true);
 }
 
@@ -115,8 +147,18 @@ bool TrajectoryFollower::execute(std::array<double, 6> &positions, bool keep_ali
   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);
+  size_t written; 
+  return server_.write(buf, sizeof(buf), written);
+}
+
+double TrajectoryFollower::interpolate(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);
 }
 
 bool TrajectoryFollower::execute(std::array<double, 6> &positions)
@@ -124,6 +166,68 @@ bool TrajectoryFollower::execute(std::array<double, 6> &positions)
   return execute(positions, true);
 }
 
+bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt)
+{
+  if(!running_)
+    return false;
+  
+  using namespace std::chrono;
+  typedef duration<double> double_seconds;
+  typedef high_resolution_clock Clock;
+  typedef Clock::time_point Time;
+
+  auto const& last = trajectory[trajectory.size()-1];
+  auto& prev = trajectory[0];
+
+  Time t0 = Clock::now();
+  Time latest = t0;
+
+  std::array<double, 6> positions;
+
+  for(auto const& point : trajectory)
+  {
+    //skip t0
+    if(&point == &prev)
+      continue;
+
+    auto duration = point.time_from_start - prev.time_from_start;
+    double d_s = duration_cast<double_seconds>(duration).count();
+
+    //interpolation loop
+    while(!interrupt)
+    {
+      latest = Clock::now();
+      auto elapsed = latest - t0;
+
+      if(point.time_from_start <= elapsed || last.time_from_start >= elapsed)
+        break;
+
+      double elapsed_s = duration_cast<double_seconds>(elapsed - prev.time_from_start).count();
+      //double prev_seconds
+      for(size_t j = 0; j < positions.size(); j++)
+      {
+        positions[j] = interpolate(
+          elapsed_s, 
+          d_s, 
+          prev.positions[j], 
+          point.positions[j],
+          prev.velocities[j],
+          point.velocities[j]
+        );
+      }
+
+      if(!execute(positions, true))
+        return false;
+
+      std::this_thread::sleep_for(double_seconds(servoj_time_));
+    }
+
+    prev = point;
+  }
+
+  return true;
+}
+
 void TrajectoryFollower::stop()
 {
   if(!running_)
@@ -132,6 +236,6 @@ void TrajectoryFollower::stop()
   std::array<double, 6> empty;
   execute(empty, false);
 
-  stream_.disconnect();
+  //server_.disconnect();
   running_ = false;
 }