 (defn move-base-unsafe 
   "Custom interface for moving base directly, without move_base."
   [#^NodeHandle nh command-fn goal-fn]
-  (let [current-pose (atom nil)
-	pub (.advertise nh "/cmd_vel" (PoseDot.) 1)
-	sub (.subscribe nh "/base_pose_ground_truth" (PoseWithRatesStamped.)
-			(sub-cb [m] (reset! current-pose (:pose (:pose_with_rates m)))) 1)]
-    (print "Waiting for pose...")
-    (while (not @current-pose)
-      (print ".")
-      (.spinOnce nh)
-      (Thread/sleep 10))
-    (let [init-pose @current-pose
+  (let [pub (.advertise nh "/cmd_vel" (PoseDot.) 1)]
+    (let [init-pose (:pose (get-current-base-odom nh))
 	  init-pos (:position init-pose)
 	  zero     {:vx 0 :vy 0 :vz 0}]
       (loop []
-	(when (not (goal-fn init-pose @current-pose))
+	(let [current-pose (:pose (get-current-base-odom nh))]
+  	 (when (not (goal-fn init-pose current-pose))
 	  (.publish pub (map-msg PoseDot (update-in 
 					  (update-in 
-					   (command-fn init-pose @current-pose)
+					   (command-fn init-pose current-pose)
 					   [:vel] #(merge zero %))
 					   [:ang_vel] #(merge zero %))))
 	  (Thread/sleep 100)
-	  (.spinOnce nh)
-	  (recur)))
+	  (recur))))
       (.publish pub (map-msg PoseDot {:vel zero :ang_vel zero}))
-      (println "Stopping: traveled" (point-distance init-pos (:position @current-pose)))
-      (Thread/sleep 4000)
-      (.spinOnce nh)
-      (println "Stopping: traveled" (point-distance init-pos (:position @current-pose))))
-
-    (.shutdown sub)
+      (println "Stopping: traveled" (point-distance init-pos (:position (:pose (get-current-base-odom nh)))))
+;      (Thread/sleep 4000)
+;      (println "Stopping: traveled" (point-distance init-pos (:position (:pose (get-current-base-odom nh))))))
+      )
     (.shutdown pub)))
 
 

    (fn [init-pose current-pose]
      (let [ac (quaternion->angle (:orientation current-pose))
	    norm-diff (double (norm-angle (- angle ac)))]
	{:ang_vel {:vz (if (> (Math/abs norm-diff) 0.1) 0.5 (* norm-diff 3))}}))
    (fn [init-pose current-pose]
      (let [ac (quaternion->angle (:orientation current-pose))
	    norm-diff (double (norm-angle (- angle ac)))]
	(< (Math/abs norm-diff) 0.005)))))


(defn base-state->disc [state res minc maxc]

   (move-base-unsafe nh
    (fn [init-pose current-pose]
      (let [dist (- distance (point-distance (:position init-pose) (:position current-pose)))]
	(println "commanding" (* dir (if (> dist 0.1) 0.2 (* dist 3))))
	{:vel {coord (* dir (if (> dist 0.1) 0.2 (* dist 3)))}}))
    (fn [init-pose current-pose]
      (let [dist (- distance (point-distance (:position init-pose) (:position current-pose)))]
	(< (Math/abs (double dist)) 0.005))))))
