diff --git a/jsk_2022_10_semi/ayumu_demo/euslisp/go_walking.l b/jsk_2022_10_semi/ayumu_demo/euslisp/go_walking.l new file mode 100644 index 0000000000..1c29ef3a65 --- /dev/null +++ b/jsk_2022_10_semi/ayumu_demo/euslisp/go_walking.l @@ -0,0 +1,38 @@ +(cond + ((< *counter2* 3) + (progn + (ros::ros-info (format nil "counter2: ~A" *counter2*)) + (setq *counter2* (+ *counter2* 1)) + (setq *sum_x* (+ *sum_x* x)) + (setq *sum_y* (+ *sum_y* y)))) + + ((= *counter2* 3) + (progn + (ros::ros-info (format nil "counter2: ~A" *counter2*)) + (setq *ref_x* (/ *sum_x* 3)) + (setq *ref_y* (/ *sum_y* 3)) + (ros::ros-info (format nil "ref_x: ~A, ref_y: ~A" *ref_x* *ref_y*)) + (send *ri* :speak "go walking") + (send *ri* :wait-interpolation) + (setq *counter2* 10))) + + ((> dis 3) + (progn + (send *ri* :go-velocity 0 0 0 500) + (send *ri* :speak "lost a person") + (send *ri* :wait-interpolation) + (unix::sleep 3))) + + (t + (when (> (+ (abs (- x *ref_x*)) (abs (- y *ref_y*))) 0.1) + (progn + (ros::ros-info "go-vel") + + ;; go-pos + ;; (send *ri* :go-pos (* 1.3 (- x *ref_x*)) (* 1.3 (- y *ref_y*)) 0) + + ;; go-velocity + (send *ri* :go-velocity 0 (* 0.7 (- y *ref_y*)) 0 500 :stop nil) + (send *ri* :wait-interpolation) + (send *ri* :go-velocity (* 1.4 (- x *ref_x*)) 0 0 500 :stop nil) + (send *ri* :wait-interpolation))))) diff --git a/jsk_2022_10_semi/ayumu_demo/euslisp/main.l b/jsk_2022_10_semi/ayumu_demo/euslisp/main.l new file mode 100644 index 0000000000..8ab4083eb3 --- /dev/null +++ b/jsk_2022_10_semi/ayumu_demo/euslisp/main.l @@ -0,0 +1,52 @@ +#! usr/bin/env roseus + +(ros::roseus-add-msgs "std_msgs") +(ros::roseus "umbrella") + +(load "package://spoteus/spot-interface.l") + +(spot-init) +;; (setq *spot* (instance spot-robot :init)) +(objects (list *spot*)) + +;; 初期姿勢 +(send *spot* :angle-vector #f(0 45 -90 0 45 -90 0 45 -90 0 45 -90 0 -170 160 0 10 90 0)) +(send *ri* :angle-vector (send *spot* :angle-vector) 3000) +(send *ri* :gripper-close) + +;; =============================== +;; 傘を差し出し済みかどうか +(setq *umbrella_replace_done* nil) + +;; 追従する人物の基準位置座標 +(setq *ref_x* 100) +(setq *ref_y* 100) + +(setq *counter* 0) +(setq *counter2* 0) + +(setq *pre_position_number* 100) + +(setq *sum_x* 0) +(setq *sum_y* 0) +;; =============================== + +(defun sub (msg) + (setq data (send msg :data)) + + (setq position_number (elt data 0)) + (setq x (elt data 1)) + (setq y (elt data 2)) + (setq z (elt data 3)) + (setq dis (elt data 4)) + + (ros::ros-info (format nil "x: ~A, y: ~A" x y)) + + (if (not *umbrella_replace_done*) + (load "set_umbrella.l") + (load "go_walking.l"))) + +(ros::subscribe "/nearest_pos" std_msgs::float32multiarray #'sub) + +(ros::rate 10) +(ros::spin) diff --git a/jsk_2022_10_semi/ayumu_demo/euslisp/set_umbrella.l b/jsk_2022_10_semi/ayumu_demo/euslisp/set_umbrella.l new file mode 100644 index 0000000000..107c1f53c9 --- /dev/null +++ b/jsk_2022_10_semi/ayumu_demo/euslisp/set_umbrella.l @@ -0,0 +1,73 @@ +(defun control_gripper () + (send *ri* :gripper-open) + + ;; (send *ri* :speak-jp "傘をセットしてください") + (send *ri* :wait-interpolation) + (unix::sleep 2) + + (send *ri* :gripper-close) + (send *ri* :wait-interpolation)) + + + + + +(ros::ros-info (format nil "position_number: ~A" position_number)) + +;; 近くに人がいないとき +(if (= position_number 100) + (progn + (setq *counter* 0) + (setq *pre_position_number* position_number) + (ros::ros-info (format nil "counter: ~A" *counter*))) + + (cond + ((= *counter* 0) + (progn + (ros::ros-info (format nil "(= *counter* 0)")) + ;; (ros::ros-info (format nil "counter: ~A" *counter*)) + (setq *counter* 1) + (setq *pre_position_number* position_number) + (ros::ros-info (format nil "counter: ~A" *counter*)) + ;;(return-from sub) + )) + ((and (> *counter* 0) (< *counter* 5)) + (progn + (ros::ros-info (format nil "(and (> *counter* 0) (< *counter* 5))")) + ;; (ros::ros-info (format nil "counter: ~A" *counter*)) + (if (= *pre_position_number* position_number) + (progn + (setq *counter* (+ *counter* 1))) + (progn + (setq *counter* 1))) + (setq *pre_position_number* position_number) + (ros::ros-info (format nil "counter: ~A" *counter*)) + ;;(return-from sub) + )) + ((= *counter* 5) + (progn + (ros::ros-info (format nil "(= *counter* 3)")) + ;; (ros::ros-info (format nil "counter: ~A" *counter*)) + + ;; spotの左側に人がいるとき + (when (= position_number 1) + (progn + (send *ri* :go-pos (* 1.3 (- x 0.37)) -0.3 0) + ;; (send *ri* :wait-interpolation) + (send *spot* :angle-vector #f(0 45 -90 0 45 -90 0 45 -90 0 45 -90 90 -100 70 0 30 -90 -80)) + (send *ri* :angle-vector (send *spot* :angle-vector) 3000) + (send *ri* :wait-interpolation))) + + ;; spotの右側に人がいるとき + (when (= position_number 2) + (progn + (send *ri* :go-pos (* 1.3 (- x 0.37)) 0.3 0) + ;; (send *ri* :wait-interpolation) + (send *spot* :angle-vector #f(0 45 -90 0 45 -90 0 45 -90 0 45 -90 -90 -100 70 0 30 -90 -80)) + (send *ri* :angle-vector (send *spot* :angle-vector) 3000) + (send *ri* :wait-interpolation))) + + (control_gripper) + + ;; 傘の差し出し完了 + (setq *umbrella_replace_done* t))))) diff --git a/jsk_2022_10_semi/ayumu_demo/node_distance.py b/jsk_2022_10_semi/ayumu_demo/node_distance.py new file mode 100644 index 0000000000..444e20c109 --- /dev/null +++ b/jsk_2022_10_semi/ayumu_demo/node_distance.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python + +import rospy +from jsk_recognition_msgs.msg import BoundingBoxArray +from std_msgs.msg import Float32MultiArray + +pub = rospy.Publisher('nearest_pos', Float32MultiArray, queue_size=1) + +def callback(msg): + boxes = msg.boxes + + # there is no person around the camera + if (not boxes): + rospy.loginfo("no person") + return -1 + + length = len(boxes) + min_distance = 10000 + min_ind = 0 + + # identify the person closest to the camera + for i in range(length): + position = boxes[i].pose.position + distance = position.x*position.x +position.y*position.y + if (min_distance > distance): + min_distance = distance + min_ind = i + + coordinate = [boxes[min_ind].pose.position.x, boxes[min_ind].pose.position.y, boxes[min_ind].pose.position.z] + + position_number = 100 + + if ((-0.6 <= coordinate[0]) and (coordinate[0] < 1.5)): + if ((0.3 < coordinate[1]) and (coordinate[1] < 0.6)): + position_number = 1 # left + elif ((-0.6 < coordinate[1]) and (coordinate[1] < -0.3)): + position_number = 2 # right + + min_position = [] + min_position.append(position_number) + min_position.append(coordinate[0]) + min_position.append(coordinate[1]) + min_position.append(coordinate[2]) + min_position.append(min_distance) + + min_forPublish = Float32MultiArray(data=min_position) + pub.publish(min_forPublish) + +def listener(): + rospy.Subscriber("/rect_array_in_panorama_to_bounding_box_array/bbox_array", BoundingBoxArray, callback) + rospy.spin() + +if __name__ == '__main__': + rospy.init_node('distance', anonymous=True) + listener() diff --git a/jsk_2022_10_semi/ayumu_demo/panorama_human_3d_detection.launch b/jsk_2022_10_semi/ayumu_demo/panorama_human_3d_detection.launch new file mode 100644 index 0000000000..89c7563031 --- /dev/null +++ b/jsk_2022_10_semi/ayumu_demo/panorama_human_3d_detection.launch @@ -0,0 +1,89 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + frame_fixed: $(arg frame_fixed) + dimensions_labels: + person: [0.5, 0.5, 1.5] + + + + + + + + + + + + + \ No newline at end of file diff --git a/jsk_2022_10_semi/euslisp/ayumu/spot-ik.l b/jsk_2022_10_semi/euslisp/ayumu/spot-ik.l new file mode 100644 index 0000000000..03ab97dcf3 --- /dev/null +++ b/jsk_2022_10_semi/euslisp/ayumu/spot-ik.l @@ -0,0 +1,21 @@ +;; ファイルの読み込み +(load "package://spoteus/spot-interface.l") + +;; spotオブジェクトの作成 +;;(spot-init) +(setq *spot* (instance spot-robot :init)) +;;(objects (list *spot*)) + +;: spotの手先座標系をコピーしend-coordsに代入 +(setq end-coords (send (send *spot* :arm :end-coords) :copy-worldcoords)) +(objects (list *spot* end-coords)) + +;; end-coordsを絶対座標系におけるz軸方向に100mm平行移動する +(send end-coords :translate #f(0 0 100) :world) + +(send *spot* :inverse-kinematics + end-coords ;; 目標位置/姿勢 + :move-target (send *spot* :arm :end-coords) ;; 動かすもの + :revert-if-fail t + :rotational-axis t + :translation-axis t) diff --git a/jsk_2022_10_semi/euslisp/ayumu/sub_pos.l b/jsk_2022_10_semi/euslisp/ayumu/sub_pos.l new file mode 100644 index 0000000000..0d9de03e57 --- /dev/null +++ b/jsk_2022_10_semi/euslisp/ayumu/sub_pos.l @@ -0,0 +1,26 @@ +#! usr/bin/env roseus + +(ros::roseus-add-msgs "jsk_recognition_msgs") +(ros::roseus "walking") + +(ros::ros-info "start") + +(defun sub (msg) + (setq boxes (send msg :boxes)) + (if boxes + (progn + ;; (ros::ros-info "subscribe msg [boxes: ~A]" boxes) + ;; 1人目の情報を取得 + (setq pose (send (elt boxes 0) :pose)) + ;; (ros::ros-info "subscribe msg [pose: ~A]" pose) + (setq x (send pose :position :x)) + (setq y (send pose :position :y)) + (setq z (send pose :position :z)) + (ros::ros-info "subscribe msg [x: ~A, y: ~A, z: ~A]" x y z)) + (ros::ros-info "no person"))) + + +(ros::subscribe "/rect_array_in_panorama_to_bounding_box_array/bbox_array" jsk_recognition_msgs::BoundingBoxArray #'sub) + +(ros::spin) +(exit) diff --git a/jsk_2022_10_semi/euslisp/ayumu/umbrella.l b/jsk_2022_10_semi/euslisp/ayumu/umbrella.l new file mode 100644 index 0000000000..1fd3f663fc --- /dev/null +++ b/jsk_2022_10_semi/euslisp/ayumu/umbrella.l @@ -0,0 +1,73 @@ +;; ファイルの読み込み +(load "package://spoteus/spot-interface.l") + +;; spotオブジェクトの作成 +(spot-init) +;;(setq *spot* (instance spot-robot :init)) +(objects (list *spot*)) + +(send *ri* :speak-jp "お散歩に行きましょう") + +;; 傘を持つ姿勢 +(send *spot* :angle-vector #f(0 45 -90 0 45 -90 0 45 -90 0 45 -90 90 -100 70 0 30 -90 -80)) + +(send *ri* :angle-vector (send *spot* :angle-vector) 8000) +(send *ri* :wait-interpolation) + +;; 手の開閉 +(send *ri* :gripper-open) +(send *ri* :wait-interpolation) + +(send *ri* :speak-jp "傘をセットしてください") +(unix::sleep 3) + +(send *ri* :gripper-close) +(send *ri* :wait-interpolation) + +;: spotの手先座標系をコピーしend-coordsに代入 +(setq end-coords (send (send *spot* :arm :end-coords) :copy-worldcoords)) +(objects (list *spot* end-coords)) + +;; end-coordsを絶対座標系におけるz軸正の方向に100mm平行移動する +(send end-coords :translate #f(0 0 100) :world) + +(send *spot* :inverse-kinematics + end-coords ;; 目標位置/姿勢 + :move-target (send *spot* :arm :end-coords) ;; 動かすもの + :revert-if-fail t + :rotational-axis :z + :translation-axis t) + +(send *ri* :angle-vector (send *spot* :angle-vector) 5000) +(send *ri* :wait-interpolation) + +;; end-coordsを絶対座標系におけるz軸負の方向に600mm平行移動する +(send end-coords :translate #f(0 0 -600) :world) + +(send *spot* :inverse-kinematics + end-coords + :move-target (send *spot* :arm :end-coords) + :revert-if-fail t + :rotational-zxis :z + :translation-axis t) + +(send *ri* :angle-vector (send *spot* :angle-vector) 5000) +(send *ri* :wait-interpolation) + +(send *spot* :angle-vector #f(0 45 -90 0 45 -90 0 45 -90 0 45 -90 90 -100 70 0 30 -90 -80)) + +(send *ri* :angle-vector (send *spot* :angle-vector) 5000) +(send *ri* :wait-interpolation) + +;; end-coordsを絶対座標系におけるx軸負の方向に600mm平行移動する +(send end-coords :translate #f(-100 0 500) :world) + +(send *spot* :inverse-kinematics + end-coords + :move-target (send *spot* :arm :end-coords) + :revert-if-fail t + :rotational-zxis :z + :translation-axis t) + +(send *ri* :angle-vector (send *spot* :angle-vector) 5000) +(send *ri* :wait-interpolation) diff --git a/jsk_2022_10_semi/euslisp/ayumu/walking.l b/jsk_2022_10_semi/euslisp/ayumu/walking.l new file mode 100644 index 0000000000..1272b42ef8 --- /dev/null +++ b/jsk_2022_10_semi/euslisp/ayumu/walking.l @@ -0,0 +1,57 @@ +#! usr/bin/env roseus + +;; ファイルの読み込み +(load "package://spoteus/spot-interface.l") + +(ros::roseus-add-msgs "jsk_recognition_msgs") +(ros::roseus "walking") + +;; spotオブジェクトの作成 +(spot-init) +;;(setq *spot* (instance spot-robot :init)) +(objects (list *spot*)) + +(send *ri* :speak-jp "傘を持ちます") + +;; 傘を持つ姿勢 +(send *spot* :angle-vector #f(0 45 -90 0 45 -90 0 45 -90 0 45 -90 90 -100 70 0 30 -90 -80)) +(send *ri* :angle-vector (send *spot* :angle-vector) 8000) +(send *ri* :wait-interpolation) + +;; 手の開閉、傘をセット +(send *ri* :gripper-open) +(send *ri* :wait-interpolation) +(send *ri* :speak-jp "傘をセットしてください") +(unix::sleep 3) +(send *ri* :gripper-close) +(send *ri* :wait-interpolation) + +(send *ri* :speak-jp "お散歩に行きましょう") + +(ros::ros-info "start subscribing") + +(defun sub (msg) + (setq boxes (send msg :boxes)) + (if boxes + (progn + ;; (ros::ros-info "subscribe msg [boxes: ~A]" boxes) + + ;; 1人目の情報を取得 + (setq pose (send (elt boxes 0) :pose)) + ;; (ros::ros-info "subscribe msg [pose: ~A]" pose) + (setq x (send pose :position :x)) + (setq y (send pose :position :y)) + (setq z (send pose :position :z)) + (ros::ros-info "subscribe msg [x: ~A, y: ~A, z: ~A]" x y z) + + ;; 人間とspotとの距離感の調整 + (send *ri* :go-velocity #横方向 x-基準値#) + + ;; 人間とspotとの前後関係の調整 + (send *ri* :go-velocity #前後方向 y-基準値#)) + (ros::ros-info "no person"))) + +(ros::subscribe "/rect_array_in_panorama_to_bounding_box_array/bbox_array" jsk_recognition_msgs::BoundingBoxArray #'sub) + +(ros::spin) +(exit) diff --git a/jsk_2022_10_semi/launch/human_detector.launch b/jsk_2022_10_semi/launch/human_detector.launch new file mode 100644 index 0000000000..21e6211525 --- /dev/null +++ b/jsk_2022_10_semi/launch/human_detector.launch @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + frame_fixed: "camera" + dimensions_labels: + person: [0.5, 0.5, 1.5] + + + + + + + + diff --git a/jsk_2022_10_semi/launch/human_detector2.launch b/jsk_2022_10_semi/launch/human_detector2.launch new file mode 100644 index 0000000000..d3790bdd2c --- /dev/null +++ b/jsk_2022_10_semi/launch/human_detector2.launch @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + frame_fixed: "camera" + image_transport: compressed + dimensions_labels: + person: [0.5, 0.5, 1.5] + + + + + + + +