jsk-ros-pkg / jsk_robot

jsk-ros-pkg/jsk_robot
https://github.com/jsk-ros-pkg/jsk_robot
73 stars 97 forks source link

[naoqieus] robot-interfaceのみで使われているトピックにも,group nameを追加したいです #1012

Open kochigami opened 5 years ago

kochigami commented 5 years ago

一つのパソコンで複数台ロボットを動かすために, pepper-interfaceやnao-interfaceで作られるインスタンスが扱うトピックに,group nameをつけたいと考えています.

pepper(nao)-interfacenaoqi-interfaceを継承しており, naoqi-interfacerobot-interfaceを継承しています.

robot-interfaceのみで使われているトピックにも,group nameを追加したいのですが,その方法が分かりません.

分からないことが2つあります: 1,/joint_states:robot-interfaceでパブリッシュしているが,namespaceの指定の方法が分からない. 2./tf, /tf_static,dummy_state, /robot_interface_marker_array: 多分robot-interfaceで扱っていると思うのですが,robot-interface.lの中を見ても,見つけられない.

1については, https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/robot-interface.l#L125 https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/robot-interface.l#L175 を見て, naoqi-interface.lの21行目に, (send-super* :init :robot robot :type type :groupname "naoqi_interface" :namespace gns args)というように,:namespace gns を追加しました.

すると,以下のエラーが出ました.

