[jsk_unitree_startup] Add emotion reaction

jsk_unitree_robot/jsk_unitree_startup/apps/emotion_reaction/emotion_reaction.app

@@ -0,0 +1,5 @@
+display: Emotion Reaction
+platform: go1
+launch: jsk_unitree_startup/emotion_reaction.xml
+interface: jsk_unitree_startup/emotion_reaction.interface
+icon: jsk_unitree_startup/emotion_reaction.png

jsk_unitree_robot/jsk_unitree_startup/apps/emotion_reaction/emotion_reaction.interface

@@ -0,0 +1,2 @@
+published_topics: {}
+subscribed_topics: {}

jsk_unitree_robot/jsk_unitree_startup/apps/emotion_reaction/emotion_reaction.xml

@@ -0,0 +1,4 @@
+<launch>
+  <include file="$(find jsk_unitree_startup)/launch/emotion_reaction.launch" >
+  </include>
+</launch>

jsk_unitree_robot/jsk_unitree_startup/apps/unitree_apps.installed

@@ -3,3 +3,5 @@ apps:
     display: go1 watch dog
   - app: jsk_unitree_startup/lead_teleop
     display: go1 lead teleop
+  - app: jsk_unitree_startup/emotion_reaction
+    display: go1 emotion reaction

jsk_unitree_robot/jsk_unitree_startup/autostart/jsk_startup.sh

@@ -30,6 +30,7 @@ if [ "$ROS_IP" == "192.168.123.14" ];then
     while ! eval rostopic info /robotsound 2$toStartlog; do sleep 2; done
     sleep 2 # wait for a while...
     roslaunch jsk_unitree_startup unitree_bringup.launch network:=ethernet &
+    roslaunch jsk_unitree_startup google_chat_ros.launch &
 fi
 
 eval echo "[jsk_startup] done... " $toStartlog

jsk_unitree_robot/jsk_unitree_startup/launch/emotion_reaction.launch

@@ -0,0 +1,8 @@
+<launch>
+
+  <node name="emotion_talker"
+        pkg="jsk_unitree_startup" type="emotion_talker.py" />
+  <node name="emotion_subscriber"
+        pkg="jsk_unitree_startup" type="emotion_subscriber.l" />
+
+</launch>

jsk_unitree_robot/jsk_unitree_startup/launch/google_chat_ros.launch

@@ -0,0 +1,22 @@
+<launch>
+  <arg name="google_chat_credentials_json" default="/var/lib/robot/dialogflow/sparky-pdse-c8cd9692a84f.json" />
+  <arg name="dialogflow_credentials_json" default="/var/lib/robot/dialogflow/facialexpression-rpwe-01533d0109c5.json" />
+
+  <!-- Google Chat ROS -->
+  <include file="$(find google_chat_ros)/launch/google_chat.launch">
+    <arg name="receiving_mode" value="pubsub" />
+    <arg name="project_id" value="sparky-pdse" />
+    <arg name="subscription_id" value="chat-sub" />
+    <arg name="respawn" value="true" />
+    <arg name="google_cloud_credentials_json" value="$(arg google_chat_credentials_json)"/>
+    <arg name="to_dialogflow_client" value="true" />
+  </include>
+
+  <!-- Dialogflow Client ROS -->
+  <include file="$(find dialogflow_task_executive)/launch/dialogflow_ros.launch">
+    <arg name="credential" value="$(arg dialogflow_credentials_json)" />
+    <arg name="project_id" value="facialexpression-rpwe" />
+    <arg name="enable_hotword" value="false" />
+  </include>
+
+</launch>

jsk_unitree_robot/jsk_unitree_startup/launch/unitree_bringup.launch

@@ -39,6 +39,12 @@
         args="call --wait /robot/start_app 'name: jsk_unitree_startup/lead_teleop'"
         output="screen" />
 
+  <!-- start emotion_reaction.launch -->
+  <node name="rosservice"
+        pkg="rosservice" type="rosservice"
+        args="call --wait /robot/start_app 'name: jsk_unitree_startup/emotion_reaction'"
+        output="screen" />
+
   <!-- let people to know unitree is bringup -->
   <node pkg="jsk_unitree_startup" type="wakeup.l"
         name="wakeup" output="log" />

jsk_unitree_robot/jsk_unitree_startup/scripts/emotion_subscriber.l

