From 6d6390fa53db43544b5413f515d9f444c01d9d60 Mon Sep 17 00:00:00 2001
From: Patrick Goebel <patrick@pirobot.org>
Date: Sun, 6 Nov 2016 06:08:37 -0800
Subject: [PATCH] Finished tweaks on follow_controller.py to execute joint
 action trajectories

---
 .../ros_arduino_python/follow_controller.py   | 10 ++++++-
 .../ros_arduino_python/servo_controller.py    | 28 +++----------------
 2 files changed, 13 insertions(+), 25 deletions(-)

diff --git a/ros_arduino_python/src/ros_arduino_python/follow_controller.py b/ros_arduino_python/src/ros_arduino_python/follow_controller.py
index 1999d4e..39968c9 100644
--- a/ros_arduino_python/src/ros_arduino_python/follow_controller.py
+++ b/ros_arduino_python/src/ros_arduino_python/follow_controller.py
@@ -61,7 +61,7 @@ class FollowController(Controller):
     def execute_cb(self, goal):
         rospy.loginfo(self.name + ": Action goal recieved.")
         traj = goal.trajectory
-
+        
         if set(self.joints) != set(traj.joint_names):
             for j in self.joints:
                 if j not in traj.joint_names:
@@ -115,6 +115,10 @@ class FollowController(Controller):
         start = traj.header.stamp
         if start.secs == 0 and start.nsecs == 0:
             start = rospy.Time.now()
+        
+        # Set the start time for each joint
+        for i in range(len(self.joints)):
+            self.device.joints[self.joints[i]].time_move_started = start
 
         r = rospy.Rate(self.rate)
         last = [ self.device.joints[joint].position for joint in self.joints ]
@@ -138,8 +142,12 @@ class FollowController(Controller):
                         last[i] += cmd
                         self.device.joints[self.joints[i]].time_move_started = rospy.Time.now()
                         self.device.joints[self.joints[i]].in_trajectory = True
+                        self.device.joints[self.joints[i]].is_moving = True
                         self.device.joints[self.joints[i]].desired = last[i]
                     else:
+                        self.device.joints[self.joints[i]].speed = 0.0
+                        self.device.joints[self.joints[i]].in_trajectory = True
+                        self.device.joints[self.joints[i]].is_moving = False
                         velocity[i] = 0
                 r.sleep()
         return True
diff --git a/ros_arduino_python/src/ros_arduino_python/servo_controller.py b/ros_arduino_python/src/ros_arduino_python/servo_controller.py
index b76bf5a..6a0c638 100755
--- a/ros_arduino_python/src/ros_arduino_python/servo_controller.py
+++ b/ros_arduino_python/src/ros_arduino_python/servo_controller.py
@@ -36,7 +36,6 @@ class Joint:
 
         self.position = 0.0        # radians
         self.start_position = 0.0
-        self.goal_position = 0.0
         self.position_last = 0.0   # radians
         self.velocity = 0.0        # rad/s
         self.servo_speed = 0.0     # rad/s
@@ -144,9 +143,6 @@ class Servo(Joint):
         # Record the starting position
         self.start_position = self.get_current_position()
 
-        # Record the goal_postion
-        self.goal_position = msg.data
-
         # Set the target position for the next servo controller update
         self.desired = msg.data
 
@@ -280,24 +276,6 @@ class ContinousServo(Joint):
         self.is_moving = True
         self.direction = copysign(1, self.desired - self.position)
 
-#     def command_cb(self, msg):
-#         # Check limits
-#         if msg.data > self.max_speed:
-#             msg.data = self.max_speed
-#
-#         if msg.data < self.min_speed:
-#             msg.data = self.min_speed
-#
-#         # Adjust for the neutral offset
-#         if self.invert:
-#             servo_ticks = self.neutral - msg.data * self.ticks_per_rad_per_sec
-#         else:
-#             servo_ticks = self.neutral + msg.data * self.ticks_per_rad_per_sec
-#
-#         # Set the target speed for the next servo controller update
-#         self.desired =  radians(servo_ticks)
-
-
 class ServoController():
     def __init__(self, device, name):
         self.name = name
@@ -317,6 +295,7 @@ class ServoController():
         if rospy.Time.now() > self.next_update:
             for servo in self.servos:
 
+                
                 # Check to see if we are within 2 degrees of the desired position or gone past it
                 if not servo.in_trajectory and servo.is_moving:
                     if abs(servo.position - servo.desired) < radians(2.0) or copysign(1, servo.desired - servo.position) != servo.direction:
@@ -324,11 +303,12 @@ class ServoController():
                         servo.is_moving = False
                         servo.velocity = 0.0
                         duration =  rospy.Time.now() - servo.time_move_started
-
+                        
                 # If the servo is still moving, update its interpolated position and velocity
                 if servo.is_moving:
                     try:
                         # Get the interpolated current position of the servo
+                        servo.position = servo.get_interpolated_position()
                         if not servo.in_trajectory:
                             servo.position = servo.get_interpolated_position()
                         else:
@@ -337,7 +317,7 @@ class ServoController():
                         # We cannot interpolate both position and velocity so just set velocity to the current speed
                         servo.velocity = servo.servo_speed
                         servo.position_last = servo.position
-
+                        
                         # Update diagnostics counters
                         servo.diagnostics.reads += 1
                         servo.diagnostics.total_reads += 1