[semi2022] add sahara project

jsk_2022_10_semi/euslisp/sahara/all.lisp

@@ -0,0 +1,2 @@
+(load "init.lisp")
+(load "test.lisp")

jsk_2022_10_semi/euslisp/sahara/camera.launch

@@ -0,0 +1,34 @@
+<launch>
+    <arg name="spot" default="true" />
+    <arg name="spot_coral" default="true" />
+    <arg name="detector" default="$(find coral_usb)/launch/semi_2022.launch" />
+    <!-- <arg name="detector" default="$(find coral_usb)/launch/edgetpu_object_detector.launch" /> -->
+    <arg name="score_thresh" value="0.4" />
+    <group unless="$(arg spot_coral)">
+        <group if="$(arg spot)">
+            <include file="$(arg detector)">
+                <arg name="IMAGE_TRANSPORT" value="compressed" />
+                <arg name="INPUT_IMAGE" value="/spot/camera/hand_color/image" />
+                <arg name="score_thresh" value="$(arg score_thresh)" />
+            </include>
+        </group>
+        <group unless="$(arg spot)">
+            <node pkg="usb_cam" type="usb_cam_node" name="usb_cam" />
+            <include file="$(arg detector)">
+                <arg name="IMAGE_TRANSPORT" value="compressed" />
+                <arg name="INPUT_IMAGE" value="/usb_cam/image_raw" />
+                <arg name="score_thresh" value="$(arg score_thresh)" />
+            </include>
+        </group>
+        <node pkg="image_view" type="image_view" name="image_view">
+            <remap from="image" to="/edgetpu_object_detector/output/image" />
+        </node>
+    </group>
+    <group if="$(arg spot_coral)">
+        <node pkg="image_view" type="image_view" name="image_view">
+            <remap from="image" to="/edgetpu_object_detector/output/image/compressed" />
+        </node>
+    </group>
+    <!-- <node pkg="rqt_reconfigure" type="rqt_reconfigure" name="rqt_reconfigure">
+    </node> -->
+</launch>

jsk_2022_10_semi/euslisp/sahara/demo.l

