 (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)))
 
 

	(recur)))
    (.shutdown pub)))

(defn spin-base-to
  "Spins base to a desired angle, with no collision checking."
  [#^NodeHandle nh angle]

     (apply-gripper-force nh (isa? (:class gs) ::Right) (* (:force gs) (if (:open? gs) 1 -1)))))


; Bottles - 45 (everything)


(comment 
(defn open [] (move-gripper-to-state nh (make-robot-gripper-state nh true)))
(defn close [] (move-gripper-to-state nh (make-robot-gripper-state nh false 0.45 nil)))
(defn fw [x] (move-base-rel nh :vx x))
(defn r  [x] (move-base-rel nh :vy x))
)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; Arms ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
