Merge pull request #73 from cpaxton/devel

Removed redoing unnecessary replanning on smart_move_multipurpose
cpaxton · Mar 31, 2017 · 0e9f45c · 0e9f45c
2 parents aac8acb + f85e76c
commit 0e9f45c
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 9 deletions.
diff --git a/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py b/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py
@@ -253,7 +253,6 @@ def release(self):
     listen to robot joint state information
     '''
     def js_cb(self,msg):
-
         if len(msg.position) is self.dof:
             self.old_q0 = self.q0
             self.q0 = np.array(msg.position)
@@ -980,15 +979,11 @@ def smartmove_multipurpose_gripper(self, stamp, possible_goals, distance, grippe
                 (code2,res2) = self.planner.getPlan(T,q_2,obj=obj)
                 if ((not res2 is None) and len(res2.planned_trajectory.joint_trajectory.points) > 0):
                     msg = self.send_and_publish_planning_result(res,stamp,acceleration,velocity)
-                    rospy.sleep(0.1)
-                    (code2,res2) = self.planner.getPlan(T,self.q0,obj=obj)
                     if msg[0:7] == 'SUCCESS':
                         msg = self.send_and_publish_planning_result(res2,stamp,acceleration,velocity)
                         if msg[0:7] == 'SUCCESS':
                             gripper_function(obj)
 
-                            rospy.sleep(0.1)
-
                             traj = res2.planned_trajectory.joint_trajectory
                             traj.points.reverse()
                             end_t = traj.points[0].time_from_start

diff --git a/costar_robot/costar_robot_manager/src/costar_robot/iiwa_driver.py b/costar_robot/costar_robot_manager/src/costar_robot/iiwa_driver.py
@@ -51,8 +51,8 @@ def send_trajectory(self,traj,stamp,acceleration=0.5,velocity=0.5,cartesian=Fals
           print " -- %s"%(str(pt.positions))
           start_t = rospy.Time.now()
 
-          if self.cur_stamp > stamp:
-            return 'FAILURE - preempted'
+          if not self.valid_verify(stamp):
+            return 'FAILURE -- preempted'
 
           rospy.sleep(rospy.Duration(pt.time_from_start.to_sec() - t.to_sec()))
           t = pt.time_from_start
@@ -68,9 +68,9 @@ def send_trajectory(self,traj,stamp,acceleration=0.5,velocity=0.5,cartesian=Fals
             rate.sleep()
 
         if self.at_goal:
-            return 'SUCCESS - moved to pose'
+            return 'SUCCESS -- moved to pose'
         else:
-            return 'FAILURE - did not reach destination'
+            return 'FAILURE -- did not reach destination'
 
     def handle_tick(self):
         super(CostarIIWADriver,self).handle_tick()