     (cons (> (:in_collision res) 0)
 	  (lazy-seq [(apply hash-map (interleave (:link_names res) (:link_poses res)))]))))
 
+
+
+
 (defn robot-forward-kinematics
   "Like forward-kinematics, but takes a robot state and possibly world state,
    if a collision map is to be published."
  ([#^NodeHandle nh robot]
     (forward-kinematics nh (get-joint-map robot)))
  ([#^NodeHandle nh robot world]
-    (put-single-message nh "/fk_node/collision_map" 
-      (map-msg (world->collision-map world)) 1)
+    (put-single-message-cached nh "/fk_node/collision_map" 
+      (map-msg (world->collision-map world)) )
     (robot-forward-kinematics nh robot)))
 
 (defn rand-double [[mn mx]]

     (safe-inverse-kinematics nh right? pose-stamped robot world random-retries false))
  ([#^NodeHandle nh right? pose-stamped robot world random-retries start-random?]
     (when world
       (put-single-message nh "/fk_node/collision_map" 
	 (map-msg (world->collision-map world)) 1))
     (let [all-joints (get-joint-map robot)]
      (loop [tries random-retries 
	    init-joints (if start-random? (random-arm-joint-map nh right?)

	  :let [limits (*robot-joint-limits* joint)]]
      [joint (rand-double 
	      (or limits
		 (do ;(println "Warning: no limits for joint" joint)
		     [0 (* 2 Math/PI)])))])))

(defn safe-inverse-kinematics 
