[WIP][hrpsys_ros_bridge_tutorials] Enable to call :def-limb-controller-method before *ri* initialization by pazeshun · Pull Request #1095 · start-jsk/rtmros_common

pazeshun · 2020-04-24T04:38:27Z

Includes #1096

Problem

Currently, some of EusLisp interface in rtmros_tutorials cannot be initialized with a partial controller (e.g., :rarm-controller):

$ roseus hironxjsk-interface.l 
configuring by "/opt/ros/kinetic/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 
connected to Xserver DISPLAY=:0
X events are being asynchronously monitored.
;; 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 irtpointcloud irtx eusjpeg euspng png irtimage irtglrgb 
;; extending gcstack 0x48eb9d0[16374] --> 0x4d70ad0[32748] top=3d66
irtgl irtglc irtviewer 
EusLisp 9.27( 1.2.1) for Linux64 created on ip-172-30-1-63(Wed Feb 19 14:11:11 UTC 2020)
roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-63 Wed Feb 19 14:11:11 UTC 2020  1.2.1))
eustf roseus_c_util 
;; extending gcstack 0x4d70ad0[32738] --> 0x5c78ee0[65476] top=7ec4
1.irteusgl$ hironxjsk-init :type :rarm-controller
Call Stack (max depth: 20):
  0: at (send self ctype)
  1: at (mapcar #'(lambda (param) (let* ((controller-action (cdr (assoc :controller-action param))) (action-type (cdr (assoc :action-type param))) (action (instance controller-action-client :init self (if namespace (format nil "~A/~A" namespace controller-action) controller-action) action-type :groupname groupname))) (push action tmp-actions))) (send self ctype))
  2: at (cond (create-actions (mapcar #'(lambda (param) (let* ((controller-action (cdr (assoc :controller-action param))) (action-type (cdr (assoc :action-type param))) (action (instance controller-action-client :init self (if namespace (format nil "~A/~A" namespace controller-action) controller-action) action-type :groupname groupname))) (push action tmp-actions))) (send self ctype)) (setq tmp-actions (nreverse tmp-actions)) (dolist (action tmp-actions) (unless controller-timeout (ros::ros-warn "Waiting for actionlib interface forever because controller-timeout is nil")) (unless (and joint-action-enable (send action :wait-for-server controller-timeout)) (ros::ros-warn "~A is not respond, ~A-interface is disabled" action (send robot :name)) (ros::ros-warn "~C[3~CmStarting 'Kinematics Simulator'~C[0m" 27 49 27) (ros::ros-warn "~C[3~Cm (If you do not intend to start Kinematics Simulator, make sure that you can run 'rostopic info /~A/goal' and 'rostopic info /~A/cancel' and check whether Subscribers exists. If there is no Subscribers, please check joint_trajectory_action node.)~C[0m" 27 52 (send action :name) (send action :name) 27) (ros::ros-warn "~C[3~Cm (Please also check 'rostopic info /~A/feedback' and 'rostopic info /~A/result' and check whether Publishers exists. If there is no Publishers, please check joint_trajectory_action node.)~C[0m" 27 52 (send action :name) (send action :name) 27) (ros::ros-warn "~C[3~Cm (If joint_trajectory_action node already exists, you might have a network problem. Please make sure that you can run 'rosnode ping JOINT_TRAJECTORY_ACTION_SERVER_NODE_NAME' and 'rosnode ping ~A')~C[0m" 27 52 (ros::get-name) 27) (when joint-enable-check (setq joint-action-enable nil) (return)))) (dolist (param (send self ctype)) (let* ((controller-state (cdr (assoc :controller-state param))) (key (intern (string-upcase controller-state) *keyword-package*))) (ros::subscribe (if namespace (format nil "~A/~A" namespace controller-state) controller-state) control_msgs::jointtrajectorycontrollerstate #'send self :set-robot-state1 key :groupname groupname)))) (t (mapcar #'(lambda (param) (let* ((controller-action (cdr (assoc :controller-action param))) (action-type (cdr (assoc :action-type param))) (name (if namespace (format nil "~A/~A" namespace controller-action) controller-action)) action) (setq action (find-if #'(lambda (ac) (string= name (send ac :name))) controller-actions)) (if action (push action tmp-actions)))) (send self ctype)) (setq tmp-actions (nreverse tmp-actions))))
  3: at (let (tmp-actions) (cond (create-actions (mapcar #'(lambda (param) (let* ((controller-action (cdr (assoc :controller-action param))) (action-type (cdr (assoc :action-type param))) (action (instance controller-action-client :init self (if namespace (format nil "~A/~A" namespace controller-action) controller-action) action-type :groupname groupname))) (push action tmp-actions))) (send self ctype)) (setq tmp-actions (nreverse tmp-actions)) (dolist (action tmp-actions) (unless controller-timeout (ros::ros-warn "Waiting for actionlib interface forever because controller-timeout is nil")) (unless (and joint-action-enable (send action :wait-for-server controller-timeout)) (ros::ros-warn "~A is not respond, ~A-interface is disabled" action (send robot :name)) (ros::ros-warn "~C[3~CmStarting 'Kinematics Simulator'~C[0m" 27 49 27) (ros::ros-warn "~C[3~Cm (If you do not intend to start Kinematics Simulator, make sure that you can run 'rostopic info /~A/goal' and 'rostopic info /~A/cancel' and check whether Subscribers exists. If there is no Subscribers, please check joint_trajectory_action node.)~C[0m" 27 52 (send action :name) (send action :name) 27) (ros::ros-warn "~C[3~Cm (Please also check 'rostopic info /~A/feedback' and 'rostopic info /~A/result' and check whether Publishers exists. If there is no Publishers, please check joint_trajectory_action node.)~C[0m" 27 52 (send action :name) (send action :name) 27) (ros::ros-warn "~C[3~Cm (If joint_trajectory_action node already exists, you might have a network problem. Please make sure that you can run 'rosnode ping JOINT_TRAJECTORY_ACTION_SERVER_NODE_NAME' and 'rosnode ping ~A')~C[0m" 27 52 (ros::get-name) 27) (when joint-enable-check (setq joint-action-enable nil) (return)))) (dolist (param (send self ctype)) (let* ((controller-state (cdr (assoc :controller-state param))) (key (intern (string-upcase controller-state) *keyword-package*))) (ros::subscribe (if namespace (format nil "~A/~A" namespace controller-state) controller-state) control_msgs::jointtrajectorycontrollerstate #'send self :set-robot-state1 key :groupname groupname)))) (t (mapcar #'(lambda (param) (let* ((controller-action (cdr (assoc :controller-action param))) (action-type (cdr (assoc :action-type param))) (name (if namespace (format nil "~A/~A" namespace controller-action) controller-action)) action) (setq action (find-if #'(lambda (ac) (string= name (send ac :name))) controller-actions)) (if action (push action tmp-actions)))) (send self ctype)) (setq tmp-actions (nreverse tmp-actions)))) (setf (gethash ctype controller-table) tmp-actions) tmp-actions)
  4: at (send self :add-controller controller-type :joint-enable-check t :create-actions t)
  5: at (setq controller-actions (send self :add-controller controller-type :joint-enable-check t :create-actions t))
  6: at (apply #'send-message self (class . super) :init args)
  7: at (apply #'send-message self (class . super) :init args)
  8: at (apply #'send-message self (class . super) :init args)
  9: at (send-super* :init args)
  10: at (let ((#:prog12724 (send-super* :init args))) (progn (send self :define-all-rosbridge-srv-methods) (ros::subscribe "/motor_states" hrpsys_ros_bridge::motorstates #'send self :rtmros-motor-states-callback :groupname groupname) (mapcar #'(lambda (x) (ros::subscribe (format nil "/~A" (string-downcase x)) geometry_msgs::wrenchstamped #'send self :rtmros-force-sensor-callback x :groupname groupname) (ros::subscribe (format nil "/off_~A" (string-downcase x)) geometry_msgs::wrenchstamped #'send self :rtmros-force-sensor-callback (read-from-string (format nil ":off-~A" (string-downcase x))) :groupname groupname) (ros::subscribe (format nil "/ref_~A" (string-downcase x)) geometry_msgs::wrenchstamped #'send self :rtmros-force-sensor-callback (read-from-string (format nil ":reference-~A" (string-downcase x))) :groupname groupname)) (send-all (send robot :force-sensors) :name)) (ros::subscribe "/zmp" geometry_msgs::pointstamped #'send self :rtmros-zmp-callback :groupname groupname) (ros::subscribe "/imu" sensor_msgs::imu #'send self :rtmros-imu-callback :groupname groupname) (ros::subscribe "/emergency_mode" std_msgs::int32 #'send self :rtmros-emergency-mode-callback :groupname groupname) (mapcar #'(lambda (x) (ros::subscribe (format nil "/~A_capture_point" x) geometry_msgs::pointstamped #'send self :rtmros-capture-point-callback (read-from-string (format nil ":~A-capture-point" x)) :groupname groupname)) '(ref act)) (mapcar #'(lambda (x) (ros::subscribe (format nil "/~A_contact_states" x) hrpsys_ros_bridge::contactstatesstamped #'send self :rtmros-contact-states-callback (read-from-string (format nil ":~A-contact-states" x)) :groupname groupname)) '(ref act))) #:prog12724)
  11: at (prog1 (send-super* :init args) (send self :define-all-rosbridge-srv-methods) (ros::subscribe "/motor_states" hrpsys_ros_bridge::motorstates #'send self :rtmros-motor-states-callback :groupname groupname) (mapcar #'(lambda (x) (ros::subscribe (format nil "/~A" (string-downcase x)) geometry_msgs::wrenchstamped #'send self :rtmros-force-sensor-callback x :groupname groupname) (ros::subscribe (format nil "/off_~A" (string-downcase x)) geometry_msgs::wrenchstamped #'send self :rtmros-force-sensor-callback (read-from-string (format nil ":off-~A" (string-downcase x))) :groupname groupname) (ros::subscribe (format nil "/ref_~A" (string-downcase x)) geometry_msgs::wrenchstamped #'send self :rtmros-force-sensor-callback (read-from-string (format nil ":reference-~A" (string-downcase x))) :groupname groupname)) (send-all (send robot :force-sensors) :name)) (ros::subscribe "/zmp" geometry_msgs::pointstamped #'send self :rtmros-zmp-callback :groupname groupname) (ros::subscribe "/imu" sensor_msgs::imu #'send self :rtmros-imu-callback :groupname groupname) (ros::subscribe "/emergency_mode" std_msgs::int32 #'send self :rtmros-emergency-mode-callback :groupname groupname) (mapcar #'(lambda (x) (ros::subscribe (format nil "/~A_capture_point" x) geometry_msgs::pointstamped #'send self :rtmros-capture-point-callback (read-from-string (format nil ":~A-capture-point" x)) :groupname groupname)) '(ref act)) (mapcar #'(lambda (x) (ros::subscribe (format nil "/~A_contact_states" x) hrpsys_ros_bridge::contactstatesstamped #'send self :rtmros-contact-states-callback (read-from-string (format nil ":~A-contact-states" x)) :groupname groupname)) '(ref act)))
  12: at (apply #'send-message self (class . super) :init :joint-states-queue-size 2 :robot hironxjsk-robot args)
  13: at (apply #'send-message self (class . super) :init :joint-states-queue-size 2 :robot hironxjsk-robot args)
  14: at (apply #'send-message self (class . super) :init :joint-states-queue-size 2 :robot hironxjsk-robot args)
  15: at (send-super* :init :joint-states-queue-size 2 :robot hironxjsk-robot args)
  16: at (let ((#:prog12723 (send-super* :init :joint-states-queue-size 2 :robot hironxjsk-robot args))) (progn (dolist (limb '(:rarm :larm :head :torso)) (send self :def-limb-controller-method limb) (send self :add-controller (read-from-string (format nil "~A-controller" limb)) :joint-enable-check t :create-actions t)) (setq hand-servo-num 4)) #:prog12723)
  17: at (prog1 (send-super* :init :joint-states-queue-size 2 :robot hironxjsk-robot args) (dolist (limb '(:rarm :larm :head :torso)) (send self :def-limb-controller-method limb) (send self :add-controller (read-from-string (format nil "~A-controller" limb)) :joint-enable-check t :create-actions t)) (setq hand-servo-num 4))
  18: at (apply #'send #:inst2722 :init args)
  19: at (apply #'send #:inst2722 :init args)
  And more...
/opt/ros/kinetic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: cannot find method :rarm-controller in (send self ctype)

This is because partial controllers are not defined before *ri* initialization.
We usually use :def-limb-controller-method to define partial controllers (e.g., here and here), but :def-limb-controller-method cannot be used before *ri* initialization because it uses robot slot of *ri*:
https://github.com/start-jsk/rtmros_common/blob/1.4.3/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l#L317

$ roseus hironxjsk-interface.l 
configuring by "/opt/ros/kinetic/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 
connected to Xserver DISPLAY=:0
X events are being asynchronously monitored.
;; 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 irtpointcloud irtx eusjpeg euspng png irtimage irtglrgb 
;; extending gcstack 0x60269d0[16374] --> 0x64abac0[32748] top=3d66
irtgl irtglc irtviewer 
EusLisp 9.27( 1.2.1) for Linux64 created on ip-172-30-1-63(Wed Feb 19 14:11:11 UTC 2020)
roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-63 Wed Feb 19 14:11:11 UTC 2020  1.2.1))
eustf roseus_c_util 
;; extending gcstack 0x64abac0[32738] --> 0x73b3ed0[65476] top=7ec4
1.irteusgl$ (instance hironxjsk-interface :def-limb-controller-method :rarm)
Call Stack (max depth: 20):
  0: at (send robot limb :joint-list)
  1: at (send-all (send robot limb :joint-list) :name)
  2: at (append (send-all (send robot limb :joint-list) :name) nil)
  3: at (cons 'list (append (send-all (send robot limb :joint-list) :name) nil))
  4: at (list (cons 'list (append (send-all (send robot limb :joint-list) :name) nil)))
  5: at (cons ':joint-names (list (cons 'list (append (send-all (send robot limb :joint-list) :name) nil))))
  6: at (cons 'cons (cons ':joint-names (list (cons 'list (append (send-all (send robot limb :joint-list) :name) nil)))))
  7: at (list (cons 'cons (cons ':joint-names (list (cons 'list (append (send-all (send robot limb :joint-list) :name) nil))))))
  8: at (cons (cons 'cons (cons ':action-type (list 'control_msgs::followjointtrajectoryaction))) (list (cons 'cons (cons ':joint-names (list (cons 'list (append (send-all (send robot limb :joint-list) :name) nil)))))))
  9: at (cons (cons 'cons (cons ':controller-state (list (format nil "~A_controller/state" (string-downcase limb))))) (cons (cons 'cons (cons ':action-type (list 'control_msgs::followjointtrajectoryaction))) (list (cons 'cons (cons ':joint-names (list (cons 'list (append (send-all (send robot limb :joint-list) :name) nil))))))))
  10: at (cons (cons 'cons (cons ':controller-action (list (format nil "~A_controller/follow_joint_trajectory_action" (string-downcase limb))))) (cons (cons 'cons (cons ':controller-state (list (format nil "~A_controller/state" (string-downcase limb))))) (cons (cons 'cons (cons ':action-type (list 'control_msgs::followjointtrajectoryaction))) (list (cons 'cons (cons ':joint-names (list (cons 'list (append (send-all (send robot limb :joint-list) :name) nil)))))))))
  11: at (cons (cons 'cons (cons ':group-name (list (string-downcase limb)))) (cons (cons 'cons (cons ':controller-action (list (format nil "~A_controller/follow_joint_trajectory_action" (string-downcase limb))))) (cons (cons 'cons (cons ':controller-state (list (format nil "~A_controller/state" (string-downcase limb))))) (cons (cons 'cons (cons ':action-type (list 'control_msgs::followjointtrajectoryaction))) (list (cons 'cons (cons ':joint-names (list (cons 'list (append (send-all (send robot limb :joint-list) :name) nil))))))))))
  12: at (cons 'list (cons (cons 'cons (cons ':group-name (list (string-downcase limb)))) (cons (cons 'cons (cons ':controller-action (list (format nil "~A_controller/follow_joint_trajectory_action" (string-downcase limb))))) (cons (cons 'cons (cons ':controller-state (list (format nil "~A_controller/state" (string-downcase limb))))) (cons (cons 'cons (cons ':action-type (list 'control_msgs::followjointtrajectoryaction))) (list (cons 'cons (cons ':joint-names (list (cons 'list (append (send-all (send robot limb :joint-list) :name) nil)))))))))))
  13: at (list (cons 'list (cons (cons 'cons (cons ':group-name (list (string-downcase limb)))) (cons (cons 'cons (cons ':controller-action (list (format nil "~A_controller/follow_joint_trajectory_action" (string-downcase limb))))) (cons (cons 'cons (cons ':controller-state (list (format nil "~A_controller/state" (string-downcase limb))))) (cons (cons 'cons (cons ':action-type (list 'control_msgs::followjointtrajectoryaction))) (list (cons 'cons (cons ':joint-names (list (cons 'list (append (send-all (send robot limb :joint-list) :name) nil))))))))))))
  14: at (cons 'list (list (cons 'list (cons (cons 'cons (cons ':group-name (list (string-downcase limb)))) (cons (cons 'cons (cons ':controller-action (list (format nil "~A_controller/follow_joint_trajectory_action" (string-downcase limb))))) (cons (cons 'cons (cons ':controller-state (list (format nil "~A_controller/state" (string-downcase limb))))) (cons (cons 'cons (cons ':action-type (list 'control_msgs::followjointtrajectoryaction))) (list (cons 'cons (cons ':joint-names (list (cons 'list (append (send-all (send robot limb :joint-list) :name) nil)))))))))))))
  15: at (list (cons 'list (list (cons 'list (cons (cons 'cons (cons ':group-name (list (string-downcase limb)))) (cons (cons 'cons (cons ':controller-action (list (format nil "~A_controller/follow_joint_trajectory_action" (string-downcase limb))))) (cons (cons 'cons (cons ':controller-state (list (format nil "~A_controller/state" (string-downcase limb))))) (cons (cons 'cons (cons ':action-type (list 'control_msgs::followjointtrajectoryaction))) (list (cons 'cons (cons ':joint-names (list (cons 'list (append (send-all (send robot limb :joint-list) :name) nil))))))))))))))
  16: at (cons 'nil (list (cons 'list (list (cons 'list (cons (cons 'cons (cons ':group-name (list (string-downcase limb)))) (cons (cons 'cons (cons ':controller-action (list (format nil "~A_controller/follow_joint_trajectory_action" (string-downcase limb))))) (cons (cons 'cons (cons ':controller-state (list (format nil "~A_controller/state" (string-downcase limb))))) (cons (cons 'cons (cons ':action-type (list 'control_msgs::followjointtrajectoryaction))) (list (cons 'cons (cons ':joint-names (list (cons 'list (append (send-all (send robot limb :joint-list) :name) nil)))))))))))))))
  17: at (cons (read-from-string (format nil "~A-controller" limb)) (cons 'nil (list (cons 'list (list (cons 'list (cons (cons 'cons (cons ':group-name (list (string-downcase limb)))) (cons (cons 'cons (cons ':controller-action (list (format nil "~A_controller/follow_joint_trajectory_action" (string-downcase limb))))) (cons (cons 'cons (cons ':controller-state (list (format nil "~A_controller/state" (string-downcase limb))))) (cons (cons 'cons (cons ':action-type (list 'control_msgs::followjointtrajectoryaction))) (list (cons 'cons (cons ':joint-names (list (cons 'list (append (send-all (send robot limb :joint-list) :name) nil))))))))))))))))
  18: at (list (cons (read-from-string (format nil "~A-controller" limb)) (cons 'nil (list (cons 'list (list (cons 'list (cons (cons 'cons (cons ':group-name (list (string-downcase limb)))) (cons (cons 'cons (cons ':controller-action (list (format nil "~A_controller/follow_joint_trajectory_action" (string-downcase limb))))) (cons (cons 'cons (cons ':controller-state (list (format nil "~A_controller/state" (string-downcase limb))))) (cons (cons 'cons (cons ':action-type (list 'control_msgs::followjointtrajectoryaction))) (list (cons 'cons (cons ':joint-names (list (cons 'list (append (send-all (send robot limb :joint-list) :name) nil)))))))))))))))))
  19: at (cons (send (class self) :name) (list (cons (read-from-string (format nil "~A-controller" limb)) (cons 'nil (list (cons 'list (list (cons 'list (cons (cons 'cons (cons ':group-name (list (string-downcase limb)))) (cons (cons 'cons (cons ':controller-action (list (format nil "~A_controller/follow_joint_trajectory_action" (string-downcase limb))))) (cons (cons 'cons (cons ':controller-state (list (format nil "~A_controller/state" (string-downcase limb))))) (cons (cons 'cons (cons ':action-type (list 'control_msgs::followjointtrajectoryaction))) (list (cons 'cons (cons ':joint-names (list (cons 'list (append (send-all (send robot limb :joint-list) :name) nil))))))))))))))))))
  And more...
/opt/ros/kinetic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: cannot find method :rarm in (send robot limb :joint-list)

Solution

This PR fixes the above problem by avoiding evalution of robot when :def-limb-controller-method is called.

…hod before *ri* initialization

Naoki-Hiraoka · 2020-04-24T05:33:00Z

Good!

If my understanding is collect,
After this PR, conventional code will still work.
And after this PR, the problem mentioned above will be solved permanently by removing this line and inserting the following lines here.

(dolist (limb '(:rarm :larm :rleg :lleg :head :torso))
  (instance hrp2jsknts-interface :def-limb-controller-method limb))

pazeshun · 2020-04-24T05:50:57Z

@Naoki-Hiraoka

After this PR, conventional code will still work.

Yes, you are right.

And after this PR, the problem mentioned above will be solved permanently by removing this line and inserting the following lines here.
(dolist (limb '(:rarm :larm :rleg :lleg :head :torso))
  (instance hrp2jsknts-interface :def-limb-controller-method limb))

This may not work, because I think (instance hrp2jsknts-interface :def-limb-controller-method limb) creates a new instance of hrp2jsknts-interface and calls :def-limb-controller-method of that instance.
In your dolist, instance of hrp2jsknts-interface is temporarily created and lost at each time.

Why I used instance in test code is that using instance seems the easiest way to call :def-limb-controller-method without *ri* initialization, and that code is only for test.
I think adding the following lines just after here may work.

(dolist (limb '(:rarm :larm :rleg :lleg :head :torso))
  (send self :def-limb-controller-method limb))

Perhaps introducing let ((limbs '(:rarm :larm :rleg :lleg :head :torso))) is better, because that list will be used twice.

Naoki-Hiraoka · 2020-04-24T06:11:39Z

And after this PR, the problem mentioned above will be solved permanently by removing this line and inserting the following lines here.
(dolist (limb '(:rarm :larm :rleg :lleg :head :torso))
(instance hrp2jsknts-interface :def-limb-controller-method limb))

I think this works.

$ roseus ./hrp2jsknts-interface.l 
1.irteusgl$ load "/tmp/rtmros_common/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l "
t
2.irteusgl$ setq *ri* (instantiate hrp2jsknts-interface)
#<hrp2jsknts-interface #X7ceedd8>
3.irteusgl$ send *ri* :methods :rleg-controller
nil
4.irteusgl$ (dolist (limb '(:rarm :larm :rleg :lleg :head :torso))
  (instance hrp2jsknts-interface :def-limb-controller-method limb))
nil
5.irteusgl$ send *ri* :methods :rleg-controller
(:rleg-controller)

With this method, :def-limb-controller-method is called once(6 times) every time the file is loaded, instead of every time hrp2jsknts-init is called.

pazeshun · 2020-04-24T06:15:30Z

I see, thank you for your information!

YoheiKakiuchi · 2020-04-30T02:00:22Z

I don't understand why you want to use :def-limb-controller-method before *ri* initialization

pazeshun · 2020-04-30T03:03:35Z

@YoheiKakiuchi
In particular, I want to use :def-limb-controller-method before *ri* initialization for shortening EusLisp code in Gazebo of HIRONXJSK/NEXTAGE (start-jsk/rtmros_tutorials#572, start-jsk/rtmros_tutorials#574).
In that Gazebo, we use ros_control instead of hrpsys for easy implementation (tork-a/rtmros_nextage#255 (comment)).
Using ros_control, we cannot easily prepare fullbody_controller, because multiple controllers cannot be assigned to one joint (e.g., rarm_controller and fullbody_controller to RARM_JOINT0).
In that situation, we need to define each controller before *ri* initialization:
https://github.com/start-jsk/rtmros_tutorials/pull/572/files#diff-5291d4594d73a226423e86ee44d2c58fR10-R43
I know this is the same as PR2 case, but very small modification to :def-limb-controller-method in this PR enables that method to be used before *ri* initialization and shorten controller definition.

pazeshun · 2020-04-30T03:27:35Z

But I know this is too robot-specific reason, so in the top comment, I talked about a merit for all rtmros robots.
When users who trained with other robots (PR2, Baxter, Fetch...) want to do #545 , they should firstly try to initialize *ri* with a partial controller (e.g., :head-controller), because adding :head-controller to every :angle-vector is a little bit painful.

YoheiKakiuchi · 2020-04-30T04:11:16Z

I understand you want to use :default-controller which is created by appending methods defined by :def-limb-controller-method.

Very simple solution, you can create robot model before calling :def-limb-controller-method(it can be called before initialization) .

pazeshun · 2020-04-30T06:30:07Z

Very simple solution, you can create robot model before calling :def-limb-controller-method(it can be called before initialization) .

Yes, you are right, I already tried that solution and confirmed it works.
However, I feel this is just a temporary solution because instantiating robot model occurs twice. That is useless.

I thought required modification is very small and the modified :def-limb-controller-method seems to help someone who wants to initialize *ri* with a partial controller, so I made this PR.
If maintainers think this modification is big or encourage to use (send *ri* :angle-vector hoge 3000 :head-controller) every time in #545 case, this PR is useless.

default-controller returens list of possible controllers: https://github.com/jsk-ros-pkg/jsk_robot/blob/c161b6a87fa00a29909833bb1250c5ee96e42de6/jsk_baxter_robot/baxtereus/baxter-interface.l#L59-L63

pazeshun added 2 commits April 24, 2020 13:08

[hrpsys_ros_bridge] Add test of :def-limb-controller-method

fdf527e

[hrpsys_ros_bridge_tutorials] Enable to call :def-limb-controller-met…

84af78c

…hod before *ri* initialization

pazeshun mentioned this pull request Apr 24, 2020

[hrpsys_ros_bridge] Add test of :def-limb-controller-method #1096

Open

pazeshun changed the title ~~[hrpsys_ros_bridge_tutorials] Enable to call :def-limb-controller-method before *ri* initialization~~ [WIP][hrpsys_ros_bridge_tutorials] Enable to call :def-limb-controller-method before *ri* initialization Apr 24, 2020

pazeshun mentioned this pull request May 4, 2020

do not use https://github.com/start-jsk/rtmros_common/pull/1095 pazeshun/rtmros_tutorials#4

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[WIP][hrpsys_ros_bridge_tutorials] Enable to call :def-limb-controller-method before ri initialization#1095

[WIP][hrpsys_ros_bridge_tutorials] Enable to call :def-limb-controller-method before ri initialization#1095
pazeshun wants to merge 2 commits intostart-jsk:masterfrom
pazeshun:def-limb-controller-method-before-init

pazeshun commented Apr 24, 2020 •

edited

Loading

Uh oh!

Naoki-Hiraoka commented Apr 24, 2020

Uh oh!

pazeshun commented Apr 24, 2020

Uh oh!

Naoki-Hiraoka commented Apr 24, 2020 •

edited

Loading

Uh oh!

pazeshun commented Apr 24, 2020

Uh oh!

YoheiKakiuchi commented Apr 30, 2020

Uh oh!

pazeshun commented Apr 30, 2020

Uh oh!

pazeshun commented Apr 30, 2020

Uh oh!

YoheiKakiuchi commented Apr 30, 2020 •

edited

Loading

Uh oh!

pazeshun commented Apr 30, 2020

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

Conversation

pazeshun commented Apr 24, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Problem

Solution

Uh oh!

Naoki-Hiraoka commented Apr 24, 2020

Uh oh!

pazeshun commented Apr 24, 2020

Uh oh!

Naoki-Hiraoka commented Apr 24, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

pazeshun commented Apr 24, 2020

Uh oh!

YoheiKakiuchi commented Apr 30, 2020

Uh oh!

pazeshun commented Apr 30, 2020

Uh oh!

pazeshun commented Apr 30, 2020

Uh oh!

YoheiKakiuchi commented Apr 30, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

pazeshun commented Apr 30, 2020

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

pazeshun commented Apr 24, 2020 •

edited

Loading

Naoki-Hiraoka commented Apr 24, 2020 •

edited

Loading

YoheiKakiuchi commented Apr 30, 2020 •

edited

Loading