 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; Get, move to state ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 
 (defn get-current-arm-state-msg [nh right?]
-  (call-srv nh (str "/" (if right? "r" "l") 
+  (call-srv-cached nh (str "/" (if right? "r" "l") 
 		    "_arm_joint_trajectory_controller/TrajectoryQuery" )
 	    (map-msg {:class TrajectoryQuery$Request :trajectoryid -1})))
 

(defn move-arm-directly-to-state [#^NodeHandle nh arm-state]
  (let [right? (isa? (:type arm-state) ::Right)
	{:keys [jointnames jointpositions]} (get-current-arm-state-msg nh right?)]
    (call-srv nh (str "/" (if right? "r" "l") "_arm_joint_trajectory_controller/TrajectoryStart")
      (map-msg
       {:class TrajectoryStart$Request :hastiming 0 :requesttiming 0
	:traj {:class JointTraj

	slop        (map #(mod (- %2 (+ %1 (/ res 2))) res) minc [(:x final-base-state) (:y final-base-state)]) 
	minc        (map + minc slop)]
;    (println slop minc)
    (when-not (= window @*last-window*) ; Assume costmap is static.
      (println "Setting costmap")
      (call-srv  nh "/navfn_node/set_costmap" 
		(map-msg (world->costmap world minc maxc)))
      (reset! *last-window* window))
    (let [result 
	  (call-srv-cached nh "/navfn_node/make_plan" 
		    (map-msg {:class MakeNavPlan$Request
			:start (base-state->pose-stamped (base-state->disc initial-base-state res minc maxc))
			:goal  (base-state->pose-stamped (base-state->disc final-base-state res minc maxc))}))]
