footstep plannerの出力する足配置結果が，successorで与えたものと異なる

[snozawa]

footstep plannerの出力する足配置結果が，successorで与えたものと異なります． 詳細は後で報告します．

[snozawa]

@YoheiKakiuchi さん

再現コードは以下です

1. roslaunch jsk_footstep_planner optimistic_footstep_planner.launch USE_SIMPLE_FOOTSTEP_CONTROLLER:=true GLOBAL_FRAME:=odom USE_PERCEPTION:=false ROBOT:=HRP2JSKNT USE_CONTROLLER:=false USE_MARKER:=false USE_OBSTACLE_MODEL:=true

2. 
   roseus
   (defun test-init ()
   (load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-interface.l")
   (hrp2jsknt)
   (setq *robot* *hrp2jsknt*)
   (objects (list *robot*))
   (ros::load-ros-manifest "jsk_footstep_planner")
   (load "package://jsk_footstep_controller/euslisp/util.l")
   (initialize-eus-footstep-planning-client)
   ;; *successors* is list of (float-vector x y)                                                                                                                                                                                          
   (setq *successors* (mapcar #'(lambda (ss) (float-vector (* 1e3 (cdr (assoc "x" ss :test #'string=))) (* 1e3 (cdr (assoc "y" ss :test #'string=))))) (ros::get-param "/footstep_planner/successors")))
   *footstep-planning-client*)

(defun test () (publish-footstep-planning-obstacle-model-from-eus-pointcloud (instance pointcloud :init)) (let ((result (plan-footstep-from-goal-coords (make-coords :pos (float-vector 600 -200 0) :rpy (list (deg2rad 45) 0 0)))) (result-footstep (footstep-array->coords result)) (footstep-dif-list (mapcar #'(lambda (fs0 fs1) (let ((tmp (subseq (send (send fs0 :transformation fs1) :worldpos) 0 2))) ;; (float-vector x y)
(if (eq (send fs0 :name) :lleg) tmp (float-vector (elt tmp 0) (- (elt tmp 1)))))) ;; If rleg -> lleg, revert y component
(butlast result-footstep) (cdr result-footstep))) (ret (mapcar #'(lambda (dif) (null (null (member dif successors* :test #'(lambda (x y) (< (distance x y) 1e-1)))))) (butlast footstep-dif-list 2)))) ;; Last 2 elements are finalize footsteps, which does not equal to successors
(warn ";; ret = ~A (~A)~%" (every #'identity ret) ret) (every #'identity ret) )) irteusgl$ (test-init) irteusgl$ (test)

再現コードは、実際に得られたfootstepリストから計算される一歩ごとの差分と、successorとを、比較します。
（これらは一致するはず）

これで、現状は

;; ret = nil ((nil nil nil nil nil nil t))

とでて、全部がtにはならないです。
現状でもHRP2JSKNT_footstep_planner_params.yamlを

lleg_footstep_offset: [0.016411, 0, 0] #こめんとあうと

rleg_footstep_offset: [0.016411, 0, 0] #こめんとあうと

のように更新すると、

;; ret = t ((t t t t t t t))

となって結果がsuccessorと整合とれます。

https://github.com/jsk-ros-pkg/jsk_control/pull/648
を摘要して、上記のコメントアウトなしで動けば大丈夫です。

[snozawa]

Solved via https://github.com/jsk-ros-pkg/jsk_control/pull/648