@@ -0,0 +1,28 @@
+(load "package://fetcheus/fetch-interface.l")
+;git clone https://github.com/jsk-ros-pkg/jsk_pr2eus.git
+;rosdep install --from-path . --ignore-src -y -r
+;catkin build
+;rlwrap roseus
+
+(unless (boundp '*ri*) (fetch-init))
+(objects (list *fetch*))
+
+(send *fetch* :reset-pose)
+(send *irtviewer* :draw-objects)
+
+;send *ri* :state :potentio-vector
+;send *fetch* :angle-vector (send *ri* :state :potentio-vector)
+;send *ri* :angle-vector (send *fetch* :angle-vector) 5000
+
+;(setq *rarm-end* (send *fetch* :rarm :end-coords :copy-worldcoords))
+;(send *rarm-end* :translate (float-vector 10.0 10.0 -10.0))
+;(send *fetch* :rarm :inverse-kinematics *rarm-end* 2000)
+(send *fetch* :rarm :shoulder-y :joint-angle)
+(send *fetch* :rarm :shoulder-y :joint-angle 30)
+(send *fetch* :rarm :shoulder-r :joint-angle -120)
+(send *fetch* :rarm :elbow-r :joint-angle 30)
+(send *irtviewer* :draw-objects)
+
+;(#<bodyset-link #X55cc21e03b50 shoulder_pan_link  32.65 0.0 746.01 / 1.309 0.0 0.0> #<bodyset-link #X55cc21e132e0 shoulder_lift_link  62.932 113.013 806.01 / 1.309 1.4 -8.327e-17> #<bodyset-link #X55cc21e0c6e0 upperarm_roll_link  72.566 148.968 590.197 / 1.309 1.4 -0.2> #<bodyset-link #X55cc22c459c8 elbow_flex_link  78.417 170.803 459.132 / -1.635 0.018 -3.108> #<bodyset-link #X55cc22d63218 forearm_roll_link  65.816 -25.761 455.538 / -1.635 0.018 -2.933> #<bodyset-link #X55cc22da9518 wrist_flex_link  57.853 -149.985 453.267 / 2.744 -1.351 -1.249> #<bodyset-link #X55cc22dd2d10 wrist_roll_link  30.014 -138.306 588.437 / 2.744 -1.351 -1.249>)
+;(20.0 -70.0 80.2141 70.0 98.5487 10.0 70.0 0.0 0.0 0.0)
+;(20.7292 43.1076 -34.5316 -56.8115 64.3184 -41.7296 66.5712 -0.006954 -0.753425 1.62224)

jsk_2022_10_semi/euslisp/sahara/demo_copy.l

@@ -0,0 +1,32 @@
+(load "package://fetcheus/fetch-interface.l")
+(fetch-init)
+
+;(objects (list *fetch*))
+
+;;(print (send *ri* :state :potentio-vector))
+;;(send *fetch* :angle-vector (send *ri* :state :potentio-vector))
+;;reset (list 17.3036 75.715 80.1657 -11.5478 98.5958 -0.003545 95.1357 -0.028927 0.56082 0.567554)
+;;      (list 17.2349 75.6931 80.4074 -12.0312 102.221 -0.04749 51.498 -0.072829 0.368209 1.68816)
+;;      (list 17.2731 75.6931 80.4733 -65.4028 102.177 -0.025528 51.476 -0.072829 0.413527 1.66618)
+;;   (17.2654 75.737 80.4294 -65.4028 102.111 0.0404 51.454 -0.138747 18.2804 -19.6473)
+(print "start")
+(send *fetch* :reset-pose)
+(send *ri* :angle-vector (send *fetch* :angle-vector) 5000)
+(send *ri* :wait-interpolation)
+(print "first")
+(send *fetch* :angle-vector (list 17.2349 75.6931 80.4074 -12.0312 102.221 -0.04749 51.498 -0.072829 0.368209 1.68816))
+(send *ri* :angle-vector (send *fetch* :angle-vector) 5000)
+(send *ri* :wait-interpolation)
+(print "second")
+(send *fetch* :angle-vector (list 17.2731 75.6931 80.4733 -65.4028 102.177 -0.025528 51.476 -0.072829 0.413527 1.66618))
+(send *ri* :angle-vector (send *fetch* :angle-vector) 5000)
+(send *ri* :wait-interpolation)
+(print "third")
+(send *fetch* :angle-vector (list 17.2654 75.737 80.4294 -65.4028 102.111 0.0404 51.454 -0.138747 18.2804 -19.6473))
+(send *ri* :angle-vector (send *fetch* :angle-vector) 5000)
+(send *ri* :wait-interpolation)
+(print "third")
+(send *fetch* :reset-pose)
+(send *ri* :angle-vector (send *fetch* :angle-vector) 5000)
+(send *ri* :wait-interpolation)
+(print "finish")

jsk_2022_10_semi/euslisp/sahara/find_ball.py

@@ -0,0 +1,42 @@
+#!/usr/bin/env python
+import rospy
+import message_filters
+from jsk_recognition_msgs.msg import ClassificationResult,RectArray
+rospy.init_node("find_ball")
+
+
+def callback_each(class_msg,rect_msg,label_name,publisher):
+    # print "callback"
+    # print(class_msg.labels)
+    # print(class_msg.label_names)
+    ball_rect_list = []
+    if len(rect_msg.rects)>=len(class_msg.label_names):
+        for i,v in enumerate(class_msg.label_names):
+            if v==label_name :
+                ball_rect_list.append(rect_msg.rects[i])
+    if len(ball_rect_list) > 0:
+        ball_rectarray = RectArray()
+        ball_rectarray.header = rect_msg.header
+        ball_rectarray.rects = ball_rect_list
+        print(ball_rectarray)
+        print(label_name,class_msg.label_proba[0])
+        publisher.publish(ball_rectarray)
+
+def callback(class_msg,rect_msg):
+    callback_each(class_msg,rect_msg,"lined_ball",pub_lined_ball_rect)
+    callback_each(class_msg,rect_msg,"yellow_ball",pub_yellow_ball_rect)
+    callback_each(class_msg,rect_msg,"white_ball",pub_white_ball_rect)
+
+pub_lined_ball_rect = rospy.Publisher('/find_ball/lined_ball_rect',RectArray , queue_size=1)
+pub_yellow_ball_rect = rospy.Publisher('/find_ball/yellow_ball_rect',RectArray , queue_size=1)
+pub_white_ball_rect = rospy.Publisher('/find_ball/white_ball_rect',RectArray , queue_size=1)
+
+
+sub_class = message_filters.Subscriber('/edgetpu_object_detector/output/class', ClassificationResult)
+sub_rect = message_filters.Subscriber('/edgetpu_object_detector/output/rects', RectArray)
+
+sync = message_filters.ApproximateTimeSynchronizer([sub_class,sub_rect], 10, 0.5)
+sync.registerCallback(callback)
+
+print "start"
+rospy.spin()

jsk_2022_10_semi/euslisp/sahara/init.lisp

@@ -0,0 +1,10 @@
+(load "package://spoteus/spot-interface.l")
+(ros::roseus-add-msgs "jsk_recognition_msgs")
+(if t ;実機につなぐなら
+    (spot-init)
+    (setq *spot* (instance spot-robot :init))
+)
+;(send *ri* :speak-jp "こんにちは")
+(send *ri* :speak "initialized")
+
+(objects (list *spot*))

jsk_2022_10_semi/euslisp/sahara/pick_object_test.l

@@ -0,0 +1,25 @@
+#!/usr/bin/env roseus
+
+(load "package://spoteus/spot-interface.l")
+(ros::roseus-add-msgs "jsk_recognition_msgs")
+
+(spot-init nil)
+(setq *image-source* "hand_color")
+
+(ros::subscribe "/edgetpu_object_detector/output/rects" jsk_recognition_msgs::RectArray #'cb)
+
+(defun pick-object-in-image (image-source center-x center-y)
+  (send *ri* :pick-object-in-image image-source center-x center-y))
+
+(defun cb (msg)
+  (let ((x (send (send msg :rects) :x))
+        (y (send (send msg :rects) :y))
+        (width (send (send msg :rects) :width))
+        (height (send (send msg :rects) :height))
+        (center-x (+ x (/ width 2)))
+        (center-y (+ y (/ height 2))))
+    (pick-object-in-image *image-source* center-x center-y)
+    (unix::sleep 5)
+    (send *ri* :stow-arm)
+    ;;(send *ri* :wait-interpolation)                                                                
+    ))

jsk_2022_10_semi/euslisp/sahara/ri_methods

@@ -0,0 +1,151 @@
+:init
+:default-controllerarm-controller:set-impedance-param
+:unstow-arm
+:stow-arm
+:gripper-open
+:gripper-close
+:start-grasp
+:stop-grasp
+:pick-object-in-image
+:pick-object-in-image-feedback-cb
+:pick-object-in-image-feedback-msg
+:pick-object-in-image-wait-for-result
+:pick-object-in-image-get-state
+:spot-status-metrics-callback
+:spot-status-leases-callback
+:spot-status-feet-callback
+:spot-status-estop-callback
+:spot-status-wifi-callback
+:spot-status-manipulator-state-callback
+:spot-status-power-state-callback
+:spot-status-battery-states-callback
+:spot-status-behavior-faults-callback
+:spot-status-system-faults-callback
+:spot-feedback-callback
+:state
+:estop-gentle
+:estop-hard
+:claim
+:release
+:power-on
+:power-off
+:self-right
+:stand
+:sit
+:stop
+:set-locomotion-mode
+:set-stair-mode
+:dock
+:undock
+:send-cmd-vel-raw
+:go-velocity
+:go-pos
+:go-pos-wait
+:go-pos-no-wait
+:body-pose
+:find-waypoint-position-from-id
+:find-waypoint-id-from-position
+:initial-localization-fiducial
+:initial-localization-waypoint
+:upload-path
+:list-graph
+:navigate-to
+:create-quaternion-from-rpy
+:create-quaternion-msg-from-rpy
+:init
+:odom-callback
+:go-stop
+:make-plan
+:move-to
+:move-to-send
+:move-to-wait
+:go-waitp
+:go-pos
+:go-pos-no-wait
+:go-wait
+:send-cmd-vel-raw
+:go-velocity
+:go-pos-unsafe
+:go-pos-unsafe-no-wait
+:go-pos-unsafe-wait
+:move-trajectory-sequence
+:move-trajectory
+:state
+:robot-interface-simulation-callback
+:clear-costmap
+:change-inflation-range
+:init
+:add-controller
+:robot-interface-simulation-callback
+:publish-joint-state
+:angle-vector-safe
+:angle-vector-duration
+:angle-vector-simulation
+:angle-vector
+:angle-vector-sequence
+:wait-interpolation
+:interpolatingp
+:wait-interpolation-smooth
+:interpolating-smoothp
+:stop-motion
+:cancel-angle-vector
+:worldcoords
+:torque-vector
+:potentio-vector
+:reference-vector
+:actual-vector
+:error-vector
+:state-vector
+:stamp
+:send-ros-controller
+:set-robot-state1
+:get-robot-state
+:ros-state-callback
+:wait-until-update-all-joints
+:update-robot-state
+:state
+:default-controller
+:sub-angle-vector
+:robot
+:viewer
+:objects
+:set-simulation-default-look-at
+:draw-objects
+:find-object
+:joint-action-enable
+:simulation-modep
+:warningp
+:spin-once
+:send-trajectory
+:send-trajectory-each
+:ros-wait
+:go-pos
+:go-pos-no-wait
+:go-wait
+:go-velocity
+:go-stop
+:gripper
+:joint-trajectory-to-angle-vector-list
+:show-goal-hand-coords
+:find-descendants-dae-links
+:show-mesh-traj-with-color
+:nod
+:eus-mannequin-mode
+:play-sound
+:speak
+:speak-en
+:speak-jp
+:plist
+:get
+:put
+:name
+:remprop
+:prin1
+:prin1
+:warning
+:error
+:slots
+:methods
+:super
+:get-val 
+:set-val