@@ -0,0 +1,48 @@
+#!/usr/bin/env roseus
+
+(ros::load-ros-manifest "roseus")
+
+(ros::roseus "emotion-lister")
+(load "package://unitreeeus/unitree-interface.l")
+
+(require "package://jsk_unitree_startup/scripts/motions/happy.l")
+(require "package://jsk_unitree_startup/scripts/motions/joy.l")
+(require "package://jsk_unitree_startup/scripts/motions/affirmation.l")
+(require "package://jsk_unitree_startup/scripts/motions/negation.l")
+(require "package://jsk_unitree_startup/scripts/motions/love.l")
+(require "package://jsk_unitree_startup/scripts/motions/scared.l")
+(require "package://jsk_unitree_startup/scripts/motions/curious.l")
+(require "package://jsk_unitree_startup/scripts/motions/astonished.l")
+
+
+(defun emotion-cb(msg)
+  (let ((emotion (send msg :data)))
+    (ros::ros-info "Callback chatting-cb called with ~A" emotion)
+    (cond ((string= emotion "happy")
+	   (happy-main))
+	  ((string=  emotion "joy")
+	   (joy-main))
+	  ((string= emotion "affirmation")
+	   (affirmation-main))
+	  ((string= emotion "negation")
+	   (negation-main))
+	  ((string= emotion "love")
+	   (love-main))
+	  ((string= emotion "scared")
+	   (scared-main))
+	  ((string= emotion "curious")
+	   (curious-main))
+	  ((string= emotion "astonished")
+	   (astonished-main))
+	  (t
+	   (ros::ros-warn "called unknown emotion ~A" emotion)))
+    emotion))
+
+(defun main()
+  (go1-init)
+  (ros::subscribe "/emotion" std_msgs::String #'emotion-cb)
+  (ros::rate 10)
+  (while (ros::ok)
+    (ros::spin-once)))
+
+(main)

jsk_unitree_robot/jsk_unitree_startup/scripts/emotion_talker.py

@@ -0,0 +1,117 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import rospy
+from std_msgs.msg import String
+from speech_recognition_msgs.msg import SpeechRecognitionCandidates
+from jsk_recognition_msgs.msg import PeoplePoseArray
+from dialogflow_task_executive.msg import DialogResponse
+import time
+import message_filters
+
+from threading import Lock
+
+
+class testNode():
+    def __init__(self):
+        # Publisher
+        self.pub = rospy.Publisher('/emotion', String, queue_size=1)
+
+        # Subscriber
+        self.sub1 = rospy.Subscriber('/speech_to_text', SpeechRecognitionCandidates, self.speech_callback)
+        self.sub2 = rospy.Subscriber('/people_pose', PeoplePoseArray, callback=self.callback)
+        self.sub3 = rospy.Subscriber('/dialog_response', DialogResponse, callback=self.df_cb)
+        self.duration_time = rospy.Duration(30)
+        self.prev_pose_detected_time = rospy.Time.now() - self.duration_time
+
+        self.lock = Lock()
+
+        queue_size = 10
+        fps = 100.
+        delay = 1 / fps * 0.5
+
+    def callback(self, msg):
+        if (rospy.Time.now() - self.prev_pose_detected_time) < self.duration_time:
+            return
+        poses = msg.poses
+        if len(poses) >= 1:
+            item = poses[0].limb_names
+            scores = poses[0].scores
+            target_limbs = [
+                "left_eye", "nose", "right_eye", "right_ear",
+                "left_ear"]
+            if all([name in item for name in target_limbs]):
+                message = "happy"
+                rospy.loginfo("received {}, current emotion is {}".format(item, message))
+                with self.lock:
+                    self.publish(message)
+                    self.prev_pose_detected_time = rospy.Time.now()
+                    rospy.sleep(5.0)
+
+    def publish(self, data):
+        self.pub.publish(data)
+
+    def speech_callback(self, msg):
+        word = msg.transcript[0]
+        if word in ["こんにちは",
+                    "ヤッホー",
+                    "こんばんは",
+                    "おはよう",
+                    "おはようございます"]:
+            message = "happy"
+        elif word in ["可愛いね",
+                      "可愛い",
+                      "かわいいね",
+                      "かわいい"]:
+            message = "joy"
+        elif word in ["散歩に行こう",
+                      "散歩",
+                      "行こう",
+                      "いこう"]:
+            message = "affirmation"
+        elif word in ["今日は行けないよ",
+                      "いけないよ",
+                      "行けないよ"]:
+            message = "negation"
+        elif word in ["大好きだよ",
+                      "好き",
+                      "すき"]:
+            message = "love"
+        elif word in ["あっち行って",
+                      "さようなら"]:
+            message = "scared"
+        else:
+            message = word
+        rospy.loginfo("received {}, current emotion is {}".format(word, message))
+        with self.lock:
+            self.publish(message)
+            rospy.sleep(5.0)
+
+    def df_cb(self, data):
+        if data.action == "Happy" or data.action == "input.welcome":
+            self.publish("happy")
+        elif data.action == "Smirking" or data.action == "Squinting":
+            self.publish("joy")
+        elif data.action == "Love":
+            self.publish("love")
+        elif data.action == "Fearful" or data.action == "Cry":
+            self.publish("scared")
+        elif data.action == "Relived":
+            self.publish("affirmation")
+        elif data.action == "Boring" or data.action == "Unpleasant":
+            self.publish("negation")
+        elif data.action == "input.unknown":
+            self.publish("curious")
+        elif data.action == "Angry" or data.action == "Astonished":
+            self.publish("astonished")
+        else:
+            rospy.logwarn("Unknown emotion")
+
+if __name__ == '__main__':
+    rospy.init_node('speech_to_emotion')
+
+    time.sleep(3.0)
+    node = testNode()
+
+    while not rospy.is_shutdown():
+        rospy.sleep(0.1)

jsk_unitree_robot/jsk_unitree_startup/scripts/motions/affirmation.l

@@ -0,0 +1,46 @@
+#!/usr/bin/env roseus                                                                               \
+
+(ros::load-ros-manifest "roseus")
+(ros::roseus "unitree-affirmation")
+;;(load "package://unitreeeus/unitree-interface.l")
+
+;;down
+(defun affirmation-pose-1(&optional (time 3000))
+  (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector))
+         (send *ri* :body-pose '(0 0.2 0))
+         ))
+;;up
+(defun affirmation-pose-2(&optional (time 3000))
+  (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector))
+         (send *ri* :body-pose '(0 -0.2 0))
+         ))
+
+(defun reset-pose(&optional (time 3000))
+  (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector))
+         (send *go1* :angle-vector #f(0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0))
+         (send *ri* :body-pose '(0 0 0))
+         ))
+
+(defun affirmation-main()
+  (print "affirmation")
+  ;;(go1-init)
+  (reset-pose 1000)
+  (affirmation-pose-1 800)
+  (unix:usleep(* 1000 400))
+  (affirmation-pose-2 800)
+  (unix:usleep(* 1000 400))
+  (affirmation-pose-1 800)
+  (unix:usleep(* 1000 400))
+  (affirmation-pose-2 800)
+  (unix:usleep(* 1000 600))
+
+  (reset-pose 1000)
+  (affirmation-pose-1 800)
+  (unix:usleep(* 1000 400))
+  (affirmation-pose-2 800)
+  (unix:usleep(* 1000 400))
+  (affirmation-pose-1 800)
+  (unix:usleep(* 1000 400))
+  (affirmation-pose-2 800)
+  (unix:usleep(* 1000 600))
+  )

