 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; Base ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 
+(def *current-base-pose* (atom nil))
+(def *base-pose-sub* (atom nil))
 
 (defstruct robot-base-state    :class :x :y :theta)
 

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

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

	  :pose   {:position    {:x x :y y :z z}
		   :orientation (axis-angle->quaternion-msg [ax ay az] angle)}}})))
  
(def *upright-rgripper-constraint*
  (let [tolv 0.5
	tol {:x tolv :y tolv :z tolv}]
   {:joint_constraint nil
    :pose_constraint 
     [{:type (+ PoseConstraint/ORIENTATION_R PoseConstraint/ORIENTATION_P)
       :link_name "r_gripper_palm_link"
       :pose {:header *bl-header* :pose (make-pose [0 0 0] [0 0 0 1])}
       :orientation_importance 1.0
       :orientation_tolerance_above tol
       :orientation_tolerance_below tol
       :position_tolerance_above tol
       :position_tolerance_below tol}]}))
       

(defn plan-arm-motion
  "joint constraints are passed as map from name to either value,  
   which will use this tolerance, or interval, or joint_constraint map.
   pose constraints are lists of PoseConstraints maps -- no shortcuts for now.
   Init-joints should include base and torso, in general."
  ([#^NodeHandle nh right? world robot-state joint-constraints pose-constraints]
     (plan-arm-motion nh right? world robot-state joint-constraints pose-constraints *no-constraints*))
  ([#^NodeHandle nh right? world robot-state joint-constraints pose-constraints path-constraints]
  (println "Putting collision map")
  (put-single-message-cached nh "/collision_map_future" (map-msg (world->collision-map world)))
;  (println "\n\n\n\n\n\n\n\n" right?)
