add graphnav navigation interface

[k-okada]

example and eus interface to use graphnav interface, see 5833273 to how to use this.

you need to use https://github.com/k-okada/spot_ros/tree/add_graphnav to work with this example.

@sktometometo @mqcmd196

[sktometometo]

@k-okada https://github.com/k-okada/spot_ros/tree/add_graphnav seems to lack files below.

• spot_driver/scripts/graph_nav_util.py
• spot_msgs/action/NavigateTo.action
• spot_msgs/srv/ListGraph.srv

[sktometometo]

I got an error below when executing move-to.l

spot@spot-jsk:~/catkin_ws/src/jsk_robot/jsk_spot_robot/spoteus$ roseus move-to.l 
configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l"
;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin 
;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb 
;; extending gcstack 0x555710fa8690[16374] --> 0x555711404460[32748] top=3c88
irtgl irtglc irtviewer 
EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-129(Sat Oct 17 05:56:37 UTC 2020)
roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-129 Sat Oct 17 05:56:37 UTC 2020  1.2.2))
eustf roseus_c_util [ INFO] [1605943737.365468193]: Call "/spot/claim" returns "bosdyn.api.AcquireLeaseResponse (ResourceAlreadyClaimedError): Use TakeLease method to forcefully grab the already claimed lease."
[ INFO] [1605943738.366273548]: powering on...
[ INFO] [1605943738.386214574]: Call "/spot/power_on" returns "Success"
[ INFO] [1605943738.387254187]: run (send *ri* :stand) to stand the robot
[ INFO] [1605943738.408519974]: Call "/spot/list_graph" returns ..
Call Stack (max depth: 20):
  0: at (elt ids index)
  1: at (setq ret (elt ids index))
  2: at (if (< index (length ids)) (setq ret (elt ids index)))
  3: at (let (ret) (if (< index 0) (setq index (+ (length ids) index))) (if (< index (length ids)) (setq ret (elt ids index))) ret)
  4: at (send self :find-waypoint-id-from-position navigate-to ids)
  5: at (setq navigate-to (send self :find-waypoint-id-from-position navigate-to ids))
  6: at (if (numberp navigate-to) (setq navigate-to (send self :find-waypoint-id-from-position navigate-to ids)))
  7: at (let (ids c goal ret) (setq ids (send self :list-graph upload-path)) (if (numberp navigate-to) (setq navigate-to (send self :find-waypoint-id-from-position navigate-to ids))) (if (numberp initial-localization-waypoint) (setq initial-localization-waypoint (send self :find-waypoint-id-from-position initial-localization-waypoint ids))) (setq c (instance ros::simple-action-client :init "/spot/navigate_to" spot_msgs::navigatetoaction)) (send c :wait-for-server) (ros::ros-info "run navigation from ~A" upload-path) (ros::ros-info "use fiducal localization ~A" initial-localization-fiducial) (ros::ros-info "initial waypoit ~3D/~3D ~A" (if initial-localization-waypoint (position initial-localization-waypoint ids :test #'string=) -1) (length ids) initial-localization-waypoint) (ros::ros-info "   goal waypoit ~3D/~3D ~A" (position navigate-to ids :test #'string=) (length ids) navigate-to) (setq goal (instance spot_msgs::navigatetoactiongoal :init)) (send goal :goal :upload_path upload-path) (send goal :goal :navigate_to navigate-to) (send goal :goal :initial_localization_fiducial initial-localization-fiducial) (send goal :goal :initial_localization_waypoint initial-localization-waypoint) (send c :send-goal goal :feedback-cb #'(lambda (msg) (let ((id (send msg :feedback :waypoint_id))) (ros::ros-info "~A/~A ~A" (position id ids :test #'string=) (length ids) id)))) (setq ret (send c :wait-for-result)) (ros::ros-info "~A ~A" ret (send (send c :get-result) :message)) ret)
  8: at (send *ri* :navigate-to path -1 :initial-localization-fiducial t :initial-localization-waypoint 0)
/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl: ERROR th=0  no sequence in (elt ids index)E: