[semi2022]add Iwata project

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)))))

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)

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)))))

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()

jsk_2022_10_semi/ayumu_demo/panorama_human_3d_detection.launch

@@ -0,0 +1,89 @@
+<launch>
+  <arg name="launch_insta360" default="true" />
+  <arg name="launch_coral" default="true" />
+
+  <arg name="publish_image" default="true" />
+  <arg name="split_image" default="true" />
+  <arg name="camera_name" default="insta360" />
+  <arg name="create_panorama" default="true" />
+  <arg name="throttle" default="false" />
+  <arg name="panorama_resolution_mode" default="low" />
+  <arg name="resize" default="false" />
+
+  <arg name="frame_fixed" default="camera" />
+  <arg name="static_transform_publisher" default="true" />
+  <arg name="gui" default="true" />
+
+  <arg name="INPUT_IMAGE" default="/$(arg camera_name)/image_raw" doc="topic name of 360 camera image" />
+  <arg unless="$(arg resize)"
+       name="INPUT_PANORAMA_IMAGE" default="/dual_fisheye_to_panorama/output" />
+  <arg if="$(arg resize)"
+       name="INPUT_PANORAMA_IMAGE" default="/dual_fisheye_to_panorama/output/quater" />
+  <arg unless="$(arg resize)"
+       name="INPUT_PANORAMA_INFO" default="/dual_fisheye_to_panorama/panorama_info" />
+  <arg if="$(arg resize)"
+       name="INPUT_PANORAMA_INFO" default="/dual_fisheye_to_panorama/panorama_info/quater" />
+  <arg name="INPUT_CLASS" default="/edgetpu_panorama_object_detector/output/class" />
+  <arg name="INPUT_RECTS" default="/edgetpu_panorama_object_detector/output/rects" />
+
+  <include if="$(arg launch_insta360)"
+           file="$(find jsk_perception)/launch/insta360_air.launch">
+    <arg name="publish_image" value="$(arg publish_image)" />
+    <arg name="split_image" value="$(arg split_image)" />
+    <arg name="camera_name" value="$(arg camera_name)" />
+    <arg name="create_panorama" value="$(arg create_panorama)" />
+    <arg name="throttle" value="$(arg throttle)" />
+    <arg name="panorama_resolution_mode" value="$(arg panorama_resolution_mode)" />
+    <arg name="gui" value="false" />
+  </include>
+
+  <group if="$(arg resize)">
+    <node pkg="nodelet" type="nodelet" name="panorama_image_resize"
+          args="standalone image_proc/resize">
+      <param name="scale_height" value="0.5" />
+      <param name="scale_width" value="0.5" />
+      <remap from="image" to="/dual_fisheye_to_panorama/output" />
+      <remap from="~image" to="/dual_fisheye_to_panorama/output/quater" />
+    </node>
+
+    <node pkg="jsk_recognition_utils" type="resize_panorama_info.py" name="panorama_info_resize">
+      <param name="scale_height" value="0.5" />
+      <param name="scale_width" value="0.5" />
+      <remap from="~input" to="/dual_fisheye_to_panorama/panorama_info" />
+      <remap from="~output" to="/dual_fisheye_to_panorama/panorama_info/quater" />
+    </node>
+  </group>
+
+  <include if="$(arg launch_coral)"
+           file="$(find coral_usb)/launch/edgetpu_panorama_object_detector.launch">
+    <arg name="INPUT_IMAGE" value="$(arg INPUT_PANORAMA_IMAGE)" />
+  </include>
+
+  <node if="$(arg static_transform_publisher)"
+        name="camera_transform_publisher" pkg="tf" type="static_transform_publisher"
+        args="0 0 0 0 0 0 $(arg camera_name)_link camera 10"/>
+
+  <!-- rect_array_in_panorama_to_bounding_box_array -->
+  <node pkg="jsk_perception" type="rect_array_in_panorama_to_bounding_box_array.py"
+        name="rect_array_in_panorama_to_bounding_box_array">
+    <remap from="~panorama_info" to="$(arg INPUT_PANORAMA_INFO)" />
+    <remap from="~input_class" to="$(arg INPUT_CLASS)" />
+    <remap from="~input_rects" to="$(arg INPUT_RECTS)" />
+    <rosparam subst_value="true">
+        frame_fixed: $(arg frame_fixed)
+        dimensions_labels:
+            person: [0.5, 0.5, 1.5]
+    </rosparam>
+  </node>
+
+  <!-- visualize output of object detector -->
+  <node pkg="jsk_perception" type="draw_rects" name="draw_rects">
+    <remap from="~input" to="$(arg INPUT_PANORAMA_IMAGE)" />
+    <remap from="~input/rects" to="$(arg INPUT_RECTS)" />
+    <remap from="~input/class" to="$(arg INPUT_CLASS)" />
+  </node>
+
+  <node if="$(arg gui)" pkg="rviz" type="rviz" name="$(anon rviz)"
+        args="-d $(find jsk_perception)/sample/config/sample_rect_array_in_panorama_to_bounding_box_array.rviz" />
+
+</launch>

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)

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)