     (.shutdown pub)))
 
 
-(defn move-base-forward-unsafe 
-  "Directly moves using base controllers, without invoking planning"
-  [#^NodeHandle nh distance]
+(defn move-base-rel
+  "Directly moves using base controllers (unsafe), without invoking planning"
+  [#^NodeHandle nh coord distance]
+  (assert (#{:vx :vy} coord))
+  (let [distance (double distance)
+	dir (Math/signum distance)
+	distance (Math/abs distance)]
    (move-base-unsafe nh
     (fn [init-pose current-pose]
       (let [dist (- distance (point-distance (:position init-pose) (:position current-pose)))]
-	{:vel {:vx (if (> dist 0.1) 0.5 (* dist 3))}}))
+	{:vel {coord (* dir (if (> dist 0.1) 0.5 (* dist 3)))}}))
     (fn [init-pose current-pose]
       (let [dist (- distance (point-distance (:position init-pose) (:position current-pose)))]
-	(< dist 0.005)))))
+	(< (Math/abs (double dist)) 0.005))))))
 
 (defn norm-angle [a]
   (cond (> a Math/PI) (- a (* 2 Math/PI))
 	(< a (- Math/PI)) (+ a (* 2 Math/PI))
 	:else a))
 
-(defn spin-base-unsafe 
+(defn spin-base
   "Spins base at a specified vecocity (pos = ccw) for a specified time."
   [#^NodeHandle nh vel secs]
   (let [pub (.advertise nh "/cmd_vel" (PoseDot.) 1)

    {:class JointTraj :names ks 
     :points 
       (for [t (range (inc steps))]
	 {:positions (map + j1 (map #(* % (/ t steps)) jd)) :time (* step-time t)})}))
   

(defn start-trajectory [#^NodeHandle nh srv-prefix traj]

	    :orientation (angle->quaternion (:theta base-state))}})


;(defn get-current-base-pose [#^NodeHandle nh]
;  (get-single-message nh "/base_pose_ground_truth" (PoseWithRatesStamped.)))

(let [mem (atom {})]
  (def get-current-base-pose 
     (fn [#^NodeHandle nh]
       (let [a 
	     (or (@mem nh)
		 (let [a (atom nil)]
		   (.subscribe nh "/amcl_pose" (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)))
