 	 (while (not @a) (.spinOnce nh))
 	 @a))))
 
+(let [mem (atom {})]
+  (def get-current-base-odom
+     (fn [#^NodeHandle nh]
+       (let [a 
+	     (or (@mem nh)
+		 (let [a (atom nil)]
+		   (.subscribe nh "/robot_pose_ekf/odom_combined" 
+			       (PoseWithCovariance.)
+			       (sub-cb [m] (reset! a m)) 1)
+		   (swap! mem assoc nh a)
+		   a))]
+	 (.spinOnce nh)
+	 (while (not @a) (.spinOnce nh))
+	 @a))))
+
 (defn get-current-base-state [#^NodeHandle nh]
   (pose-stamped->base-state (get-current-base-pose nh)))
 

  [#^NodeHandle nh arm-state]
  (run-action nh (str "/move_" (if (isa? (:class arm-state) ::Right) "right" "left") "_arm") 
    (map-msg {:class MoveArmGoal 
	      :path_constraints *no-constraints*
	      :goal_constraints {:pose_constraint  []
				 :joint_constraint (vec (map parse-joint-constraint 

(defn move-base-unsafe 
  "Custom interface for moving base directly, without move_base."
  [#^NodeHandle nh command-fn goal-fn]
  (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 []
	(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)
					   [:vel] #(merge zero %))
					   [:ang_vel] #(merge zero %))))
	  (Thread/sleep 100)
	  (recur))))
      (.publish pub (map-msg PoseDot {:vel zero :ang_vel zero}))
      (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)))


