Major refactor

include/ur_modern_driver/ros/action_server.h

@@ -5,6 +5,7 @@
 #include <condition_variable>
 #include <mutex>
 #include <set>
+#include <thread>
 #include <ros/ros.h>
 #include <actionlib/server/action_server.h>
 #include <actionlib/server/server_goal_handle.h>
@@ -17,7 +18,7 @@
 #include "ur_modern_driver/ros/trajectory_follower.h"
 
 
-class ActionServer : public URRTPacketConsumer, public Service
+class ActionServer : public Service //,public URRTPacketConsumer
 {
 private:
   typedef control_msgs::FollowJointTrajectoryAction Action;
@@ -35,9 +36,11 @@ private:
 
   
   GoalHandle curr_gh_;
+  std::atomic<bool> interrupt_traj_;
   std::atomic<bool> has_goal_, running_;
   std::mutex tj_mutex_;
   std::condition_variable tj_cv_;
+  std::thread tj_thread_;
 
   TrajectoryFollower& follower_;
 
@@ -50,20 +53,16 @@ private:
   bool validateTrajectory(GoalHandle& gh, Result& res);
 
   bool try_execute(GoalHandle& gh, Result& res);
+  void interruptGoal(GoalHandle& gh);
 
   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);
 
+  void start();
   virtual void onRobotStateChange(RobotState state);
 };

include/ur_modern_driver/ros/controller.h

@@ -34,6 +34,7 @@ private:
   std::map<std::string, HardwareInterface*> available_interfaces_;
 
   std::atomic<bool> service_enabled_;
+  std::atomic<uint32_t> service_cooldown_;
 
   // helper functions to map interfaces
   template <typename T>
@@ -51,6 +52,7 @@ private:
   void read(RTShared& state);
   bool update(RTShared& state);
   bool write();
+  void reset();
 
 public:
   ROSController(URCommander& commander,  std::vector<std::string>& joint_names, double max_vel_change);

include/ur_modern_driver/ros/hardware_interface.h

@@ -13,6 +13,7 @@ public:
   virtual bool write() = 0;
   virtual void start() {}
   virtual void stop() {}
+  virtual void reset() {}
 };
 
 using hardware_interface::JointHandle;
@@ -48,6 +49,7 @@ private:
 public:
   VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector<std::string> &joint_names, double max_vel_change);
   virtual bool write();
+  virtual void reset();
   typedef hardware_interface::VelocityJointInterface parent_type;
 };
 

include/ur_modern_driver/ros/io_service.h

@@ -37,6 +37,8 @@ private:
       case ur_msgs::SetIO::Request::FUN_SET_FLAG:
         res = commander_.setFlag(req.pin, flag);
       break;
+      default:
+        LOG_WARN("Invalid setIO function called (%d)", req.fun);
     }
 
     return (resp.success = res);

include/ur_modern_driver/ros/trajectory_follower.h

@@ -5,21 +5,39 @@
 #include <cstddef>
 #include <cstring>
 #include <string>
+#include <thread>
 #include <inttypes.h>
+#include "ur_modern_driver/log.h"
 #include "ur_modern_driver/ur/commander.h"
 #include "ur_modern_driver/ur/server.h"
-#include "ur_modern_driver/ur/stream.h"
+
+struct TrajectoryPoint
+{
+  std::array<double, 6> positions;
+  std::array<double, 6> velocities;
+  std::chrono::microseconds time_from_start;
+
+  TrajectoryPoint()
+  {
+  }
+
+  TrajectoryPoint(std::array<double, 6> &pos, std::array<double, 6> &vel, std::chrono::microseconds tfs)
+    : positions(pos)
+    , velocities(vel)
+    , time_from_start(tfs)
+  {
+  }
+};
 
 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_;
+  int reverse_port_;
   std::string program_;
 
   template <typename T>
@@ -30,15 +48,16 @@ private:
     return s;
   }
 
+  std::string buildProgram();
   bool execute(std::array<double, 6> &positions, bool keep_alive);  
+  double interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel); 
 
 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);
+  bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt);
   void stop();
-  void halt(); //maybe
+  void interrupt();
 };