Add auto-start

jsk-ros-pkg · Dec 6, 2024 · 1120a27 · 1120a27
1 parent 781dac4
commit 1120a27
Showing 1 changed file with 9 additions and 6 deletions.
diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l
@@ -533,18 +533,21 @@
      (instance geometry_msgs::quaternion :init :x qx :y qy :z qz :w qw)))
   )
 
-(defun spot-init (&optional (create-viewer))
+(defun spot-init (&optional (create-viewer) &key (auto-start t))
   (unless (boundp '*spot*) (spot) (send *spot* :reset-pose))
   (unless (ros::ok) (ros::roseus "spot_eus_interface"))
   (unless (boundp '*ri*) (setq *ri* (instance spot-interface :init)))
 
   (ros::spin-once)
   (send *ri* :spin-once)
-  (send *ri* :claim)
-  (while (member (send *ri* :state :power-state-motor-power-state) (list 'off nil))
-    (unix::sleep 1)
-    (ros::ros-info "powering on...")
-    (send *ri* :power-on))
+  (if auto-start
+    (progn
+      (send *ri* :claim)
+      (while (member (send *ri* :state :power-state-motor-power-state) (list 'off nil))
+        (unix::sleep 1)
+        (ros::ros-info "powering on...")
+        (send *ri* :power-on))
+      ))
   ;;
   (unless (every #'(lambda (x) (eq x 'made)) (mapcar #'(lambda (x) (cdr (assoc :contact x))) (send *ri* :state :feet)))
       (ros::ros-info "run (send *ri* :stand) to stand the robot"))