[merge after https://github.com/k-okada/jsk_robot/pull/45 ] add google_chat_ros dialogflow_task_executive in package.xml

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,3 @@
+<launch>
+  <include file="$(find jsk_unitree_startup)/launch/emotion_reaction.launch" />
+</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 emotoin reaction

jsk_unitree_robot/jsk_unitree_startup/launch/emotion_reaction.launch

@@ -0,0 +1,25 @@
+<launch>
+  <arg name="google_chat_credentials_json" />
+  <arg name="dialogflow_credentials_json" />
+
+  <node pkg="jsk_unitree_startup" type="emotion_talker.py" name="emotion_talker" />
+  <node pkg="jsk_unitree_startup" type="emotion_subscriber.l" name="emotion_subscriber" />
+
+  <!-- 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

@@ -33,6 +33,10 @@
 
   <include file="$(find app_manager)/launch/app_manager.launch" />
 
+  <include file="$(find respeaker_ros)/launch/sample_respeaker.launch" >
+    <arg name="language" value="ja-JP" />
+  </include>
+
   <!-- 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/package.xml

@@ -15,6 +15,8 @@
   <run_depend>app_manager</run_depend>
   <run_depend>rosserial_python</run_depend>
   <run_depend>teleop_twist_joy</run_depend>
+  <run_depend>dialogflow_task_executive</run_depend>
+  <run_depend>google_chat_ros</run_depend>
 
   <!-- for diagnostic aggregator -->
   <run_depend>diagnostic_aggregator</run_depend>

jsk_unitree_robot/jsk_unitree_startup/scripts/emotion_subscriber.l

@@ -0,0 +1,41 @@
+#!/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")
+
+(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))
+	  (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,111 @@
+#!/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):
+        # TODO to Ichikura, mapping more emotions to more behaviors
+        if data.action == "Happy":
+            self.publish("happy")
+        elif data.action == "Smirking":
+            self.publish("joy")
+        elif data.action == "Love":
+            self.publish("love")
+        elif data.action == "Fearful":
+            self.publish("scared")
+        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/emotions.l

@@ -0,0 +1,83 @@
+#!/usr/bin/env roseus
+
+;; load simple model for fast-boot
+(require :go1 "package://unitreeeus/go1-simple.l")
+
+;; copied from https://github.com/jsk-ros-pkg/jsk_demos/blob/0a82540da013b8e21307354bb1c6a552185e36a2/jsk_spot_watch_dog/scripts/watch-dog.l
+(load "package://unitreeeus/unitree-interface.l")
+
+(ros::roseus-add-msgs "jsk_recognition_msgs")
+(ros::roseus-add-msgs "speech_recognition_msgs")
+;;;
+;;; Happy
+;;;
+(defun happy-pose-1(&optional (time 3000))
+  (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector))
+	 (send *go1* :body-pose '(0.4 -0.4 -0.2))
+	 (send *ri* :body-pose '(0.4 -0.4 -0.2))
+	 ))
+
+(defun happy-pose-2(&optional (time 3000))
+  (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector))
+	 (send *go1* :body-pose '(-0.4 -0.4 0.2))
+	 (send *ri* :body-pose '(-0.4 -0.4 0,2))
+	 ))
+
+(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 happy ()
+  (reset-pose 1000)
+  (happy-pose-1 800)
+  (unix:usleep (* 1000 1000))
+  (reset-pose 800)
+  (unix:usleep (* 1000 800))
+  (happy-pose-2 800)
+  (unix:usleep (* 1000 1000))
+  (reset-pose 800)
+  (unix:usleep (* 1000 800))
+
+  (happy-pose-1 800)
+  (unix:usleep (* 1000 1000))
+  (reset-pose 800)
+  (unix:usleep (* 1000 800))
+  (happy-pose-2 800)
+  (unix:usleep (* 1000 1000))
+  (reset-pose 800)
+  (unix:usleep (* 1000 800))
+  )
+
+;;;
+;;; Main
+;;;
+
+(go1-init)
+(ros::ros-info "go1-initialized")
+
+(defun people-cb (msg)
+  (ros::ros-info "~A" (send-all (send msg :poses) :limb_names))
+  (if (> (length (send msg :poses)) 0)
+      (happy))
+  )
+(defun speech-cb (msg)
+  (ros::ros-info "~A" (send msg :transcript))
+  (if (string= (car (send msg :transcript)) "こんにちは")
+      (happy)
+      ))
+(ros::subscribe "people_pose" jsk_recognition_msgs::PeoplePoseArray #'people-cb)
+(ros::subscribe "speech_to_text" speech_recognition_msgs::SpeechRecognitionCandidates #'speech-cb)
+
+(ros::rate 10)
+(do-until-key
+    (ros::sleep)
+    (ros::spin-once))
+;; (send *ri* :play-sound
+;;       (pathname (ros::get-param "~bark_sound" "/opt/jsk/User/src/jsk_robot/jsk_unitree_robot/jsk_unitree_startup/autostart/bark.wav")))
+;; (ros::ros-info "send :play-sound, wait for 2 sec..")
+;; (unix::sleep 2)
+;; (ros::ros-info "exitting...")
+;; (sys::exit 0)
+;

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