From 18cb823f2f1da5ef32a4dc81e07d068ac0576e6c Mon Sep 17 00:00:00 2001 From: Ayumu Iwata Date: Wed, 30 Nov 2022 16:02:24 +0900 Subject: [PATCH 1/9] add ayumu demo --- jsk_2022_10_semi/euslisp/ayumu/spot-ik.l | 21 +++++++ jsk_2022_10_semi/euslisp/ayumu/sub_pos.l | 26 ++++++++ jsk_2022_10_semi/euslisp/ayumu/umbrella.l | 73 +++++++++++++++++++++++ jsk_2022_10_semi/euslisp/ayumu/walking.l | 57 ++++++++++++++++++ 4 files changed, 177 insertions(+) create mode 100644 jsk_2022_10_semi/euslisp/ayumu/spot-ik.l create mode 100644 jsk_2022_10_semi/euslisp/ayumu/sub_pos.l create mode 100644 jsk_2022_10_semi/euslisp/ayumu/umbrella.l create mode 100644 jsk_2022_10_semi/euslisp/ayumu/walking.l 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) From 5ad84d8d2da94ed8dd7de233451ebc52d3411b66 Mon Sep 17 00:00:00 2001 From: Ayumu Iwata Date: Wed, 30 Nov 2022 19:13:03 +0900 Subject: [PATCH 2/9] add human_detector.launch --- jsk_2022_10_semi/launch/human_detector.launch | 73 +++++++++++++++++++ 1 file changed, 73 insertions(+) create mode 100644 jsk_2022_10_semi/launch/human_detector.launch 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] + + + + + + + + From eba487cff54c1b0289e32c65d78cf8ce87b35ed1 Mon Sep 17 00:00:00 2001 From: Ayumu Iwata Date: Sat, 3 Dec 2022 20:55:20 +0900 Subject: [PATCH 3/9] add human_detector2.launch : improved version of human_detector.launch --- .../launch/human_detector2.launch | 57 +++++++++++++++++++ 1 file changed, 57 insertions(+) create mode 100644 jsk_2022_10_semi/launch/human_detector2.launch 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..f62532e474 --- /dev/null +++ b/jsk_2022_10_semi/launch/human_detector2.launch @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + frame_fixed: "camera" + dimensions_labels: + person: [0.5, 0.5, 1.5] + + + + + + + + From 5f2e2476a4d7490def45eb9f5853eeae37d33858 Mon Sep 17 00:00:00 2001 From: Ayumu Iwata Date: Wed, 7 Dec 2022 19:35:19 +0900 Subject: [PATCH 4/9] fix for spot --- jsk_2022_10_semi/ayumu_demo/node_distance.py | 66 +++++++++++++++++++ .../launch/human_detector2.launch | 8 ++- 2 files changed, 72 insertions(+), 2 deletions(-) create mode 100644 jsk_2022_10_semi/ayumu_demo/node_distance.py 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..c2451cad61 --- /dev/null +++ b/jsk_2022_10_semi/ayumu_demo/node_distance.py @@ -0,0 +1,66 @@ +#!/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 + + if (not boxes): + rospy.loginfo("no person") + return -1 + + length = len(boxes) + min_distance = 10000 + min_ind = 0 + + 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 + + #min_position = boxes[min_ind].pose.position + #rospy.loginfo("%f, %f, %f", min_position.x, min_position.y, min_position.z) + + 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] < -0.3)): + if ((0 <= coordinate[1]) and (coordinate[1] < 0.3)): + position_number = 1 + elif ((-0.3 < coordinate[1]) and (coordinate[1] < 0)): + position_number = 2 + elif ((-0.9 <= coordinate[0]) and (coordinate[0] < -0.6)): + if ((0 <= coordinate[1]) and (coordinate[1] < 0.3)): + position_number = 3 + elif ((-0.3 < coordinate[1]) and (coordinate[1] < 0)): + position_number = 4 + elif ((-1.2 <= coordinate[0]) and (coordinate[0] < -0.9)): + if ((0 <= coordinate[1]) and (coordinate[1] < 0.3)): + position_number = 5 + elif ((-0.3 < coordinate[1]) and (coordinate[1] < 0)): + position_number = 6 + + min_position = [] + min_position.append(position_number) + min_position.append(boxes[min_ind].pose.position.x) + min_position.append(boxes[min_ind].pose.position.y) + min_position.append(boxes[min_ind].pose.position.z) + 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/launch/human_detector2.launch b/jsk_2022_10_semi/launch/human_detector2.launch index f62532e474..d3790bdd2c 100644 --- a/jsk_2022_10_semi/launch/human_detector2.launch +++ b/jsk_2022_10_semi/launch/human_detector2.launch @@ -1,19 +1,22 @@ + + + - + - + @@ -40,6 +43,7 @@ frame_fixed: "camera" + image_transport: compressed dimensions_labels: person: [0.5, 0.5, 1.5] From e94e4cedc0f87ea5af7ebf37d9f6f1bc1a0fe497 Mon Sep 17 00:00:00 2001 From: Ayumu Iwata Date: Wed, 14 Dec 2022 15:28:30 +0900 Subject: [PATCH 5/9] mkdir ayumu_demo/ and add node_distance.py --- jsk_2022_10_semi/ayumu_demo/node_distance.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/jsk_2022_10_semi/ayumu_demo/node_distance.py b/jsk_2022_10_semi/ayumu_demo/node_distance.py index c2451cad61..c170387ba7 100644 --- a/jsk_2022_10_semi/ayumu_demo/node_distance.py +++ b/jsk_2022_10_semi/ayumu_demo/node_distance.py @@ -24,9 +24,6 @@ def callback(msg): min_distance = distance min_ind = i - #min_position = boxes[min_ind].pose.position - #rospy.loginfo("%f, %f, %f", min_position.x, min_position.y, min_position.z) - coordinate = [boxes[min_ind].pose.position.x boxes[min_ind].pose.position.y boxes[min_ind].pose.position.z] position_number = 100 From 55878bbb422dbc2ebc12b5ec80760a2a85884281 Mon Sep 17 00:00:00 2001 From: Ayumu Iwata Date: Wed, 14 Dec 2022 19:43:04 +0900 Subject: [PATCH 6/9] fix ayumu_dmeo/node_distance.py --- jsk_2022_10_semi/ayumu_demo/node_distance.py | 24 +++++++------------- 1 file changed, 8 insertions(+), 16 deletions(-) diff --git a/jsk_2022_10_semi/ayumu_demo/node_distance.py b/jsk_2022_10_semi/ayumu_demo/node_distance.py index c170387ba7..068929bed0 100644 --- a/jsk_2022_10_semi/ayumu_demo/node_distance.py +++ b/jsk_2022_10_semi/ayumu_demo/node_distance.py @@ -24,25 +24,17 @@ def callback(msg): 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] + 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] < -0.3)): - if ((0 <= coordinate[1]) and (coordinate[1] < 0.3)): - position_number = 1 - elif ((-0.3 < coordinate[1]) and (coordinate[1] < 0)): - position_number = 2 - elif ((-0.9 <= coordinate[0]) and (coordinate[0] < -0.6)): - if ((0 <= coordinate[1]) and (coordinate[1] < 0.3)): - position_number = 3 - elif ((-0.3 < coordinate[1]) and (coordinate[1] < 0)): - position_number = 4 - elif ((-1.2 <= coordinate[0]) and (coordinate[0] < -0.9)): - if ((0 <= coordinate[1]) and (coordinate[1] < 0.3)): - position_number = 5 - elif ((-0.3 < coordinate[1]) and (coordinate[1] < 0)): - position_number = 6 + if ((-0.6 <= coordinate[0]) and (coordinate[0] < 1.5)): + if ((0.3 < coordinate[1]) and (coordinate[1] < 0.6)): + position_number = 1 # left back + elif ((-0.6 < coordinate[1]) and (coordinate[1] < -0.3)): + position_number = 2 # right back + + min_position = [] min_position.append(position_number) From 42d0afebcff61b381d732a9e6911ef60fb9bc09d Mon Sep 17 00:00:00 2001 From: Ayumu Iwata Date: Wed, 14 Dec 2022 19:44:23 +0900 Subject: [PATCH 7/9] add euslisp/(main go_walking set_umbrella) to ayumu_demo/ --- .../ayumu_demo/euslisp/go_walking.l | 26 +++++ jsk_2022_10_semi/ayumu_demo/euslisp/main.l | 28 ++++++ .../ayumu_demo/euslisp/set_umbrella.l | 95 +++++++++++++++++++ 3 files changed, 149 insertions(+) create mode 100644 jsk_2022_10_semi/ayumu_demo/euslisp/go_walking.l create mode 100644 jsk_2022_10_semi/ayumu_demo/euslisp/main.l create mode 100644 jsk_2022_10_semi/ayumu_demo/euslisp/set_umbrella.l 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..755091b80b --- /dev/null +++ b/jsk_2022_10_semi/ayumu_demo/euslisp/go_walking.l @@ -0,0 +1,26 @@ +#! usr/bin/env roseus + +(ros::roseus-add-msgs "std_msgs") +(ros::roseus "go_walking") + +(defun sub_to_walking (msg) + (when (= *umbrella_replace_done* nil) + (return-from sub_to_walking)) + + (setq data (send msg :data)) + + (setq x (elt data 1)) + (setq y (elt data 2)) + + (when (> (abs (- x *pre_x*)) 0.5) + ;; 左右に動く + (send *ri* :go-pos 0 (- x *pre_x*) 0) + (setq *pre_x* x)) + + (when (> (abs (- y *pre_y*)) 0.5) + ;; 前後に動く + (send *ri* :go-pos (- y *pre_y*) 0 0) + (setq *pre_y* y)) + ) + +(ros::subscribe "/nearest_pos" std_msgs::float32multiarray #'sub_to_walking) 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..0c4ad4bd99 --- /dev/null +++ b/jsk_2022_10_semi/ayumu_demo/euslisp/main.l @@ -0,0 +1,28 @@ +(load "package://spoteus/spot-interface.l") + +(spot-init) +;; (setq *spot* (instance spot-robot :init)) +(objects (list *spot*)) +(ros::ros-error "ok") + +;; 初期姿勢 +(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 *pre_x* 100) +(setq *pre_y* 100) +;; =============================== + +;; 傘をセットする +(load "set_umbrella.l") + +;; 散歩をする +;;(load "go_walking.l") + +(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..106778d52d --- /dev/null +++ b/jsk_2022_10_semi/ayumu_demo/euslisp/set_umbrella.l @@ -0,0 +1,95 @@ +#! usr/bin/env roseus + +(ros::ros-error "start set_umbrella.l") + +(ros::roseus-add-msgs "std_msgs") +(ros::roseus "set_umbrella") + +(setq *threshold* 0.5) ;; 近くに人間がいるかどうかの距離の基準 +(setq *counter* 0) +(setq *pre_position_number* 100) +;; (setq *umbrella_replace_done* nil) ;; 傘を差し出し済みかどうか + +(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) + ) + +(defun sub_to_set (msg) + (when *umbrella_replace_done* + (return-from sub_to_set)) + + (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 "position_number: ~A" position_number)) + + (when (= position_number 100) + (progn + (setq *counter* 0) + (setq *pre_position_number* position_number) + (ros::ros-info (format nil "counter: ~A" *counter*)) + (return-from sub_to_set))) + + (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_to_set))) + ((and (> *counter* 0) (< *counter* 5)) + (progn + (ros::ros-info (format nil "(and (> *counter* 0) (< *counter* 3))")) + (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) + ;; (print *counter*) + (ros::ros-info (format nil "counter: ~A" *counter*)) + (return-from sub_to_set))) + ((= *counter* 5) + (progn + (ros::ros-info (format nil "(= *counter* 3)")) + (ros::ros-info (format nil "counter: ~A" *counter*)) + ;; (print position_number) + ;; (print "position") + (when (= position_number 1) + (progn + (send *ri* :go-pos (* 1.2 (- x 0.37)) -0.2 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))) + + (when (= position_number 2) + (progn + (send *ri* :go-pos (* 1.2 (- x 0.37)) 0.2 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 *pre_x* x) + (setq *pre_y* y) + (setq *umbrella_replace_done* t))))) + +(ros::subscribe "/nearest_pos" std_msgs::float32multiarray #'sub_to_set) From cf517161682125073a3d6781c5b6453a89e665bd Mon Sep 17 00:00:00 2001 From: Ayumu Iwata Date: Tue, 20 Dec 2022 20:18:05 +0900 Subject: [PATCH 8/9] fix for walking --- .../ayumu_demo/euslisp/go_walking.l | 56 ++++++++----- jsk_2022_10_semi/ayumu_demo/euslisp/main.l | 40 +++++++-- .../ayumu_demo/euslisp/set_umbrella.l | 82 +++++++------------ jsk_2022_10_semi/ayumu_demo/node_distance.py | 16 ++-- 4 files changed, 104 insertions(+), 90 deletions(-) diff --git a/jsk_2022_10_semi/ayumu_demo/euslisp/go_walking.l b/jsk_2022_10_semi/ayumu_demo/euslisp/go_walking.l index 755091b80b..1c29ef3a65 100644 --- a/jsk_2022_10_semi/ayumu_demo/euslisp/go_walking.l +++ b/jsk_2022_10_semi/ayumu_demo/euslisp/go_walking.l @@ -1,26 +1,38 @@ -#! usr/bin/env roseus +(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)))) -(ros::roseus-add-msgs "std_msgs") -(ros::roseus "go_walking") + ((= *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))) -(defun sub_to_walking (msg) - (when (= *umbrella_replace_done* nil) - (return-from sub_to_walking)) - - (setq data (send msg :data)) + ((> 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") - (setq x (elt data 1)) - (setq y (elt data 2)) + ;; go-pos + ;; (send *ri* :go-pos (* 1.3 (- x *ref_x*)) (* 1.3 (- y *ref_y*)) 0) - (when (> (abs (- x *pre_x*)) 0.5) - ;; 左右に動く - (send *ri* :go-pos 0 (- x *pre_x*) 0) - (setq *pre_x* x)) - - (when (> (abs (- y *pre_y*)) 0.5) - ;; 前後に動く - (send *ri* :go-pos (- y *pre_y*) 0 0) - (setq *pre_y* y)) - ) - -(ros::subscribe "/nearest_pos" std_msgs::float32multiarray #'sub_to_walking) + ;; 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 index 0c4ad4bd99..8ab4083eb3 100644 --- a/jsk_2022_10_semi/ayumu_demo/euslisp/main.l +++ b/jsk_2022_10_semi/ayumu_demo/euslisp/main.l @@ -1,9 +1,13 @@ +#! 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*)) -(ros::ros-error "ok") ;; 初期姿勢 (send *spot* :angle-vector #f(0 45 -90 0 45 -90 0 45 -90 0 45 -90 0 -170 160 0 10 90 0)) @@ -14,15 +18,35 @@ ;; 傘を差し出し済みかどうか (setq *umbrella_replace_done* nil) -;; 前の人物の位置座標 -(setq *pre_x* 100) -(setq *pre_y* 100) +;; 追従する人物の基準位置座標 +(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) ;; =============================== -;; 傘をセットする -(load "set_umbrella.l") +(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"))) -;; 散歩をする -;;(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 index 106778d52d..107c1f53c9 100644 --- a/jsk_2022_10_semi/ayumu_demo/euslisp/set_umbrella.l +++ b/jsk_2022_10_semi/ayumu_demo/euslisp/set_umbrella.l @@ -1,15 +1,3 @@ -#! usr/bin/env roseus - -(ros::ros-error "start set_umbrella.l") - -(ros::roseus-add-msgs "std_msgs") -(ros::roseus "set_umbrella") - -(setq *threshold* 0.5) ;; 近くに人間がいるかどうかの距離の基準 -(setq *counter* 0) -(setq *pre_position_number* 100) -;; (setq *umbrella_replace_done* nil) ;; 傘を差し出し済みかどうか - (defun control_gripper () (send *ri* :gripper-open) @@ -18,78 +6,68 @@ (unix::sleep 2) (send *ri* :gripper-close) - (send *ri* :wait-interpolation) - ) - -(defun sub_to_set (msg) - (when *umbrella_replace_done* - (return-from sub_to_set)) - - (setq data (send msg :data)) + (send *ri* :wait-interpolation)) - (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 "position_number: ~A" position_number)) - (when (= position_number 100) + + +(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*)) - (return-from sub_to_set))) + (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*)) + ;; (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_to_set))) + ;;(return-from sub) + )) ((and (> *counter* 0) (< *counter* 5)) (progn - (ros::ros-info (format nil "(and (> *counter* 0) (< *counter* 3))")) - (ros::ros-info (format nil "counter: ~A" *counter*)) + (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* (+ *counter* 1))) - (progn - (setq *counter* 1))) + (setq *counter* 1))) (setq *pre_position_number* position_number) - ;; (print *counter*) (ros::ros-info (format nil "counter: ~A" *counter*)) - (return-from sub_to_set))) + ;;(return-from sub) + )) ((= *counter* 5) (progn (ros::ros-info (format nil "(= *counter* 3)")) - (ros::ros-info (format nil "counter: ~A" *counter*)) - ;; (print position_number) - ;; (print "position") + ;; (ros::ros-info (format nil "counter: ~A" *counter*)) + + ;; spotの左側に人がいるとき (when (= position_number 1) (progn - (send *ri* :go-pos (* 1.2 (- x 0.37)) -0.2 0) - (send *ri* :wait-interpolation) + (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.2 (- x 0.37)) 0.2 0) - (send *ri* :wait-interpolation) + (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 *pre_x* x) - (setq *pre_y* y) + + ;; 傘の差し出し完了 (setq *umbrella_replace_done* t))))) - -(ros::subscribe "/nearest_pos" std_msgs::float32multiarray #'sub_to_set) diff --git a/jsk_2022_10_semi/ayumu_demo/node_distance.py b/jsk_2022_10_semi/ayumu_demo/node_distance.py index 068929bed0..444e20c109 100644 --- a/jsk_2022_10_semi/ayumu_demo/node_distance.py +++ b/jsk_2022_10_semi/ayumu_demo/node_distance.py @@ -9,6 +9,7 @@ def callback(msg): boxes = msg.boxes + # there is no person around the camera if (not boxes): rospy.loginfo("no person") return -1 @@ -16,7 +17,8 @@ def callback(msg): 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 @@ -30,17 +32,15 @@ def callback(msg): if ((-0.6 <= coordinate[0]) and (coordinate[0] < 1.5)): if ((0.3 < coordinate[1]) and (coordinate[1] < 0.6)): - position_number = 1 # left back + position_number = 1 # left elif ((-0.6 < coordinate[1]) and (coordinate[1] < -0.3)): - position_number = 2 # right back - - + position_number = 2 # right min_position = [] min_position.append(position_number) - min_position.append(boxes[min_ind].pose.position.x) - min_position.append(boxes[min_ind].pose.position.y) - min_position.append(boxes[min_ind].pose.position.z) + 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) From 38f1ea9b74adeaf769c34923a7e36a81891824f8 Mon Sep 17 00:00:00 2001 From: Ayumu Iwata Date: Wed, 21 Dec 2022 18:17:22 +0900 Subject: [PATCH 9/9] add panorama_human_3d_detection.launch --- .../panorama_human_3d_detection.launch | 89 +++++++++++++++++++ 1 file changed, 89 insertions(+) create mode 100644 jsk_2022_10_semi/ayumu_demo/panorama_human_3d_detection.launch 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