fixed oscillations

Author: asaba96

param/tasks.yaml

@@ -6,7 +6,7 @@ max_translation_speed: 1.0
 # maximum z velocity
 max_z_velocity: 2.0
 # planner lag, in seconds
-planning_lag: .35
+planning_lag: .100
 # timeout for time to get a plan
 planning_timeout: .5
 # distance from goal to stop replanning

src/iarc7_motion/iarc_tasks/xyztranslation_task.py

@@ -2,6 +2,7 @@
 import rospy
 import tf2_ros
 import threading
+import cPickle as pickle
 
 from .abstract_task import AbstractTask
 from iarc_tasks.task_states import (TaskRunning,
@@ -35,6 +36,9 @@ def __init__(self, task_request):
 
         self._starting_motion_point = None
 
+        # self._list_info = []
+        # self._request_list = []
+
         self._lock = threading.RLock()
 
         self._linear_gen = self.topic_buffer.get_linear_motion_profile_generator()
@@ -66,12 +70,16 @@ def get_desired_command(self):
 
             if self._state == XYZTranslationTaskState.COMPLETING:
                 if (rospy.Time.now() + rospy.Duration(.10)) >= self._complete_time:
+                    # name = '/home/andrew/data.bin'
+                    # file = open(name, 'wb')
+                    # pickle.dump(self._list_info, file)
+                    # file.close()
+                    # self._linear_gen.dump_info()
                     return (TaskDone(),)
                 else:
                     return (TaskRunning(), NopCommand())
 
             expected_time = rospy.Time.now() + self._PLANNING_LAG
-
             self._starting_motion_point = self._linear_gen.expected_point_at_time(expected_time).motion_point
 
             _starting_pose = self._starting_motion_point.pose.position
@@ -129,6 +137,8 @@ def _generate_request(self, expected_time, reset_timer=True):
         goal.motion_point.pose.position.y = self._goal_y
         goal.motion_point.pose.position.z = self._goal_z
 
+        self._request_list = [start, goal, None]
+
         request.start = start
         request.goal = goal
 
@@ -141,6 +151,8 @@ def _feedback_callback(self, status, msg):
         with self._lock:
             self._feedback = msg
             self._plan = msg.plan
+            # self._request_list[2] = msg.plan
+            # self._list_info.append(self._request_list)
             self._state = XYZTranslationTaskState.PLAN_RECEIVED
 
     def cancel(self):

src/iarc7_motion/linear_motion_profile_generator.py

@@ -8,7 +8,7 @@
 from nav_msgs.msg import Path
 
 from iarc7_msgs.msg import MotionPointStamped, MotionPointStampedArray
-
+import cPickle as pickle
 
 # Convert a 3 element numpy array to a Vector3 message
 def np_to_msg(array, msg):
@@ -94,6 +94,8 @@ def __init__(self, start_motion_point):
         self._override_start_position = None
         self._override_start_velocity = None
 
+        # self._info = []
+
     linear_motion_profile_generator = None
 
     @staticmethod
@@ -103,8 +105,30 @@ def get_linear_motion_profile_generator():
                 MotionPointStamped())
         return LinearMotionProfileGenerator.linear_motion_profile_generator
 
+    # def dump_info(self):
+    #     name = '/home/andrew/new_data.bin'
+    #     file = open(name, 'wb')
+    #     pickle.dump(self._info, file)
+    #     file.close()
+
     def set_global_plan(self, plan):
-        self._last_motion_plan = plan
+        if len(self._last_motion_plan.motion_points) == 0:
+            # self._info.append((self._last_motion_plan, plan))
+            self._last_motion_plan = plan
+        else:
+            new_plan = MotionPointStampedArray()
+            new_point_stamp = plan.motion_points[0].header.stamp
+
+            for i in range(0, len(self._last_motion_plan.motion_points)):
+                current_stamp = self._last_motion_plan.motion_points[i].header.stamp
+                if current_stamp > new_point_stamp:
+                    new_plan.motion_points = self._last_motion_plan.motion_points[:i]
+                    break
+
+            new_plan.motion_points.extend(plan.motion_points)
+            # self._info.append((self._last_motion_plan, plan))
+
+            self._last_motion_plan = new_plan
 
     # Get a starting motion point for a given time
     def _get_start_point(self,