 		
 ;  TODO: add timeout
 ;; TODO: add preempt
-
 ;; TODO: attach bottles
 ;; TODO: ignore result of arm action.
 ;  TODO: integrate with find_bottles
-;  TODO: extend scrips.
+
+;;  TODO: extend scrips.
     ; Smart script for grasping - move base + ask for new point, if needed.
-
+;; TODO: fix rotate-toa-angle
 
 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;     States      ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

   (do (pt (open-gripper nh)) true)
   (do (Thread/sleep 3000) (final-approach nh obj) true)
   (do (pt (close-gripper nh)) true)
   (do (Thread/sleep 3000) (move-base-rel nh :vx -0.2) true)
   (pt (move-arm-to-state nh (arm-joint-state true "home") true))))

(defn grasp-rviz [nh]
  (let [[obj-map obj-bl] (get-rviz-points nh true)]
    (grasp-object nh obj-bl)))




	  (move-arm-to-pos ~'nh ~'j ~'upright?)))
             
     (defn ~'open [] (open-gripper ~'nh))
     (defn ~'close ([] (~'close 45)) ([~'f] (close-gripper ~'nh ~'f false)))

     (defn ~'home [] (~'go-arm-plan "home"))
     