jsk_unitree_robot/jsk_unitree_startup/scripts/motions/astonished.l

@@ -0,0 +1,41 @@
+#!/usr/bin/env roseus
+
+(ros::load-ros-manifest "roseus")
+(ros::roseus "unitree-astonished")
+;;(load "package://unitreeeus/unitree-interface.l")
+
+(defun astonished-pose-1(&optional (time 3000))
+  (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector))
+	 (send *go1* :body-pose (coords :pos #f(0 0 30)))
+	 (send *ri* :body-pose (coords :pos #f(0 0 30)))
+	 ))
+
+(defun astonished-pose-2(&optional (time 3000))
+  (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector))
+	 (send *go1* :body-pose '(0 -0.4 0))
+	 (send *ri* :body-pose '(0 -0.4 0))
+	 ))
+
+(defun reset-pose(&optional (time 3000))
+  (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector))
+	 (send *go1* :angle-vector #f(0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0))
+	 (send *ri* :body-pose '(0 0 0))
+	 ))
+
+(defun astonished-main()
+  ;;(print "astonished")
+  ;;(go1-init)
+  ;;(make-irtviewer)
+  ;;(send *irtviewer* :draw-objects)
+  ;;(objects (list *go1*))
+  (reset-pose 1000)
+  (astonished-pose-1 800)
+  (unix:usleep (* 1000 1000))
+  (reset-pose 800)
+  (unix:usleep (* 1000 400))
+  (astonished-pose-2 800)
+  (unix:usleep (* 1000 1000))
+  (reset-pose 800)
+  (unix:usleep (* 1000 800))
+
+  )

jsk_unitree_robot/jsk_unitree_startup/scripts/motions/curious.l

@@ -0,0 +1,45 @@
+#!/usr/bin/env roseus
+(ros::load-ros-manifest "roseus")
+(ros::roseus "unitree-curious")
+(load "package://unitreeeus/unitree-interface.l")
+
+(defun curious-pose-1(&optional (time 3000))
+  (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector))
+	 (send *go1* :body-pose '(0 -0.4 0))
+	 (send *ri* :body-pose '(0 -0.4 0))
+	 ))
+
+(defun curious-pose-2(&optional (time 3000))
+  (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector))
+	 (send *go1* :body-pose '(0.4 -0.4 0))
+	 (send *ri* :body-pose '(0.4 -0.4 0))
+	 ))
+
+(defun curious-pose-3(&optional (time 3000))
+  (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector))
+	 (send *go1* :body-pose '(-0.4 -0.4 0))
+	 (send *ri* :body-pose '(-0.4 -0.4 0))
+	 ))
+
+(defun reset-pose(&optional (time 3000))
+  (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector))
+	 (send *go1* :angle-vector #f(0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0))
+	 (send *ri* :body-pose '(0 0 0))
+	 ))
+
+(defun curious-main()
+  (print "curious")
+  (go1-init)
+  ;;(make-irtviewer)
+  ;;(send *irtviewer* :draw-objects)
+  ;;(objects (list *go1*))
+  (reset-pose 1000)
+  (curious-pose-1 800)
+  (unix:usleep (* 1000 800))
+  (curious-pose-2 800)
+  (unix:usleep (* 1000 1500))
+  (curious-pose-3 800)
+  (unix:usleep (* 1000 1500))
+  (reset-pose 800)
+  (unix:usleep (* 1000 800))
+  )