[semi2022]add Iwata project

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)

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)

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)

jsk_2022_10_semi/launch/human_detector.launch

@@ -0,0 +1,73 @@
+<launch>
+  <arg name="gui" default="true" />
+
+  <arg name="INPUT_PANORAMA_IMAGE" default="/dual_fisheye_to_panorama/output/quater" />
+  <arg name="INPUT_PANORAMA_INFO" default="/dual_fisheye_to_panorama/panorama_info" />
+  <arg name="INPUT_CLASS" default="/edgetpu_panorama_object_detector/output/class" />
+  <arg name="INPUT_RECTS" default="/edgetpu_panorama_object_detector/output/rects" />
+
+  <node pkg="tf" type="static_transform_publisher" name="camera_tf_publisher" args="0 0 0 0 0 -1.57 camera insta360_link 100" />
+
+  <include file="$(find jsk_perception)/sample/sample_insta360_air.launch">
+      <arg name="gui" value="false" />
+  </include>
+
+  <include file="$(find coral_usb)/launch/edgetpu_panorama_object_detector.launch">
+  	<arg name="INPUT_IMAGE" value="/dual_fisheye_to_panorama/output"/>
+  </include>
+
+  <node
+    pkg="image_transport"
+    type="republish"
+    name="panorama_decompress"
+    args="compressed in:=/dual_fisheye_to_panorama/output raw out:=/dual_fisheye_to_panorama/output"
+    />
+  <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>
+
+  <!-- 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>
+
+  <!-- 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_image" to="$(arg INPUT_PANORAMA_IMAGE)" />
+    <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>
+        frame_fixed: "camera"
+        dimensions_labels:
+            person: [0.5, 0.5, 1.5]
+    </rosparam>
+  </node>
+
+  <group if="$(arg gui)">
+    <node
+      pkg="rviz"
+      type="rviz"
+      name="$(anon rviz)"
+      args="-d $(find jsk_perception)/sample/config/sample_rect_array_in_panorama_to_bounding_box_array.rviz"
+    />
+  </group>
+
+</launch>