fixes

Author: jcmayoral

gr_navigation_managers/simple_crop_nav/simple_crop_nav.py

@@ -20,6 +20,16 @@ def __init__(self, desired_speed = 1.0):
         self.start = False
         self.forward = True
 
+        self.setPoses()
+        #rospy.Timer(rospy.Duration(10), self.change_direction)
+        rospy.Subscriber("/odometry/base_raw", Odometry, self.odom_cb)
+        self.pub = rospy.Publisher("/nav_vel", Twist, queue_size=1)
+        self.rb = utils.BagRecorder(desired_path = "/home/jose/ros_ws/src/gr_navigation/gr_navigation_managers/simple_crop_nav/data/",smach=False)
+
+        if self.forward:
+            self.rb.startBag()
+
+    def setPoses(self):
         try:
             self.listener.waitForTransform('/odom', '/base_link', rospy.Time(), rospy.Duration(2.0))
             (trans,rot) = self.listener.lookupTransform('/odom', '/base_link', rospy.Time())
@@ -29,44 +39,43 @@ def __init__(self, desired_speed = 1.0):
             endpose = PoseStamped()
             endpose.header.frame_id = "base_link"
             endpose.pose.orientation.w = 1.0
-            endpose.pose.position.x = 10.0
+            if self.forward:
+                endpose.pose.position.x = 10.0
+            else:
+                endpose.pose.position.x = -10.0
             p_end = self.listener.transformPose("odom", endpose)
             self.endpose = [p_end.pose.position.x, p_end.pose.position.y]
         except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
             sys.exit()#continue
-        #rospy.Timer(rospy.Duration(10), self.change_direction)
-        rospy.Subscriber("/odometry/base_raw", Odometry, self.odom_cb)
-        self.pub = rospy.Publisher("/nav_vel", Twist, queue_size=1)
-        self.rb = utils.BagRecorder(desired_path = "/home/jose/ros_ws/src/gr_navigation/gr_navigation_managers/simple_crop_nav/data/",smach=False)
 
-        if self.forward:
-            self.rb.startBag()
 
     def odom_cb(self, msg):
         self.currentpose[0] = msg.pose.pose.position.x
         self.currentpose[1] = msg.pose.pose.position.y
         self.start = True
 
-    def is_finished(self):
+    def is_running(self):
         return self.current_motions < self.max_motions
 
     def publish(self):
         self.pub.publish(self.twist)
+        print math.sqrt(math.pow(self.endpose[0] - self.currentpose[0],2) + math.pow(self.endpose[1] - self.currentpose[1],2))
 
-        if self.forward:
-            if math.sqrt(math.pow(self.endpose[0] - self.currentpose[0],2) + math.pow(self.endpose[1] - self.currentpose[1],2)) < 0.5:
-                self.change_direction()
-        else:
-            if math.sqrt(math.pow(self.startpose[0] - self.currentpose[0],2) + math.pow(self.startpose[1] - self.currentpose[1],2)) < 0.5:
-                self.change_direction()
+        if math.sqrt(math.pow(self.endpose[0] - self.currentpose[0],2) + math.pow(self.endpose[1] - self.currentpose[1],2)) < 0.5:
+            self.change_direction()
 
     def change_direction(self):
         self.twist.linear.x = -self.twist.linear.x
         self.forward = not self.forward
         print ("chnage direction forward ", self.forward)
         self.current_motions = self.current_motions + 1
 
+        self.setPoses()
+
         if not self.forward:
             self.rb.close()
         else:
-            self.rb.restart("test", str(self.current_motions), close_required = False)
+            if self.is_running():
+                self.rb.restart("test", str(self.current_motions), close_required = False)
+            else:
+                self.rb.close()

gr_navigation_managers/simple_crop_nav/simple_crop_nav_node.py

@@ -6,7 +6,7 @@
     rospy.init_node("simple_crop_nav_controller")
     controller = SimpleCropNavController()
 
-    while not rospy.is_shutdown() and controller.is_finished():
+    while not rospy.is_shutdown() and controller.is_running():
         controller.publish()
         rospy.sleep(0.1)
     rospy.loginfo("FINISHED")