roscore
roseus pepper-interface.l
pepper-init t "robot1" ; "robot1"はgroup name
/opt/ros/kinetic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: cannot find method :pop-angle-vector-simulation in (send action-client :pop-angle-vector-simulation)
2.E127-irteusgl$ Call Stack (max depth: 20):
  0: at (send action-client :pop-angle-vector-simulation)
  1: at (let* ((joint-names (cdr (assoc :joint-names param))) (action-name (cdr (assoc :controller-action param))) (action-client (find action-name controller-actions :key #'(lambda (x) (send x :name)) :test #'string=)) (av (send action-client :pop-angle-vector-simulation))) (when av (dolist (j joint-names) (if (find j all-joint-names :test #'string=) (let ((i (position j all-joint-names :test #'string=))) (send (elt joint-list i) :joint-angle (elt av i)))))))
  2: at (while #:dolist11544 (setq param (pop #:dolist11544)) (let* ((joint-names (cdr (assoc :joint-names param))) (action-name (cdr (assoc :controller-action param))) (action-client (find action-name controller-actions :key #'(lambda (x) (send x :name)) :test #'string=)) (av (send action-client :pop-angle-vector-simulation))) (when av (dolist (j joint-names) (if (find j all-joint-names :test #'string=) (let ((i (position j all-joint-names :test #'string=))) (send (elt joint-list i) :joint-angle (elt av i))))))))
  3: at (let ((param nil) (#:dolist11544 (send self controller-type))) nil (while #:dolist11544 (setq param (pop #:dolist11544)) (let* ((joint-names (cdr (assoc :joint-names param))) (action-name (cdr (assoc :controller-action param))) (action-client (find action-name controller-actions :key #'(lambda (x) (send x :name)) :test #'string=)) (av (send action-client :pop-angle-vector-simulation))) (when av (dolist (j joint-names) (if (find j all-joint-names :test #'string=) (let ((i (position j all-joint-names :test #'string=))) (send (elt joint-list i) :joint-angle (elt av i)))))))) nil)
  4: at (dolist (param (send self controller-type)) (let* ((joint-names (cdr (assoc :joint-names param))) (action-name (cdr (assoc :controller-action param))) (action-client (find action-name controller-actions :key #'(lambda (x) (send x :name)) :test #'string=)) (av (send action-client :pop-angle-vector-simulation))) (when av (dolist (j joint-names) (if (find j all-joint-names :test #'string=) (let ((i (position j all-joint-names :test #'string=))) (send (elt joint-list i) :joint-angle (elt av i))))))))
  5: at (let* ((joint-list (send robot :joint-list)) (all-joint-names (send-all joint-list :name))) (send self :publish-joint-state) (dolist (param (send self controller-type)) (let* ((joint-names (cdr (assoc :joint-names param))) (action-name (cdr (assoc :controller-action param))) (action-client (find action-name controller-actions :key #'(lambda (x) (send x :name)) :test #'string=)) (av (send action-client :pop-angle-vector-simulation))) (when av (dolist (j joint-names) (if (find j all-joint-names :test #'string=) (let ((i (position j all-joint-names :test #'string=))) (send (elt joint-list i) :joint-angle (elt av i)))))))) (if viewer (send self :draw-objects)))
  6: at (send #<pepper-interface #X6cdc798> :robot-interface-simulation-callback)
  7: at euserror
  8: at euserror
  9: at (send action-client :pop-angle-vector-simulation)
  10: at (let* ((joint-names (cdr (assoc :joint-names param))) (action-name (cdr (assoc :controller-action param))) (action-client (find action-name controller-actions :key #'(lambda (x) (send x :name)) :test #'string=)) (av (send action-client :pop-angle-vector-simulation))) (when av (dolist (j joint-names) (if (find j all-joint-names :test #'string=) (let ((i (position j all-joint-names :test #'string=))) (send (elt joint-list i) :joint-angle (elt av i)))))))
  11: at (while #:dolist11502 (setq param (pop #:dolist11502)) (let* ((joint-names (cdr (assoc :joint-names param))) (action-name (cdr (assoc :controller-action param))) (action-client (find action-name controller-actions :key #'(lambda (x) (send x :name)) :test #'string=)) (av (send action-client :pop-angle-vector-simulation))) (when av (dolist (j joint-names) (if (find j all-joint-names :test #'string=) (let ((i (position j all-joint-names :test #'string=))) (send (elt joint-list i) :joint-angle (elt av i))))))))
  12: at (let ((param nil) (#:dolist11502 (send self controller-type))) nil (while #:dolist11502 (setq param (pop #:dolist11502)) (let* ((joint-names (cdr (assoc :joint-names param))) (action-name (cdr (assoc :controller-action param))) (action-client (find action-name controller-actions :key #'(lambda (x) (send x :name)) :test #'string=)) (av (send action-client :pop-angle-vector-simulation))) (when av (dolist (j joint-names) (if (find j all-joint-names :test #'string=) (let ((i (position j all-joint-names :test #'string=))) (send (elt joint-list i) :joint-angle (elt av i)))))))) nil)
  13: at (dolist (param (send self controller-type)) (let* ((joint-names (cdr (assoc :joint-names param))) (action-name (cdr (assoc :controller-action param))) (action-client (find action-name controller-actions :key #'(lambda (x) (send x :name)) :test #'string=)) (av (send action-client :pop-angle-vector-simulation))) (when av (dolist (j joint-names) (if (find j all-joint-names :test #'string=) (let ((i (position j all-joint-names :test #'string=))) (send (elt joint-list i) :joint-angle (elt av i))))))))
  14: at (let* ((joint-list (send robot :joint-list)) (all-joint-names (send-all joint-list :name))) (send self :publish-joint-state) (dolist (param (send self controller-type)) (let* ((joint-names (cdr (assoc :joint-names param))) (action-name (cdr (assoc :controller-action param))) (action-client (find action-name controller-actions :key #'(lambda (x) (send x :name)) :test #'string=)) (av (send action-client :pop-angle-vector-simulation))) (when av (dolist (j joint-names) (if (find j all-joint-names :test #'string=) (let ((i (position j all-joint-names :test #'string=))) (send (elt joint-list i) :joint-angle (elt av i)))))))) (if viewer (send self :draw-objects)))
  15: at (send #<pepper-interface #X6cdc798> :robot-interface-simulation-callback)
  16: at euserror
  17: at euserror
  18: at (send action-client :pop-angle-vector-simulation)
  19: at (let* ((joint-names (cdr (assoc :joint-names param))) (action-name (cdr (assoc :controller-action param))) (action-client (find action-name controller-actions :key #'(lambda (x) (send x :name)) :test #'string=)) (av (send action-client :pop-angle-vector-simulation))) (when av (dolist (j joint-names) (if (find j all-joint-names :test #'string=) (let ((i (position j all-joint-names :test #'string=))) (send (elt joint-list i) :joint-angle (elt av i)))))))
  And more...
rostopic list 
/pepper_dcm/LeftHand_controller/command
/pepper_dcm/RightHand_controller/command
/robot/cmd_vel
/robot/dummy_state
/robot/joint_states
/robot/move_base_simple/goal
/robot/pepper_robot/pose/joint_angles
/robot/pepper_robot/pose/joint_stiffness_trajectory/cancel
/robot/pepper_robot/pose/joint_stiffness_trajectory/feedback
/robot/pepper_robot/pose/joint_stiffness_trajectory/goal
/robot/pepper_robot/pose/joint_stiffness_trajectory/result
/robot/pepper_robot/pose/joint_stiffness_trajectory/status
/robot/robot/pepper_robot/pose/joint_trajectory/cancel
/robot/robot/pepper_robot/pose/joint_trajectory/feedback
/robot/robot/pepper_robot/pose/joint_trajectory/goal
/robot/robot/pepper_robot/pose/joint_trajectory/result
/robot/robot/pepper_robot/pose/joint_trajectory/status
/robot/speech
/robot_interface_marker_array
/rosout
/rosout_agg
/tf
/tf_static

; メモ: ところどころ/robot/robotとなっているのは,naoqi-interfaceでもgroup-nameを追加しているため. 

https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/330 も関連している話なのかなと思いました. もしそうであれば,robot-interfaceで扱っているトピックにgroup nameを追加するのは諦めて,READMEにもそのように書いておこうかなと思います.


確認方法 ubuntu 16.04

kanae@kanae-ThinkPad-T480s:~$ sudo apt-cache policy ros-kinetic-pr2eus
[sudo] password for kanae: 
ros-kinetic-pr2eus:
  Installed: 0.3.10-0xenial-20181117-022850-0800
  Candidate: 0.3.10-0xenial-20181117-022850-0800
  Version table:
 *** 0.3.10-0xenial-20181117-022850-0800 500
        500 http://packages.ros.org/ros/ubuntu xenial/main amd64 Packages
        100 /var/lib/dpkg/status

922 の変更(naoqi, pepper, nao-inerface.l)を使って,

roscore
roseus pepper-interface.l
pepper-init t "robot1"

とすると,

rostopic list

/dummy_state
/joint_states
/pepper_dcm/LeftHand_controller/command ;; gazebo用なので今回は無視
/pepper_dcm/RightHand_controller/command ;; gazebo用なので今回は無視
/robot1/cmd_vel
/robot1/move_base_simple/goal
/robot1/pepper_robot/pose/joint_angles
/robot1/pepper_robot/pose/joint_stiffness_trajectory/cancel
/robot1/pepper_robot/pose/joint_stiffness_trajectory/feedback
/robot1/pepper_robot/pose/joint_stiffness_trajectory/goal
/robot1/pepper_robot/pose/joint_stiffness_trajectory/result
/robot1/pepper_robot/pose/joint_stiffness_trajectory/status
/robot1/pepper_robot/pose/joint_trajectory/cancel
/robot1/pepper_robot/pose/joint_trajectory/feedback
/robot1/pepper_robot/pose/joint_trajectory/goal
/robot1/pepper_robot/pose/joint_trajectory/result
/robot1/pepper_robot/pose/joint_trajectory/status
/robot1/speech
/robot_interface_marker_array
/rosout
/rosout_agg
/tf
/tf_static

となります.

kochigami commented 5 years ago
roscore
ROS_NAMESPACE="robot1" roseus pepper-interface.l
pepper-init t

naoqieus master

rostopic list
/animated_speech
/cmd_vel
/move_base_simple/goal
/robot1/dummy_state
/robot1/joint_states
/robot1/pepper_dcm/LeftHand_controller/command
/robot1/pepper_dcm/RightHand_controller/command
/robot1/pepper_robot/pose/joint_angles
/robot1/pepper_robot/pose/joint_stiffness_trajectory/cancel
/robot1/pepper_robot/pose/joint_stiffness_trajectory/feedback
/robot1/pepper_robot/pose/joint_stiffness_trajectory/goal
/robot1/pepper_robot/pose/joint_stiffness_trajectory/result
/robot1/pepper_robot/pose/joint_stiffness_trajectory/status
/robot1/pepper_robot/pose/joint_trajectory/cancel
/robot1/pepper_robot/pose/joint_trajectory/feedback
/robot1/pepper_robot/pose/joint_trajectory/goal
/robot1/pepper_robot/pose/joint_trajectory/result
/robot1/pepper_robot/pose/joint_trajectory/status
/robot1/robot_interface_marker_array
/rosout
/rosout_agg
/speech
/tf
/tf_static

naoqieus allow-set-group-ns-to-naoqi-interface

rostopic list
/cmd_vel
/move_base_simple/goal
/pepper_robot/pose/joint_angles
/pepper_robot/pose/joint_stiffness_trajectory/cancel
/pepper_robot/pose/joint_stiffness_trajectory/feedback
/pepper_robot/pose/joint_stiffness_trajectory/goal
/pepper_robot/pose/joint_stiffness_trajectory/result
/pepper_robot/pose/joint_stiffness_trajectory/status
/pepper_robot/pose/joint_trajectory/cancel
/pepper_robot/pose/joint_trajectory/feedback
/pepper_robot/pose/joint_trajectory/goal
/pepper_robot/pose/joint_trajectory/result
/pepper_robot/pose/joint_trajectory/status
/robot1/dummy_state
/robot1/joint_states
/robot1/pepper_dcm/LeftHand_controller/command
/robot1/pepper_dcm/RightHand_controller/command
/robot1/robot_interface_marker_array
/rosout
/rosout_agg
/speech
/tf
/tf_static

roscore
ROS_NAMESPACE="robot1" roseus pepper-interface.l
pepper-init t "robot1"

naoqieus allow-set-group-ns-to-naoqi-interface

rostopic list
/robot1/dummy_state
/robot1/joint_states
/robot1/pepper_dcm/LeftHand_controller/command
/robot1/pepper_dcm/RightHand_controller/command
/robot1/robot1/cmd_vel
/robot1/robot1/move_base_simple/goal
/robot1/robot1/pepper_robot/pose/joint_angles
/robot1/robot1/pepper_robot/pose/joint_stiffness_trajectory/cancel
/robot1/robot1/pepper_robot/pose/joint_stiffness_trajectory/feedback
/robot1/robot1/pepper_robot/pose/joint_stiffness_trajectory/goal
/robot1/robot1/pepper_robot/pose/joint_stiffness_trajectory/result
/robot1/robot1/pepper_robot/pose/joint_stiffness_trajectory/status
/robot1/robot1/pepper_robot/pose/joint_trajectory/cancel
/robot1/robot1/pepper_robot/pose/joint_trajectory/feedback
/robot1/robot1/pepper_robot/pose/joint_trajectory/goal
/robot1/robot1/pepper_robot/pose/joint_trajectory/result
/robot1/robot1/pepper_robot/pose/joint_trajectory/status
/robot1/robot1/speech
/robot1/robot_interface_marker_array
/rosout
/rosout_agg
/tf
/tf_static