Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Test both pr1012 and pr1040 #18

Open
wants to merge 9 commits into
base: wide_stereo_camera_info
Choose a base branch
from
Open

Test both pr1012 and pr1040 #18

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
08b8d74
update pr10*_urdf.yaml for pretty format, for make it easy to compare…
k-okada Dec 15, 2019
eb199fe
generate with euscollada 0.4.3 (with https://github.com/jsk-ros-pkg/j…
k-okada Dec 17, 2019
5ea45a5
Update pr2 model file from pr1040 urdf and camera_info
YutoUchimi Oct 18, 2019
af757ba
fix make-pr2-model-file.l to support multiple robot
k-okada Dec 17, 2019
b9cea6a
update pr2.l for both pr1012/pr1040
k-okada Dec 17, 2019
67f1627
add launch file to generate pr2.l
k-okada Dec 19, 2019
b891ba4
use subseq, according to @pazeshun 's comment
k-okada Dec 19, 2019
ab9e99f
Handle when robot-names is nil
YutoUchimi Dec 21, 2019
e68c7ee
Test both pr1012 and pr1040
YutoUchimi Dec 21, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
182 changes: 115 additions & 67 deletions pr2eus/make-pr2-model-file.l
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,11 +1,18 @@
(defun make-pr2-model-file (&key (output-directory (ros::rospack-find "pr2eus")))
(let ((robot "pr2") s
fname_urdf fname_collada fname_yaml fname_lisp
valid-cameras camera-paths)
(unless (setq s (ros::get-param "/robot_description"))
(ros::ros-error "could not load model file from /robot_description~%")
(return-from make-pr2-model-file))
;; re-define until https://github.com/jsk-ros-pkg/jsk_roseus/pull/629 is merged
(defun ros::list-param ()
"Get the list of all the parameters in the server"
(let ((s (piped-fork "rosparam list")) tmp ret)
(while (setq tmp (read s nil))
(push (format nil "~A" tmp) ret))
ret))

(defun get-pr2-names ()
;; get list of rosparam
(reverse (mapcan #'(lambda (x) (if (and (substringp "robot_description" x) (> (length x) (+ (length "robot_description") 1))) (list (subseq x 1 (- (length x) (length "/robot_description"))))))
(ros::list-param))))

(defun make-pr2-robot-model (robot robot-description)
(let (fname_urdf fname_collada fname_yaml fname_lisp)
;; variable setup
(setq fname_urdf (format nil "/tmp/~a_~d.urdf" robot (unix::getpid)))
(setq fname_collada (format nil "/tmp/~a_~d.dae" robot (unix::getpid)))
Expand All @@ -16,15 +23,20 @@
(unless (probe-file fname_lisp)
(with-open-file
(f fname_urdf :direction :output)
(format f s))
(format f robot-description))

(unix::system (format nil "rosrun collada_urdf urdf_to_collada ~A ~A" fname_urdf fname_collada))
(unix::system (format nil "rosrun euscollada collada2eus ~A ~A ~A" fname_collada fname_yaml fname_lisp))

(warning-message 2 "load model file from parameter server /robot_description to ~A~%" fname_lisp)
)
))

;; camera setup
(defun make-pr2-sensor-model (robot robot-camera-map)
(let (fname_list robot-names all-valid-cameras)
(setq robot-names (mapcar #'car robot-camera-map))
(setq all-valid-cameras (remove-duplicates (mapcan #'cdr (copy-object robot-camera-map)) :test #'string= :key #'(lambda (x) (cdr (assoc :camera-name x)))))
(setq fname_lisp (format nil "/tmp/~a_~d.l" robot (unix::getpid)))
(with-open-file
(f fname_lisp :direction :output :if-exists :append)
(format f ";;~%")
Expand All @@ -35,85 +47,121 @@
(format f ";; make-camera-from-ros-camera-info-aux is defined in roseus since 1.0.1, but to use this code in jskeus, we re-define here~%")
(format f "~A~%" (nconc (list 'defun 'make-camera-from-ros-camera-info-aux) (cddddr #'make-camera-from-ros-camera-info-aux)))
(format f ";;~%")
(format f "(defun pr2 () (setq *pr2* (instance pr2-sensor-robot :init)))~%")
(format f "(defun pr2 (&optional (name :~A)) (setq *pr2* (instance pr2-sensor-robot :init name)))~%" (car robot-names))
(format f "~%")
(setq camera-paths
(list "narrow_stereo/left"
"narrow_stereo/right"
"wide_stereo/left"
"wide_stereo/right"
"l_forearm_cam"
"r_forearm_cam"
"prosilica"
"kinect_head/rgb"
"kinect_head/depth"
))
(dolist (camera-path camera-paths)
(let* ((camera-info (format nil "~A/camera_info" camera-path))
(camera-name (substitute #\- #\/ (string-downcase camera-path)))
(var (intern (string-upcase camera-name)))
(frame-id)
(i 0))
(ros::subscribe camera-info sensor_msgs::CameraInfo
#'(lambda (msg)
(set var msg)
var))

(ros::rate 10)
(while (and (ros::ok) (not (boundp var)) (< (incf i) 50))
(ros::spin-once)
(ros::sleep))
(ros::unsubscribe camera-info)
(if (boundp var)
(progn
(setq var (eval var))
(setq frame-id (send var :header :frame_id))
(if (eq (elt frame-id 0) #\/) (setq frame-id (subseq frame-id 1)))
(warning-message 2 "received ~A ~A ~A~%" camera-info var frame-id)
(push (list (cons :camera-path camera-path) (cons :camera-name camera-name) (cons :camera-info camera-info) (cons :frame-id frame-id)) valid-cameras))
(ros::ros-error "could not receive ~A ~A" camera-info var))
) ;; let
) ;; dolist
;;
(format f "(defclass ~A-sensor-robot~%" robot)
(format f " :super pr2-robot~%")
(format f " :slots (")
(dolist (valid-camera valid-cameras)
(format f " :slots (name ")
(dolist (valid-camera all-valid-cameras)
(let* ((camera-name (cdr (assoc :camera-name valid-camera))))
(format f "~A " camera-name)))
(format f "~A " camera-name)))
(format f "))~%")
(format f "~%")
(format f "(defmethod ~A-sensor-robot~%" robot)
(format f " (:init (&rest args)~%")
(format f " (:init (n &rest args)~%")
(format f " (send-super* :init args)~%")
(format f " (setq name n)~%")
;; pr2 specific kinect_head frames
(format f " ;; kinect_head frame definition, this data is taken from jsk_pr2_startup kinect_head_launch ~%")
(format f "#|~%")
(format f " get frame coordinates data from pr1012:/etc/ros/distro/urdf/robot.xml~%")
(format f "|#~%")
(format f " ;; define cameras~%")
(dolist (valid-camera valid-cameras)
(let* ((camera-path (cdr (assoc :camera-path valid-camera)))
(camera-name (cdr (assoc :camera-name valid-camera)))
(frame-id (cdr (assoc :frame-id valid-camera)))
(var (eval (intern (string-upcase camera-name)))))
(format f " ;; ~A ~A~%" var (send var :P))
(format f " (setq ~A (make-camera-from-ros-camera-info-aux ~A ~A ~A ~A_lk :name :~A))~%~%" camera-name (send var :width) (send var :height) (send var :p) frame-id camera-path)))
;; :cameras
(format f " (setq cameras (list ")
(dolist (valid-camera valid-cameras)
(let ((camera-name (cdr (assoc :camera-name valid-camera))))
(format f " (send self :~A)" camera-name)))
(format f "))~%")
(format f " (case name~%")
(dolist (robot-name robot-names)
(setq valid-cameras (cdr (assoc robot-name robot-camera-map :test #'string=)))
(format f " ;; define cameras for ~A~%" robot-name)
(format f " (:~A~%" robot-name)
(dolist (valid-camera valid-cameras)
(let* ((camera-path (cdr (assoc :camera-path valid-camera)))
(camera-name (cdr (assoc :camera-name valid-camera)))
(frame-id (cdr (assoc :frame-id valid-camera)))
(var (eval (intern (string-upcase (format nil "~A_~A" robot-name camera-name))))))
(format f " ;; ~A ~A~%" (send (class var) :name) (send var :P))
(format f " (setq ~A (make-camera-from-ros-camera-info-aux ~A ~A ~A ~A_lk :name :~A))~%~%" camera-name (send var :width) (send var :height) (send var :p) frame-id camera-path)))
;; :cameras
(format f " (setq cameras (list ")
(dolist (valid-camera valid-cameras)
(let ((camera-name (cdr (assoc :camera-name valid-camera))))
(format f " (send self :~A)" camera-name)))
(format f "))~%")
(format f " ) ;; case ~A~%" robot-name)) ;; ease
(format f " )~%")
(format f " self)~%")
(format f "~%")
;; accessor to camera
(dolist (valid-camera valid-cameras)
(dolist (valid-camera all-valid-cameras)
(let* ((camera-name (cdr (assoc :camera-name valid-camera))))
(format f " (:~A (&rest args) (forward-message-to ~A args))~%" camera-name camera-name)))
(format f " ) ;; defmethod ~A-robot~%~%~%" robot)
) ;; with-open-file
(warning-message 1 "copy model file from ~A to ~A/~A.l~%" fname_lisp output-directory robot)
))

(defun make-pr2-model-file (&key (output-directory (ros::rospack-find "pr2eus")))
(let ((robot "pr2") robot-description robot-names
robot-camera-map camera-paths)

;; get robot naems from robot_description
(setq robot-names (get-pr2-names))
(ros::ros-info "found ~A robots, ~A" (length robot-names) robot-names)

(unless (setq robot-description (ros::get-param (format nil "/~A/robot_description" (or (car robot-names) ""))))
(ros::ros-error "could not load model file from /robot_description~%")
(return-from make-pr2-model-file))

(make-pr2-robot-model robot robot-description)

(setq camera-paths
(list "narrow_stereo/left"
"narrow_stereo/right"
"wide_stereo/left"
"wide_stereo/right"
"l_forearm_cam"
"r_forearm_cam"
"prosilica"
"kinect_head/rgb"
"kinect_head/depth"
))
(dolist (robot-name robot-names)
(let (valid-cameras)
(dolist (camera-path camera-paths)
(let* ((camera-info (format nil "/~A/~A/camera_info" robot-name camera-path))
(camera-name (substitute #\- #\/ (string-downcase camera-path)))
(var (intern (string-upcase (format nil "~A_~A" robot-name camera-name))))
(frame-id)
(i 0))
(ros::subscribe camera-info sensor_msgs::CameraInfo
#'(lambda (msg)
(set var msg)
var))
(ros::rate 10)
(while (and (ros::ok) (not (boundp var)) (< (incf i) 50))
(ros::spin-once)
(ros::sleep))
(ros::unsubscribe camera-info)
(if (boundp var)
(progn
(setq var (eval var))
(setq frame-id (send var :header :frame_id))
(if (eq (elt frame-id 0) #\/) (setq frame-id (subseq frame-id 1)))
(warning-message 2 "received ~A ~A ~A~%" camera-info (send var :P) frame-id)
(push (list (cons :camera-path camera-path) (cons :camera-name camera-name) (cons :camera-info camera-info) (cons :frame-id frame-id)) valid-cameras))
(ros::ros-error "could not receive ~A ~A" camera-info var))
)) ;; dolist (let *
(setq robot-name (if (string= robot-name "") "pr2" robot-name)) ;; for blank robot name
(push (cons robot-name valid-cameras) robot-camera-map)
))
(setq robot-camera-map (reverse robot-camera-map))
(make-pr2-sensor-model robot robot-camera-map)


(warning-message 2 ";;~%")
(warning-message 2 ";; copy model file from ~A to ~A/~A.l~%" fname_lisp output-directory robot)
(warning-message 2 ";;~%")
(unix::system (format nil "mv ~A ~A/~A.l" fname_lisp output-directory robot))
(warning-message 2 ";;~%")
(warning-message 2 ";; successfully generated ~A/~A robot file, now exiting program...~%" output-directory robot)
(warning-message 2 ";;~%")
))

;;(ros::roseus "make-pr2-modle-file")
Expand Down
11 changes: 11 additions & 0 deletions pr2eus/make-pr2-model-file.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
<launch>
<!-- launch scriept to generate pr2.l from urdf and sensor message -->
<rosparam command="load" file="$(find pr2eus)/test/pr1012_urdf.yaml" param="/pr1012/robot_description"/>
<rosparam command="load" file="$(find pr2eus)/test/pr1040_urdf.yaml" param="/pr1040/robot_description"/>
<node name="pr1012_camera_info" pkg="rosbag" type="play"
args="--prefix=/pr1012 -l $(find pr2eus)/test/pr1012_camera_info.bag" />
<node name="pr1040_camera_info" pkg="rosbag" type="play"
args="--prefix=/pr1040 -l $(find pr2eus)/test/pr1040_camera_info.bag" />
<node name="make_pr2_model_file" pkg="roseus" type="roseus" required="true" output="screen"
args="$(find pr2eus)/make-pr2-model-file.l &quot;(progn (ros::roseus \&quot;make-pr2-modle-file\&quot;) (make-pr2-model-file) (sys::exit 0))&quot;" />
</launch>
Loading