From a6024b89f64b4870668dff1db909b47c24d71e17 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 16 Oct 2024 16:07:47 +0900 Subject: [PATCH 1/6] add initial jsk_2024_10_semi files --- jsk_2024_10_semi/CMakeLists.txt | 11 +++++++++++ jsk_2024_10_semi/README.md | 3 +++ jsk_2024_10_semi/package.xml | 19 +++++++++++++++++++ 3 files changed, 33 insertions(+) create mode 100644 jsk_2024_10_semi/CMakeLists.txt create mode 100644 jsk_2024_10_semi/README.md create mode 100644 jsk_2024_10_semi/package.xml diff --git a/jsk_2024_10_semi/CMakeLists.txt b/jsk_2024_10_semi/CMakeLists.txt new file mode 100644 index 0000000000..fdd8e3d67b --- /dev/null +++ b/jsk_2024_10_semi/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 2.8.3) +project(jsk_2024_10_semi) + +find_package(catkin REQUIRED COMPONENTS) + +catkin_package() + +install(DIRECTORY euslisp + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + diff --git a/jsk_2024_10_semi/README.md b/jsk_2024_10_semi/README.md new file mode 100644 index 0000000000..acd4388fa2 --- /dev/null +++ b/jsk_2024_10_semi/README.md @@ -0,0 +1,3 @@ +# jsk_2024_10_semi + + diff --git a/jsk_2024_10_semi/package.xml b/jsk_2024_10_semi/package.xml new file mode 100644 index 0000000000..ff815d0b49 --- /dev/null +++ b/jsk_2024_10_semi/package.xml @@ -0,0 +1,19 @@ + + + jsk_2024_10_semi + 0.0.5 + The jsk_2024_10_semi package + + Kei Okada + + BSD + + catkin + + fetcheus + jsk_maps + pr2eus + + + + From 4b4f0f5dc60cb7f7a007a3ddb4ce29ae65afe6eb Mon Sep 17 00:00:00 2001 From: Rinno1478 Date: Wed, 30 Oct 2024 18:52:07 +0900 Subject: [PATCH 2/6] add catkin_virtualenv for google-generativeai --- jsk_2024_10_semi/CMakeLists.txt | 16 +++++++++++++++- jsk_2024_10_semi/package.xml | 3 +++ jsk_2024_10_semi/requirements.txt | 13 +++++++++++++ 3 files changed, 31 insertions(+), 1 deletion(-) create mode 100644 jsk_2024_10_semi/requirements.txt diff --git a/jsk_2024_10_semi/CMakeLists.txt b/jsk_2024_10_semi/CMakeLists.txt index fdd8e3d67b..3c033f7f00 100644 --- a/jsk_2024_10_semi/CMakeLists.txt +++ b/jsk_2024_10_semi/CMakeLists.txt @@ -1,11 +1,25 @@ cmake_minimum_required(VERSION 2.8.3) project(jsk_2024_10_semi) -find_package(catkin REQUIRED COMPONENTS) +find_package(catkin REQUIRED COMPONENTS catkin_virtualenv) catkin_package() +# Generate the virtualenv +catkin_generate_virtualenv( + PYTHON_INTERPRETER python3.10 + USE_SYSTEM_PACKAGES FALSE # Default TRUE + + # Provide extra arguments to the underlying pip invocation + #EXTRA_PIP_ARGS -vvv + ) + install(DIRECTORY euslisp DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) +install(FILES requirements.txt + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +catkin_install_python( + PROGRAMS scripts/conversation.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/jsk_2024_10_semi/package.xml b/jsk_2024_10_semi/package.xml index ff815d0b49..4fa478aaa5 100644 --- a/jsk_2024_10_semi/package.xml +++ b/jsk_2024_10_semi/package.xml @@ -10,10 +10,13 @@ catkin + catkin_virtualenv + fetcheus jsk_maps pr2eus + requirements.txt diff --git a/jsk_2024_10_semi/requirements.txt b/jsk_2024_10_semi/requirements.txt new file mode 100644 index 0000000000..1fecb40eaf --- /dev/null +++ b/jsk_2024_10_semi/requirements.txt @@ -0,0 +1,13 @@ +google-ai-generativelanguage==0.6.10 +google-api-core==2.22.0 +google-api-python-client==2.149.0 +google-auth==2.35.0 +google-auth-httplib2==0.2.0 +google-generativeai==0.8.3 +googleapis-common-protos==1.65.0 +protobuf==5.28.3 +grpcio==1.67.1 +grpcio-status==1.67.1 +# +speechrecognition +gtts From 221d59542e8ad2e3469b7b5b954c8d036bbdaa4c Mon Sep 17 00:00:00 2001 From: Rinno1478 Date: Wed, 6 Nov 2024 17:29:01 +0900 Subject: [PATCH 3/6] fix gemini_api_key in conversation.py --- jsk_2024_10_semi/chabudai.l | 44 ++++++ jsk_2024_10_semi/scripts/conversation.py | 176 +++++++++++++++++++++++ jsk_2024_10_semi/scripts/demo-box.l | 41 ++++++ jsk_2024_10_semi/scripts/gomoku.l | 96 +++++++++++++ jsk_2024_10_semi/scripts/test.l | 28 ++++ 5 files changed, 385 insertions(+) create mode 100644 jsk_2024_10_semi/chabudai.l create mode 100644 jsk_2024_10_semi/scripts/conversation.py create mode 100644 jsk_2024_10_semi/scripts/demo-box.l create mode 100644 jsk_2024_10_semi/scripts/gomoku.l create mode 100644 jsk_2024_10_semi/scripts/test.l diff --git a/jsk_2024_10_semi/chabudai.l b/jsk_2024_10_semi/chabudai.l new file mode 100644 index 0000000000..74b896ae5a --- /dev/null +++ b/jsk_2024_10_semi/chabudai.l @@ -0,0 +1,44 @@ +(require "package://pr2eus/pr2.l") ;;import pr2 package +(require "package://pr2eus/pr2-utils.l") +(require "package://pr2eus/pr2-interface.l") +(if (not (boundp '*pr2*)) (setq *pr2* (pr2)));; (setq *pr2* (pr2))) ;; (pr2) is alternated by *pr2* + +; (send *pr2* :reset-pose) ;; 初期姿勢 +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) +; (objects (list *pr2*)) + +;; 一辺600mmの立方体を出現させる +(setq *cube* (make-cube 600 600 600)) +;; 立方体を(700, 0, 300)移動 +(send *cube* :translate (float-vector 700 0 300)) +(objects (list *pr2*)) + +;; PR2の腕を指定の角度に動かす +(send *pr2* :larm :shoulder-p :joint-angle 15) +(send *pr2* :larm :shoulder-r :joint-angle 90) +(send *pr2* :larm :collar-y :joint-angle 45) +(send *pr2* :larm :elbow-p :joint-angle -100) +(send *pr2* :larm :elbow-r :joint-angle 0) +(send *pr2* :larm :wrist-p :joint-angle -10) +(send *pr2* :larm :wrist-r :joint-angle 0) +(send *pr2* :rarm :shoulder-p :joint-angle 15) +(send *pr2* :rarm :shoulder-r :joint-angle -90) +(send *pr2* :rarm :collar-y :joint-angle -45) +(send *pr2* :rarm :elbow-p :joint-angle -100) +(send *pr2* :rarm :elbow-r :joint-angle 0) +(send *pr2* :rarm :wrist-p :joint-angle -10) +(send *pr2* :rarm :wrist-r :joint-angle 0) +(send *pr2* :head :neck-p :joint-angle 50) +(send *irtviewer* :draw-objects) + +;; sleep 2秒 +(unix:sleep 2) +(send *pr2* :larm :shoulder-p :joint-angle 0) +(send *pr2* :larm :shoulder-r :joint-angle 5) +(send *pr2* :rarm :shoulder-p :joint-angle 0) +(send *pr2* :rarm :shoulder-r :joint-angle -5) +(send *pr2* :head :neck-p :joint-angle -10) +(send *irtviewer* :draw-objects) +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) \ No newline at end of file diff --git a/jsk_2024_10_semi/scripts/conversation.py b/jsk_2024_10_semi/scripts/conversation.py new file mode 100644 index 0000000000..0aa7ad66be --- /dev/null +++ b/jsk_2024_10_semi/scripts/conversation.py @@ -0,0 +1,176 @@ +import os +import sys +import google.generativeai as genai +from google.generativeai.types import HarmCategory, HarmBlockThreshold +import speech_recognition as sr +from gtts import gTTS +import rospy +from std_msgs.msg import String +from sound_play.msg import SoundRequest +from speech_recognition_msgs.msg import SpeechRecognitionCandidates + +# ~/.bashrcに"export GEMINI_API_KEY='自分のapi_key'"を書いている +GEMINI_API_KEY=os.getenv('GEMINI_API_KEY') +MAX_TURNS = 5 + +def recognize_speech(): + # 音声認識のためのRecognizerオブジェクトを作成 + recognizer = sr.Recognizer() + # マイクから音声を取得 + with sr.Microphone() as source: + print('\033[36m'+"マイクの入力を待っています..."+'\033[0m') + recognizer.adjust_for_ambient_noise(source) # 周囲の雑音を調整 + audio = recognizer.listen(source) + try: + # 音声を日本語で認識 + text = recognizer.recognize_google(audio, language='ja-JP') + print('\033[36m'+f"認識結果: {text}"+'\033[0m') + return text + except sr.UnknownValueError: + print('\033[36m'+"音声を認識できませんでした。"+'\033[0m') + return None + except sr.RequestError as e: + print('\033[36m'+f"音声認識サービスに問題があります: {e}"+'\033[0m') + return None + +def text_to_speech_japanese(text): + # 日本語テキストを音声に変換 + tts = gTTS(text=text, lang='ja') + # 一時的な音声ファイルを保存 + filename = 'temp_audio.mp3' + tts.save(filename) + os.system(f"mpg123 {filename}") + +def set_mood_level(mood_level:int) -> int: + """setting AI's mood level based on the messages from the user and the previous mood level of the AI. default is 5. AI must call this function every time it responds to the user. + Args: + message: the message from the user. + Returns: + mood_level: the mood level of the AI from 1 to 9. 1 is the worst mood (angry) and 9 is the best mood (happy). + """ + print(f"AI's mood level: {mood_level}") + return mood_level + +class GenerativeModel: + def __init__(self, api_key, tools=[], model_name='gemini-1.5-pro'): + genai.configure(api_key=api_key) + if len(tools) > 0: + self.model = genai.GenerativeModel(model_name=model_name, tools=tools) + else: + self.model = genai.GenerativeModel(model_name=model_name) + self.safety_settings = { + HarmCategory.HARM_CATEGORY_SEXUALLY_EXPLICIT: HarmBlockThreshold.BLOCK_ONLY_HIGH, + HarmCategory.HARM_CATEGORY_HATE_SPEECH: HarmBlockThreshold.BLOCK_ONLY_HIGH, + HarmCategory.HARM_CATEGORY_HARASSMENT: HarmBlockThreshold.BLOCK_ONLY_HIGH, + HarmCategory.HARM_CATEGORY_DANGEROUS_CONTENT:HarmBlockThreshold.BLOCK_ONLY_HIGH + } + """ + 会話の安全性設定 + HarmCategory.HARM_CATEGORY_SEXUALLY_EXPLICIT: セクシャルな表現 + HarmCategory.HARM_CATEGORY_HATE_SPEECH: 憎悪表現 + HarmCategory.HARM_CATEGORY_HARASSMENT: 嫌がらせ + HarmCategory.HARM_CATEGORY_DANGEROUS_CONTENT: 危険な表現 + HarmBlockThreshold.BLOCK_NONE: ブロックしない + HarmBlockThreshold.BLOCK_ONLY_HIGH: 高いレベルの有害な表現をブロック + HarmBlockThreshold.BLOCK_MEDIUM_AND_ABOVE: 中以上のレベルの有害な表現をブロック + HarmBlockThreshold.BLOCK_LOW_AND_ABOVE: 低以上のレベルの有害な表現をブロック + HarmBlockThreshold.HARM_BLOCK_THRESHOLD_UNSPECIFIED: 未指定、デフォルトの設定 + """ + if len(tools) > 0: + self.chat = self.model.start_chat(history=[], enable_automatic_function_calling=True) + else: + self.chat = self.model.start_chat(history=[]) + def send_pre_prompt(self, pre_prompt): + self.chat.send_message(pre_prompt, safety_settings=self.safety_settings) + def send_message(self, message): + response = self.chat.send_message(message, safety_settings=self.safety_settings) + return response + def get_history(self): + return self.chat.history + +def conversation(): + input_mode = "text" + args = sys.argv + if len(args) > 2: + print("Usage: python conversation.py [input_mode]") + return + elif len(args) == 2: + if args[1] == "speech": + input_mode = "speech" + elif args[1] == "text": + input_mode = "text" + else: + print("Usage: input_mode should be either 'speech' or 'text'") + return + # モデルの初期化 + chat_ai = GenerativeModel(GEMINI_API_KEY, tools=[set_mood_level], model_name='gemini-pro') + pre_prompt = "# 設定 \ + * あなた(gemini)はuserと対話するAIです。 \ + * userはあなたに話しかけることができます。 \ + * userが話しかけると、あなたはuserに返答します。 \ + # 制約 \ + * あなたはuserのメッセージと、前回のmood_levelに基づいて、AIの機嫌を表すmood_levelを設定します。 \ + * mood_levelの初期値は5です。 \ + * mood_levelは1から9の整数で表されます。 \ + * 毎回の返答の際には、mood_levelを表示する関数（set_mood_level）の呼び出しを必ず行います。 \ + * 返答のテキストにはmood_levelについての情報を含めてはいけません。 \ + # 注意 \ + * あなたはuserのメッセージに対して、適切な返答を行う必要があります。" + chat_ai.send_pre_prompt(pre_prompt) + for turn in range(MAX_TURNS): + user_input = None + if input_mode == "text": + user_input = input("User >> ") + elif input_mode == "speech": + user_input = recognize_speech() + if user_input is None: + print("テキスト入力モードに切り替えます。") + input_mode = "text" + user_input = input("User >> ") + if user_input == "exit": + break + response = chat_ai.send_message(user_input) + print("AI >>", response.text) + if input_mode == "speech": + text_to_speech_japanese(response) + print(chat_ai.get_history()) + +class Responder(): + def __init__(self): + # self.sub = rospy.Subscriber('test_input_text', String, self.callback) + self.sub = rospy.Subscriber('/speech_to_text', SpeechRecognitionCandidates, self.callback) + self.pub = rospy.Publisher('/robotsound_jp', SoundRequest, queue_size=1) + self.chat_ai = GenerativeModel(GEMINI_API_KEY, tools=[set_mood_level], model_name='gemini-pro') + pre_prompt = "# 設定 \ + * あなた(gemini)はuserと対話するAIです。 \ + * userはあなたに話しかけることができます。 \ + * userが話しかけると、あなたはuserに返答します。 \ + # 制約 \ + * あなたはuserのメッセージと、前回のmood_levelに基づいて、AIの機嫌を表すmood_levelを設定します。 \ + * mood_levelの初期値は5です。 \ + * mood_levelは1から9の整数で表されます。 \ + * 毎回の返答の際には、mood_levelを表示する関数（set_mood_level）の呼び出しを必ず行います。 \ + * 返答のテキストにはmood_levelについての情報を含めてはいけません。 \ + # 注意 \ + * あなたはuserのメッセージに対して、適切な返答を行う必要があります。" + self.chat_ai.send_pre_prompt(pre_prompt) + + def callback(self, data): + rospy.loginfo(data.transcript) + response = self.chat_ai.send_message(data.transcript) + rospy.loginfo(response.text) + sound_req = SoundRequest() + sound_req.sound = -3 + sound_req.command = 1 + sound_req.volume = 100.0 + sound_req.arg = str(response.text) + self.pub.publish(sound_req) + + def publish(self, data): + self.pub.publish(data) + + +if __name__ == "__main__": + rospy.init_node('responder') + responder = Responder() + rospy.spin() diff --git a/jsk_2024_10_semi/scripts/demo-box.l b/jsk_2024_10_semi/scripts/demo-box.l new file mode 100644 index 0000000000..a5fa730352 --- /dev/null +++ b/jsk_2024_10_semi/scripts/demo-box.l @@ -0,0 +1,41 @@ +(require "package://pr2eus/pr2-interface.l") +(pr2-init) + +(defclass box-label-synchronizer + :super exact-time-message-filter) + +(defmethod box-label-synchronizer + (:callback (box label) + (print (list box label)) + (print (send-all (list box label) :header :stamp)) + )) + +;; ;; test +;; (setq *box-label* (instance box-label-synchronizer :init +;; (list (list "/docker/detic_segmentor/output/boxes" jsk_recognition_msgs::BoundingBoxArray) +;; (list "/docker/detic_segmentor/detected_classes" jsk_recognition_msgs::LabelArray)))) + +(ros::load-ros-manifest "jsk_recognition_msgs") +(setq *target-box* (make-cube 100 100 100)) +(objects (list *pr2* *target-box*)) +(defun box-cb (msg) + (ros::ros-info "received ~A boxes" (length (send msg :boxes))) + (dolist (box (send msg :boxes)) + (when (= (send box :label) 41) ;; 41 -> ball + (setq *target-coords* (send (ros::tf-pose->coords (send box :pose)) :copy-worldcoords)) + (setq *target-dimensions* (send box :dimensions)) + (format t "coords ~A, dimension ~A~%" (send *target-coords* :worldcoords) (* (send *target-dimensions* :x) (send *target-dimensions* :y) (send *target-dimensions* :z))) + (when (and (< (elt (send *target-coords* :worldpos) 2) 1000) + (> (elt (send *target-coords* :worldpos) 2) 600)) + (send *target-box* :move-to *target-coords* :world) + (print "update target position") + )))) + +(ros::subscribe "/synchronized_detic_label_boxes" jsk_recognition_msgs::BoundingBoxArray #'box-cb) +;;(ros::subscribe "/kinect_head/depth_registered/boxes" jsk_recognition_msgs::BoundingBoxArray #'box-cb) +(do-until-key + (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) + (send *irtviewer* :draw-objects) + (x::window-main-one) + (ros::spin-once) + ) diff --git a/jsk_2024_10_semi/scripts/gomoku.l b/jsk_2024_10_semi/scripts/gomoku.l new file mode 100644 index 0000000000..6c676b0143 --- /dev/null +++ b/jsk_2024_10_semi/scripts/gomoku.l @@ -0,0 +1,96 @@ +(require "package://pr2eus/pr2.l") +(require "package://pr2eus/pr2-utils.l") +(require "package://pr2eus/pr2-interface.l") +(if (not (boundp '*pr2*)) (setq *pr2* (pr2))) + +; (send *pr2* :reset-pose) ;; 初期姿勢 + +;; 一辺600mmの立方体を出現させる +(setq *cube* (make-cube 600 600 600)) +;; 立方体を(700, 0, 300)移動 +(send *cube* :translate (float-vector 700 0 300)) +(objects (list *pr2* *cube*)) + +;; PR2の左腕を指定の角度に動かす +(send *pr2* :larm :shoulder-p :joint-angle 15) +(send *pr2* :larm :shoulder-r :joint-angle 90) +(send *pr2* :larm :collar-y :joint-angle 45) +(send *pr2* :larm :elbow-p :joint-angle -100) +(send *pr2* :larm :elbow-r :joint-angle 130) +(send *pr2* :larm :wrist-p :joint-angle -10) +(send *pr2* :larm :wrist-r :joint-angle 0) +(send *irtviewer* :draw-objects) + +;; sleep 1秒 +(unix:sleep 1) + +;; inverse-kinematicsを使ってPR2の左腕を現在位置から相対的に(0, 0, 100)移動 +; (let ((target-coords (make-coords :pos (float-vector 0 0 100)))) +; (send *pr2* :larm :inverse-kinematics target-coords)) + +;;Set coordination. +(send *cube* :put :left-coords + (make-cascoords + :coords (send (send *cube* :copy-worldcoords) :translate (float-vector 700 0 300)) + :parent *cube*)) + +(send *pr2* :larm :inverse-kinematics + (send (send *cube* :get :left-coords) :copy-worldcoords) + :rotation-axis :z) + +; (send *ri* :stop-grasp :arms) + +(send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.0 0.0 -2.5)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t + :debug-view t) + +; (send *ri* :start-grasp :arms) + +(send *irtviewer* :draw-objects) + +;; sleep 1秒 +(unix:sleep 1) + +(send *pr2* :larm :elbow-r :joint-angle 120) + +(send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.0 0.0 2.5)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t + :debug-view t) + +(send *irtviewer* :draw-objects) + +;; sleep 1秒 +(unix:sleep 1) + +(send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector 5.0 -5.0 0.0)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t + :debug-view t) + +(send *pr2* :larm :wrist-r :joint-angle 0) + +(send *irtviewer* :draw-objects) + +;; sleep 1秒 +(unix:sleep 1) + +(send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.0 0.0 -2.5)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t + :debug-view t) + +(send *irtviewer* :draw-objects) \ No newline at end of file diff --git a/jsk_2024_10_semi/scripts/test.l b/jsk_2024_10_semi/scripts/test.l new file mode 100644 index 0000000000..42d105d1cf --- /dev/null +++ b/jsk_2024_10_semi/scripts/test.l @@ -0,0 +1,28 @@ +(require "package://pr2eus/pr2.l") ;;import pr2 package +(require "package://pr2eus/pr2-utils.l") +(require "package://pr2eus/pr2-interface.l") +(if (not (boundp '*pr2*)) (pr2-init)) ;; (setq *pr2* (pr2))) ;; (pr2) is alternated by *pr2* + +(send *pr2* :reset-pose) ;; 初期姿勢 +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(objects (list *pr2*)) + +(send *pr2* :larm :shoulder-p :joint-angle 50) +(send *pr2* :larm :shoulder-r :joint-angle -20) +(send *pr2* :larm :wrist-p :joint-angle 20) +(send *pr2* :rarm :shoulder-p :joint-angle 50) +(send *pr2* :rarm :shoulder-r :joint-angle 20) +(send *pr2* :rarm :wrist-p :joint-angle -20) +(send *pr2* :head :neck-p :joint-angle 50) +(send *irtviewer* :draw-objects) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) + +(send *pr2* :larm :shoulder-p :joint-angle 5) +(send *pr2* :rarm :shoulder-p :joint-angle 5) +(send *pr2* :head :neck-p :joint-angle -10) +(send *irtviewer* :draw-objects) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) + From dbe29a02d523226cb43539b5885d912795938c74 Mon Sep 17 00:00:00 2001 From: Rinno1478 Date: Wed, 27 Nov 2024 18:58:36 +0900 Subject: [PATCH 4/6] add detect_circle directory --- jsk_2024_10_semi/chabudai.l | 44 ---- .../detect_circle/.vscode/settings.json | 5 + jsk_2024_10_semi/detect_circle/CMakeLists.txt | 44 ++++ .../detect_circle/launch/detect_circle.launch | 74 +++++++ .../detect_circle/msg/PointStampedArray.msg | 2 + jsk_2024_10_semi/detect_circle/package.xml | 74 +++++++ .../detect_circle/src/detect_circle.cpp | 162 ++++++++++++++ jsk_2024_10_semi/scripts/catch.l | 111 ++++++++++ jsk_2024_10_semi/scripts/chabudai.l | 125 +++++++++++ jsk_2024_10_semi/scripts/chabudai_sim.l | 112 ++++++++++ jsk_2024_10_semi/scripts/conversation.py | 197 ++++++++++-------- jsk_2024_10_semi/scripts/gomoku.l | 2 +- 12 files changed, 815 insertions(+), 137 deletions(-) delete mode 100644 jsk_2024_10_semi/chabudai.l create mode 100644 jsk_2024_10_semi/detect_circle/.vscode/settings.json create mode 100644 jsk_2024_10_semi/detect_circle/CMakeLists.txt create mode 100644 jsk_2024_10_semi/detect_circle/launch/detect_circle.launch create mode 100644 jsk_2024_10_semi/detect_circle/msg/PointStampedArray.msg create mode 100644 jsk_2024_10_semi/detect_circle/package.xml create mode 100644 jsk_2024_10_semi/detect_circle/src/detect_circle.cpp create mode 100644 jsk_2024_10_semi/scripts/catch.l create mode 100644 jsk_2024_10_semi/scripts/chabudai.l create mode 100644 jsk_2024_10_semi/scripts/chabudai_sim.l diff --git a/jsk_2024_10_semi/chabudai.l b/jsk_2024_10_semi/chabudai.l deleted file mode 100644 index 74b896ae5a..0000000000 --- a/jsk_2024_10_semi/chabudai.l +++ /dev/null @@ -1,44 +0,0 @@ -(require "package://pr2eus/pr2.l") ;;import pr2 package -(require "package://pr2eus/pr2-utils.l") -(require "package://pr2eus/pr2-interface.l") -(if (not (boundp '*pr2*)) (setq *pr2* (pr2)));; (setq *pr2* (pr2))) ;; (pr2) is alternated by *pr2* - -; (send *pr2* :reset-pose) ;; 初期姿勢 -; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) -; (send *ri* :wait-interpolation) -; (objects (list *pr2*)) - -;; 一辺600mmの立方体を出現させる -(setq *cube* (make-cube 600 600 600)) -;; 立方体を(700, 0, 300)移動 -(send *cube* :translate (float-vector 700 0 300)) -(objects (list *pr2*)) - -;; PR2の腕を指定の角度に動かす -(send *pr2* :larm :shoulder-p :joint-angle 15) -(send *pr2* :larm :shoulder-r :joint-angle 90) -(send *pr2* :larm :collar-y :joint-angle 45) -(send *pr2* :larm :elbow-p :joint-angle -100) -(send *pr2* :larm :elbow-r :joint-angle 0) -(send *pr2* :larm :wrist-p :joint-angle -10) -(send *pr2* :larm :wrist-r :joint-angle 0) -(send *pr2* :rarm :shoulder-p :joint-angle 15) -(send *pr2* :rarm :shoulder-r :joint-angle -90) -(send *pr2* :rarm :collar-y :joint-angle -45) -(send *pr2* :rarm :elbow-p :joint-angle -100) -(send *pr2* :rarm :elbow-r :joint-angle 0) -(send *pr2* :rarm :wrist-p :joint-angle -10) -(send *pr2* :rarm :wrist-r :joint-angle 0) -(send *pr2* :head :neck-p :joint-angle 50) -(send *irtviewer* :draw-objects) - -;; sleep 2秒 -(unix:sleep 2) -(send *pr2* :larm :shoulder-p :joint-angle 0) -(send *pr2* :larm :shoulder-r :joint-angle 5) -(send *pr2* :rarm :shoulder-p :joint-angle 0) -(send *pr2* :rarm :shoulder-r :joint-angle -5) -(send *pr2* :head :neck-p :joint-angle -10) -(send *irtviewer* :draw-objects) -; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) -; (send *ri* :wait-interpolation) \ No newline at end of file diff --git a/jsk_2024_10_semi/detect_circle/.vscode/settings.json b/jsk_2024_10_semi/detect_circle/.vscode/settings.json new file mode 100644 index 0000000000..a3b2b51f1d --- /dev/null +++ b/jsk_2024_10_semi/detect_circle/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "files.associations": { + "vector": "cpp" + } +} \ No newline at end of file diff --git a/jsk_2024_10_semi/detect_circle/CMakeLists.txt b/jsk_2024_10_semi/detect_circle/CMakeLists.txt new file mode 100644 index 0000000000..e88da5f708 --- /dev/null +++ b/jsk_2024_10_semi/detect_circle/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.0.2) +project(detect_circle) + +find_package(catkin REQUIRED COMPONENTS + cv_bridge + roscpp + sensor_msgs + std_msgs + geometry_msgs + # message_generation +) + +# add_message_files( +# FILES +# PointStampedArray.msg +# ) + +# generate_messages( +# DEPENDENCIES +# std_msgs +# geometry_msgs +# ) + +# catkin_package( +# CATKIN_DEPENDS cv_bridge roscpp sensor_msgs std_msgs geometry_msgs message_runtime +# ) + +catkin_package( + CATKIN_DEPENDS cv_bridge roscpp sensor_msgs std_msgs geometry_msgs +) + +include_directories( + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) + +add_executable(${PROJECT_NAME}_node src/detect_circle.cpp) + +add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(${PROJECT_NAME}_node + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} +) \ No newline at end of file diff --git a/jsk_2024_10_semi/detect_circle/launch/detect_circle.launch b/jsk_2024_10_semi/detect_circle/launch/detect_circle.launch new file mode 100644 index 0000000000..4129a0c840 --- /dev/null +++ b/jsk_2024_10_semi/detect_circle/launch/detect_circle.launch @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + queue_size: 100 + + + + + + + + + + + + + + + + + offset_x: 100 + offset_y: 40 + width: 450 + height: 300 + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/jsk_2024_10_semi/detect_circle/msg/PointStampedArray.msg b/jsk_2024_10_semi/detect_circle/msg/PointStampedArray.msg new file mode 100644 index 0000000000..101ed8bc31 --- /dev/null +++ b/jsk_2024_10_semi/detect_circle/msg/PointStampedArray.msg @@ -0,0 +1,2 @@ +Header header +geometry_msgs/Point[] points \ No newline at end of file diff --git a/jsk_2024_10_semi/detect_circle/package.xml b/jsk_2024_10_semi/detect_circle/package.xml new file mode 100644 index 0000000000..eb141b51fc --- /dev/null +++ b/jsk_2024_10_semi/detect_circle/package.xml @@ -0,0 +1,74 @@ + + + detect_circle + 0.0.0 + The detect_circle package + + + + + mech-user + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + message_generation + + + + + + message_runtime + + + + + catkin + cv_bridge + roscpp + sensor_msgs + std_msgs + geometry_msgs + cv_bridge + roscpp + sensor_msgs + std_msgs + geometry_msgs + cv_bridge + roscpp + sensor_msgs + std_msgs + geometry_msgs + + + + + + + + diff --git a/jsk_2024_10_semi/detect_circle/src/detect_circle.cpp b/jsk_2024_10_semi/detect_circle/src/detect_circle.cpp new file mode 100644 index 0000000000..446412f3ca --- /dev/null +++ b/jsk_2024_10_semi/detect_circle/src/detect_circle.cpp @@ -0,0 +1,162 @@ +// #include "opencv2/imgcodecs.hpp" +// #include "opencv2/highgui.hpp" +// #include "opencv2/imgproc.hpp" +#include +#include +// #include +// #include +// #include +#include +// #include +#include + + + +// // initial and max values of the parameters of interests. +// const int canny_threshold_initial_value = 100; +// const int accumulator_threshold_initial_value = 50; +// // const int max_accumulator_threshold = 200; +// // const int max_canny_threshold = 255; + +// int canny_threshold = canny_threshold_initial_value; +// int accumulator_threshold = accumulator_threshold_initial_value; + +// class DetectCircle +// { +// private: +// ros::NodeHandle nh; +// // ros::NodeHandle pnh("~"); +// ros::Subscriber sub; +// ros::Publisher pub_img; +// ros::Publisher pub_points; + +// public: +// DetectCircle() +// { +// sub = nh.subscribe("/usb_cam/image_raw", 1, &DetectCircle::callback, this); +// pub_img = nh.advertise("/camera/image/circle", 1); +// pub_points = nh.advertise("/camera/points/circle", 1); +// pnh.getParam("canny_threshold", canny_threshold); +// pnh.getParam("accumulator_threshold", accumulator_threshold); +// } + +// ~DetectCircle(){}; + +// void callback(const sensor_msgs::ImageConstPtr& msg) +// { +// cv_bridge::CvImagePtr cv_ptr; +// try +// { +// cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); +// } +// catch(cv_bridge::Exception& e) +// { +// ROS_ERROR("cv_bridge exception: %s", e.what()); +// return; +// } + +// cv::Mat src, src_gray; + +// src = cv_ptr->image; + +// if(src.empty()) +// { +// std::cerr << "Invalid input image\n"; +// return; +// } + +// // Convert it to gray +// cv::cvtColor(src, src_gray, cv::COLOR_BGR2GRAY); + +// // Reduce the noise so we avoid false circle detection +// cv::GaussianBlur(src_gray, src_gray, cv::Size(9, 9), 2, 2); + +// detect_circle::PointStampedArray points; +// points.header = msg->header; + +// // will hold the results of the detection +// std::vector circles; +// // runs the actual detection +// cv::HoughCircles(src_gray, circles, cv::HOUGH_GRADIENT, 1, src_gray.rows/8, canny_threshold, accumulator_threshold, 0, 0); + +// // clone the colour, input image for displaying purposes +// cv::Mat img = src.clone(); +// points.points.resize(circles.size()); +// for(size_t i = 0; i < circles.size(); i++) +// { +// cv::Point center(cvRound(circles[i][0]), cvRound(circles[i][1])); +// int radius = cvRound(circles[i][2]); +// points.points[i].x = center.x; +// points.points[i].y = center.y; +// points.points[i].z = 0; +// // circle center +// cv::circle(img, center, 3, cv::Scalar(0,255,0), -1, 8, 0); +// // circle outline +// cv::circle(img, center, radius, cv::Scalar(0,0,255), 3, 8, 0); +// } + +// sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg(); + +// ROS_INFO("Number of circles detected: %ld", circles.size()); + +// for (size_t i = 0; i < circles.size(); i++) +// { +// ROS_INFO("Circle #%ld: center at (%d, %d)", i, (int)(points.points[i].x), (int)(points.points[i].y)); +// } + +// pub_img.publish(img_msg); +// pub_points.publish(points); +// } + +// }; + +class ConvertHoughCircle2PointStamped +{ + private: + ros::NodeHandle nh; + ros::Subscriber sub; + ros::Publisher pub; + + public: + ConvertHoughCircle2PointStamped() + { + sub = nh.subscribe("/hough_circles/circles", 1, &ConvertHoughCircle2PointStamped::callback, this); + pub = nh.advertise("/camera/circle/point", 1); + } + + ~ConvertHoughCircle2PointStamped(){}; + + void callback(const opencv_apps::CircleArrayStamped& msg) + { + geometry_msgs::PointStamped point; + if (msg.circles.size() == 0) + { + ROS_INFO("No circle detected"); + return; + } + point.header = msg.header; + point.point.x = msg.circles[0].center.x; + point.point.y = msg.circles[0].center.y; + point.point.z = 0; + + ROS_INFO("Circle center at (%f, %f)", point.point.x, point.point.y); + + pub.publish(point); + } + +}; + +int main(int argc, char** argv) +{ + // ros::init(argc, argv, "detect_circle"); + // DetectCircle dc; + // ROS_INFO("detect_circle node has started"); + // ros::spin(); + + ros::init(argc, argv, "detect_circle"); + ConvertHoughCircle2PointStamped chc2ps; + ROS_INFO("convert_hough_circle2point_stamped node has started"); + ros::spin(); + + return 0; +} diff --git a/jsk_2024_10_semi/scripts/catch.l b/jsk_2024_10_semi/scripts/catch.l new file mode 100644 index 0000000000..3852344626 --- /dev/null +++ b/jsk_2024_10_semi/scripts/catch.l @@ -0,0 +1,111 @@ +(require "package://pr2eus/pr2-interface.l") +(require "package://pr2eus/pr2.l") +(load "package://pr2eus/pr2-interface.l") +(ros::roseus "pr2_send_joints") +(ros::load-ros-manifest "jsk_recognition_msgs") + +;;pr2生成 +(if (not (boundp '*pr2*)) (pr2-init)) +(setq *ri* (instance pr2-interface :init)) +(objects (list *pr2*)) + +;;コールバック関数 +(setq *tfl* (instance ros::transform-listener :init)) +(defun callback (msg) + (ros::ros-info "detect circle ~A" msg) + (print (send msg :point)) + (let ((circle-pose (instance geometry_msgs::posestamped :init))) + (send circle-pose :pose :position (send msg :point)) + (send circle-pose :pose :orientation :x 0.0) + (send circle-pose :pose :orientation :y 0.0) + (send circle-pose :pose :orientation :z 0.0) + (send circle-pose :pose :orientation :w 1.0) + (setq *target-coords* (send (ros::tf-pose->coords (send circle-pose :pose)) :copy-worldcoords)) + ) + (setq tfc (send *tfl* :lookup-transform "base_link" "head_mount_kinect_rgb_optical_frame" (ros::time 0))) + (when tfc + (setq *target-coords* (send tfc :transform *target-coords*)) + (send *target-box* :move-to *target-coords* :world) + (print "update target position"))) + +(defun init-pose () + (send *pr2* :reset-pose) + (send *ri* :stop-grasp :arms) + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) + (send *ri* :wait-interpolation) +) + +(defun pick-up () + (send *pr2* :head :neck-p :joint-angle 60) + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + + + ;;boxを適当な初期位置で配置 + (setq *target-box* (make-cube 100 100 100)) + (objects (list *pr2* *target-box*)) + + + ;;サブスクライブ + (ros::subscribe "/pointcloud_screenpoint/output_point" geometry_msgs::PointStamped #'callback) + + ;;ボールが見つかるまでループ + (ros::ros-info "wait for target ~A" (send *target-box* :worldpos)) + (ros::rate 3) ;; 3Hz + (while (< (elt (send *target-box* :worldpos) 2) 100) + (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) + (send *irtviewer* :draw-objects) + (x::window-main-one) + (ros::spin-once) + (ros::ros-info "waiting... ~A" (send *target-box* :worldpos)) + (ros::sleep) + ) + (ros::ros-info "start grasping... ~A" (send *target-box* :worldpos)) + + + ;;自分の台座とぶつからないようにアームを動かす + ;; :larm, :rarm, :arms + (setq arm :larm) + (send *pr2* arm :inverse-kinematics + (send (send *target-box* :copy-worldcoords) :translate #f(-100 0 0))) + + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *pr2* arm :inverse-kinematics + (send (send *target-box* :copy-worldcoords) :translate #f(-50 0 -20))) + + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + ;;掴む + (send *ri* :start-grasp arm :wait t) + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + + + ;;足にぶつからないような軌道で中央へ + (send *pr2* arm :inverse-kinematics + (send (send *target-box* :copy-worldcoords) :translate #f(0 0 100))) + + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) + (send *ri* :wait-interpolation) +) + + + +;;メイン処理の実行 +(ros::ros-info "init-pose") +(init-pose) +(unix:sleep 3) +(ros::ros-info "pick up") +(pick-up) +(unix:sleep 3) +(init-pose) + diff --git a/jsk_2024_10_semi/scripts/chabudai.l b/jsk_2024_10_semi/scripts/chabudai.l new file mode 100644 index 0000000000..d3eec9a361 --- /dev/null +++ b/jsk_2024_10_semi/scripts/chabudai.l @@ -0,0 +1,125 @@ +(require "package://pr2eus/pr2.l") ;;import pr2 package +(require "package://pr2eus/pr2-utils.l") +(require "package://pr2eus/pr2-interface.l") +(ros::load-ros-manifest "roseus") +(if (not (boundp '*pr2*)) (pr2-init)) + +;; 一辺600mmの立方体を出現させる +(setq *cube* (make-cube 600 600 600)) +;; 立方体を(700, 0, 300)移動 +(send *cube* :translate (float-vector 700 0 300)) +;; chabudaiフラグ +(setq *chabudai* nil) + +;;Set coordination. +(send *cube* :put :left-coords + (make-cascoords + :coords (send (send *cube* :copy-worldcoords) :translate (float-vector 700 0 300)) + :parent *cube*)) + +(send *pr2* :larm :inverse-kinematics + (send (send *cube* :get :left-coords) :copy-worldcoords) + :rotation-axis :z) + +(send *pr2* :reset-pose) ;; 初期姿勢 +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(objects (list *pr2* *cube*)) + + +(defun callback (msg) + (format t "robot_mood: ~A~%" (send msg :data)) + (when (< (send msg :data) 2) + (setq *chabudai* t) + ) +) + +(defun chabudai_gaeshi () + (send *pr2* :larm :shoulder-p :joint-angle 70) + (send *pr2* :larm :shoulder-r :joint-angle 60) + (send *pr2* :larm :collar-y :joint-angle 32) + (send *pr2* :larm :elbow-p :joint-angle -110) + (send *pr2* :larm :elbow-r :joint-angle 180) + (send *pr2* :larm :wrist-p :joint-angle -30) + (send *pr2* :larm :wrist-r :joint-angle -30) + (send *pr2* :rarm :shoulder-p :joint-angle 70) + (send *pr2* :rarm :shoulder-r :joint-angle -60) + (send *pr2* :rarm :collar-y :joint-angle -32) + (send *pr2* :rarm :elbow-p :joint-angle -110) + (send *pr2* :rarm :elbow-r :joint-angle 180) + (send *pr2* :rarm :wrist-p :joint-angle -30) + (send *pr2* :rarm :wrist-r :joint-angle 30) + (send *pr2* :head :neck-p :joint-angle 50) + (send *ri* :stop-grasp :arms) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + ;; sleep 1秒 + (unix:sleep 1) + + + (send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector 1.0 0.5 0.0)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t) + (send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector 1.0 -0.5 0.0)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :rarm :end-coords) :parent)) + :rotation-axis t) + + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + ;; sleep 1秒 + (unix:sleep 1) + + (send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.0 0.0 2.0)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t) + (send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.0 0.0 2.0)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :rarm :end-coords) :parent)) + :rotation-axis t) + + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + ;; sleep 1秒 + (unix:sleep 1) + + (send *pr2* :larm :shoulder-p :joint-angle -20) + (send *pr2* :larm :shoulder-r :joint-angle 20) + (send *pr2* :larm :elbow-p :joint-angle -100) + (send *pr2* :rarm :shoulder-p :joint-angle -20) + (send *pr2* :rarm :shoulder-r :joint-angle -20) + (send *pr2* :rarm :elbow-p :joint-angle -100) + (send *pr2* :head :neck-p :joint-angle -10) + (send *ri* :angle-vector (send *pr2* :angle-vector) 500) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) +) + +(ros::subscribe "/robot_mood" std_msgs::Int32 #'callback) + +(do-until-key + (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) + (send *irtviewer* :draw-objects) + (x::window-main-one) + (ros::spin-once) + (when *chabudai* + (chabudai_gaeshi) + (setq *chabudai* nil) + ) +) \ No newline at end of file diff --git a/jsk_2024_10_semi/scripts/chabudai_sim.l b/jsk_2024_10_semi/scripts/chabudai_sim.l new file mode 100644 index 0000000000..68977ea550 --- /dev/null +++ b/jsk_2024_10_semi/scripts/chabudai_sim.l @@ -0,0 +1,112 @@ +(require "package://pr2eus/pr2.l") ;;import pr2 package +; (require "package://pr2eus/pr2-utils.l") +; (require "package://pr2eus/pr2-interface.l") +(if (not (boundp '*pr2*)) (setq *pr2* (pr2))) + +;; 一辺600mmの立方体を出現させる +(setq *cube* (make-cube 100 100 100)) +;; 立方体を(700, 0, 300)移動 +(send *cube* :translate (float-vector 700 0 300)) + +;;Set coordination. +(send *cube* :put :left-coords + (make-cascoords + :coords (send (send *cube* :copy-worldcoords) :translate (float-vector 700 0 300)) + :parent *cube*)) + +(send *pr2* :larm :inverse-kinematics + (send (send *cube* :get :left-coords) :copy-worldcoords) + :rotation-axis :z) + +; (send *pr2* :reset-pose) ;; 初期姿勢 +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) +(objects (list *pr2* *cube*)) + + +(send *pr2* :larm :shoulder-p :joint-angle 70) +(send *pr2* :larm :shoulder-r :joint-angle 60) +(send *pr2* :larm :collar-y :joint-angle 32) +(send *pr2* :larm :elbow-p :joint-angle -110) +(send *pr2* :larm :elbow-r :joint-angle 180) +(send *pr2* :larm :wrist-p :joint-angle -30) +(send *pr2* :larm :wrist-r :joint-angle -40) +(send *pr2* :rarm :shoulder-p :joint-angle 70) +(send *pr2* :rarm :shoulder-r :joint-angle -60) +(send *pr2* :rarm :collar-y :joint-angle -32) +(send *pr2* :rarm :elbow-p :joint-angle -110) +(send *pr2* :rarm :elbow-r :joint-angle 180) +(send *pr2* :rarm :wrist-p :joint-angle -30) +(send *pr2* :rarm :wrist-r :joint-angle 40) +(send *pr2* :head :neck-p :joint-angle 50) +; (send *ri* :stop-grasp :arms) +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +;; sleep 1秒 +(unix:sleep 1) + + +(send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.5 0.0 0.0)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t + :debug-view t) +(send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.5 0.0 0.0)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :rarm :end-coords) :parent)) + :rotation-axis t + :debug-view t) + +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +;; sleep 1秒 +(unix:sleep 1) + +; (send *ri* :start-grasp :arms) +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +;; sleep 2秒 +(unix:sleep 2) + +(send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector -0.5 0.0 0.5)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t + :debug-view t) +(send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector -0.5 0.0 0.5)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :rarm :end-coords) :parent)) + :rotation-axis t + :debug-view t) + +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +;; sleep 1秒 +(unix:sleep 1) + + +(send *pr2* :larm :shoulder-p :joint-angle 0) +(send *pr2* :larm :shoulder-r :joint-angle 5) +(send *pr2* :rarm :shoulder-p :joint-angle 0) +(send *pr2* :rarm :shoulder-r :joint-angle -5) +(send *pr2* :head :neck-p :joint-angle -10) +; (send *ri* :stop-grasp :arms) +; (send *ri* :angle-vector (send *pr2* :angle-vector) 250) +; (send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) diff --git a/jsk_2024_10_semi/scripts/conversation.py b/jsk_2024_10_semi/scripts/conversation.py index 0aa7ad66be..932a507933 100644 --- a/jsk_2024_10_semi/scripts/conversation.py +++ b/jsk_2024_10_semi/scripts/conversation.py @@ -2,54 +2,46 @@ import sys import google.generativeai as genai from google.generativeai.types import HarmCategory, HarmBlockThreshold -import speech_recognition as sr -from gtts import gTTS +# import speech_recognition as sr +# from gtts import gTTS import rospy -from std_msgs.msg import String +from std_msgs.msg import String, Int32 from sound_play.msg import SoundRequest from speech_recognition_msgs.msg import SpeechRecognitionCandidates +from difflib import SequenceMatcher # ~/.bashrcに"export GEMINI_API_KEY='自分のapi_key'"を書いている GEMINI_API_KEY=os.getenv('GEMINI_API_KEY') MAX_TURNS = 5 -def recognize_speech(): - # 音声認識のためのRecognizerオブジェクトを作成 - recognizer = sr.Recognizer() - # マイクから音声を取得 - with sr.Microphone() as source: - print('\033[36m'+"マイクの入力を待っています..."+'\033[0m') - recognizer.adjust_for_ambient_noise(source) # 周囲の雑音を調整 - audio = recognizer.listen(source) - try: - # 音声を日本語で認識 - text = recognizer.recognize_google(audio, language='ja-JP') - print('\033[36m'+f"認識結果: {text}"+'\033[0m') - return text - except sr.UnknownValueError: - print('\033[36m'+"音声を認識できませんでした。"+'\033[0m') - return None - except sr.RequestError as e: - print('\033[36m'+f"音声認識サービスに問題があります: {e}"+'\033[0m') - return None +# def recognize_speech(): +# # 音声認識のためのRecognizerオブジェクトを作成 +# recognizer = sr.Recognizer() +# # マイクから音声を取得 +# with sr.Microphone() as source: +# print('\033[36m'+"マイクの入力を待っています..."+'\033[0m') +# recognizer.adjust_for_ambient_noise(source) # 周囲の雑音を調整 +# audio = recognizer.listen(source) +# try: +# # 音声を日本語で認識 +# text = recognizer.recognize_google(audio, language='ja-JP') +# print('\033[36m'+f"認識結果: {text}"+'\033[0m') +# return text +# except sr.UnknownValueError: +# print('\033[36m'+"音声を認識できませんでした。"+'\033[0m') +# return None +# except sr.RequestError as e: +# print('\033[36m'+f"音声認識サービスに問題があります: {e}"+'\033[0m') +# return None -def text_to_speech_japanese(text): - # 日本語テキストを音声に変換 - tts = gTTS(text=text, lang='ja') - # 一時的な音声ファイルを保存 - filename = 'temp_audio.mp3' - tts.save(filename) - os.system(f"mpg123 {filename}") +# def text_to_speech_japanese(text): +# # 日本語テキストを音声に変換 +# tts = gTTS(text=text, lang='ja') +# # 一時的な音声ファイルを保存 +# filename = 'temp_audio.mp3' +# tts.save(filename) +# os.system(f"mpg123 {filename}") -def set_mood_level(mood_level:int) -> int: - """setting AI's mood level based on the messages from the user and the previous mood level of the AI. default is 5. AI must call this function every time it responds to the user. - Args: - message: the message from the user. - Returns: - mood_level: the mood level of the AI from 1 to 9. 1 is the worst mood (angry) and 9 is the best mood (happy). - """ - print(f"AI's mood level: {mood_level}") - return mood_level class GenerativeModel: def __init__(self, api_key, tools=[], model_name='gemini-1.5-pro'): @@ -88,86 +80,107 @@ def send_message(self, message): def get_history(self): return self.chat.history -def conversation(): - input_mode = "text" - args = sys.argv - if len(args) > 2: - print("Usage: python conversation.py [input_mode]") - return - elif len(args) == 2: - if args[1] == "speech": - input_mode = "speech" - elif args[1] == "text": - input_mode = "text" - else: - print("Usage: input_mode should be either 'speech' or 'text'") - return - # モデルの初期化 - chat_ai = GenerativeModel(GEMINI_API_KEY, tools=[set_mood_level], model_name='gemini-pro') - pre_prompt = "# 設定 \ - * あなた(gemini)はuserと対話するAIです。 \ - * userはあなたに話しかけることができます。 \ - * userが話しかけると、あなたはuserに返答します。 \ - # 制約 \ - * あなたはuserのメッセージと、前回のmood_levelに基づいて、AIの機嫌を表すmood_levelを設定します。 \ - * mood_levelの初期値は5です。 \ - * mood_levelは1から9の整数で表されます。 \ - * 毎回の返答の際には、mood_levelを表示する関数（set_mood_level）の呼び出しを必ず行います。 \ - * 返答のテキストにはmood_levelについての情報を含めてはいけません。 \ - # 注意 \ - * あなたはuserのメッセージに対して、適切な返答を行う必要があります。" - chat_ai.send_pre_prompt(pre_prompt) - for turn in range(MAX_TURNS): - user_input = None - if input_mode == "text": - user_input = input("User >> ") - elif input_mode == "speech": - user_input = recognize_speech() - if user_input is None: - print("テキスト入力モードに切り替えます。") - input_mode = "text" - user_input = input("User >> ") - if user_input == "exit": - break - response = chat_ai.send_message(user_input) - print("AI >>", response.text) - if input_mode == "speech": - text_to_speech_japanese(response) - print(chat_ai.get_history()) +# def conversation(): +# input_mode = "text" +# args = sys.argv +# if len(args) > 2: +# print("Usage: python conversation.py [input_mode]") +# return +# elif len(args) == 2: +# if args[1] == "speech": +# input_mode = "speech" +# elif args[1] == "text": +# input_mode = "text" +# else: +# print("Usage: input_mode should be either 'speech' or 'text'") +# return +# # モデルの初期化 +# chat_ai = GenerativeModel(GEMINI_API_KEY, tools=[set_mood_level], model_name='gemini-pro') +# pre_prompt = "# 設定 \ +# * あなた(gemini)はuserと対話するAIです。 \ +# * userはあなたに話しかけることができます。 \ +# * userが話しかけると、あなたはuserに返答します。 \ +# # 制約\ +# * あなたはuserのメッセージと、前回のmood_levelに基づいて、AIの機嫌を表すmood_levelを設定します。 \ +# * mood_levelの初期値は5です。 \ +# * mood_levelは1から9の整数で表されます。 \ +# * 毎回の返答の際には、mood_levelを表示する関数（set_mood_level）の呼び出しを必ず行います。 \ +# * 返答のテキストにはmood_levelについての情報を含めてはいけません。 \ +# # 注意 \ +# * あなたはuserのメッセージに対して、適切な返答を行う必要があります。" +# chat_ai.send_pre_prompt(pre_prompt) +# for turn in range(MAX_TURNS): +# user_input = None +# if input_mode == "text": +# user_input = input("User >> ") +# elif input_mode == "speech": +# user_input = recognize_speech() +# if user_input is None: +# print("テキスト入力モードに切り替えます。") +# input_mode = "text" +# user_input = input("User >> ") +# if user_input == "exit": +# break +# response = chat_ai.send_message(user_input) +# print("AI >>", response.text) +# if input_mode == "speech": +# text_to_speech_japanese(response) +# print(chat_ai.get_history()) class Responder(): def __init__(self): # self.sub = rospy.Subscriber('test_input_text', String, self.callback) - self.sub = rospy.Subscriber('/speech_to_text', SpeechRecognitionCandidates, self.callback) + self.sub = rospy.Subscriber('/speech_to_text', SpeechRecognitionCandidates, self.callback, queue_size=1) self.pub = rospy.Publisher('/robotsound_jp', SoundRequest, queue_size=1) - self.chat_ai = GenerativeModel(GEMINI_API_KEY, tools=[set_mood_level], model_name='gemini-pro') + self.pub2 = rospy.Publisher('/robot_mood', Int32, queue_size=1) + self.chat_ai = GenerativeModel(GEMINI_API_KEY, tools=[self.set_mood_level], model_name='gemini-pro') pre_prompt = "# 設定 \ * あなた(gemini)はuserと対話するAIです。 \ * userはあなたに話しかけることができます。 \ - * userが話しかけると、あなたはuserに返答します。 \ + * userが話しかけた後、あなたはuserに返答します。 \ # 制約 \ - * あなたはuserのメッセージと、前回のmood_levelに基づいて、AIの機嫌を表すmood_levelを設定します。 \ + * あなたはuserのメッセージに基づいて、AIの機嫌を表すmood_levelを設定します。 \ * mood_levelの初期値は5です。 \ * mood_levelは1から9の整数で表されます。 \ * 毎回の返答の際には、mood_levelを表示する関数（set_mood_level）の呼び出しを必ず行います。 \ - * 返答のテキストにはmood_levelについての情報を含めてはいけません。 \ - # 注意 \ - * あなたはuserのメッセージに対して、適切な返答を行う必要があります。" + * 返答のテキストにはmood_level（気分レベル）についての情報を含めてはいけません。" self.chat_ai.send_pre_prompt(pre_prompt) + self.last_speak_time = rospy.Time.now() + self.robot_mood_level = 5 def callback(self, data): + global robot_mood_level rospy.loginfo(data.transcript) + if rospy.Time.now() - self.last_speak_time < rospy.Duration(10): + rospy.loginfo("skip") + return response = self.chat_ai.send_message(data.transcript) rospy.loginfo(response.text) sound_req = SoundRequest() sound_req.sound = -3 sound_req.command = 1 - sound_req.volume = 100.0 + sound_req.volume = 70.0 sound_req.arg = str(response.text) self.pub.publish(sound_req) + self.last_speak_time = rospy.Time.now() + robot_mood_level = Int32() + robot_mood_level.data = self.robot_mood_level + self.pub2.publish(robot_mood_level) def publish(self, data): + global robot_mood_level self.pub.publish(data) + self.pub2.publish(robot_mood_level) + + def set_mood_level(self, mood_level:int) -> int: + """setting AI's mood level based on the messages from the user and the previous mood level of the AI. default is 5. AI must call this function every time it responds to the user. + Args: + message: the message from the user. + Returns: + mood_level: the mood level of the AI from 1 to 9. 1 is the worst mood (angry) and 9 is the best mood (happy). + """ + print(f"AI's mood level: {mood_level}") + self.robot_mood_level = int(mood_level) if __name__ == "__main__": diff --git a/jsk_2024_10_semi/scripts/gomoku.l b/jsk_2024_10_semi/scripts/gomoku.l index 6c676b0143..c388b1fd3d 100644 --- a/jsk_2024_10_semi/scripts/gomoku.l +++ b/jsk_2024_10_semi/scripts/gomoku.l @@ -93,4 +93,4 @@ :rotation-axis t :debug-view t) -(send *irtviewer* :draw-objects) \ No newline at end of file +(send *irtviewer* :draw-objects) From b7a3abde102d8e6efeac64b7681d541b63878198 Mon Sep 17 00:00:00 2001 From: Rinno1478 Date: Mon, 16 Dec 2024 19:09:05 +0900 Subject: [PATCH 5/6] edit jsk_2024_10_semi/scripts/catch.l --- .../.vscode/settings.json | 0 .../CMakeLists.txt | 0 .../launch/detect_circle.launch | 22 ++-- .../msg/PointStampedArray.msg | 0 .../package.xml | 0 .../src/detect_circle.cpp | 0 jsk_2024_10_semi/scripts/catch.l | 116 +++++++++++++----- 7 files changed, 97 insertions(+), 41 deletions(-) rename {jsk_2024_10_semi/detect_circle => detect_circle}/.vscode/settings.json (100%) mode change 100644 => 100755 rename {jsk_2024_10_semi/detect_circle => detect_circle}/CMakeLists.txt (100%) mode change 100644 => 100755 rename {jsk_2024_10_semi/detect_circle => detect_circle}/launch/detect_circle.launch (81%) mode change 100644 => 100755 rename {jsk_2024_10_semi/detect_circle => detect_circle}/msg/PointStampedArray.msg (100%) mode change 100644 => 100755 rename {jsk_2024_10_semi/detect_circle => detect_circle}/package.xml (100%) mode change 100644 => 100755 rename {jsk_2024_10_semi/detect_circle => detect_circle}/src/detect_circle.cpp (100%) mode change 100644 => 100755 diff --git a/jsk_2024_10_semi/detect_circle/.vscode/settings.json b/detect_circle/.vscode/settings.json old mode 100644 new mode 100755 similarity index 100% rename from jsk_2024_10_semi/detect_circle/.vscode/settings.json rename to detect_circle/.vscode/settings.json diff --git a/jsk_2024_10_semi/detect_circle/CMakeLists.txt b/detect_circle/CMakeLists.txt old mode 100644 new mode 100755 similarity index 100% rename from jsk_2024_10_semi/detect_circle/CMakeLists.txt rename to detect_circle/CMakeLists.txt diff --git a/jsk_2024_10_semi/detect_circle/launch/detect_circle.launch b/detect_circle/launch/detect_circle.launch old mode 100644 new mode 100755 similarity index 81% rename from jsk_2024_10_semi/detect_circle/launch/detect_circle.launch rename to detect_circle/launch/detect_circle.launch index 4129a0c840..44f9cb4d8f --- a/jsk_2024_10_semi/detect_circle/launch/detect_circle.launch +++ b/detect_circle/launch/detect_circle.launch @@ -35,7 +35,7 @@ - + - + + - - - - + + + + @@ -71,4 +71,4 @@ - \ No newline at end of file + diff --git a/jsk_2024_10_semi/detect_circle/msg/PointStampedArray.msg b/detect_circle/msg/PointStampedArray.msg old mode 100644 new mode 100755 similarity index 100% rename from jsk_2024_10_semi/detect_circle/msg/PointStampedArray.msg rename to detect_circle/msg/PointStampedArray.msg diff --git a/jsk_2024_10_semi/detect_circle/package.xml b/detect_circle/package.xml old mode 100644 new mode 100755 similarity index 100% rename from jsk_2024_10_semi/detect_circle/package.xml rename to detect_circle/package.xml diff --git a/jsk_2024_10_semi/detect_circle/src/detect_circle.cpp b/detect_circle/src/detect_circle.cpp old mode 100644 new mode 100755 similarity index 100% rename from jsk_2024_10_semi/detect_circle/src/detect_circle.cpp rename to detect_circle/src/detect_circle.cpp diff --git a/jsk_2024_10_semi/scripts/catch.l b/jsk_2024_10_semi/scripts/catch.l index 3852344626..ce2655fb60 100644 --- a/jsk_2024_10_semi/scripts/catch.l +++ b/jsk_2024_10_semi/scripts/catch.l @@ -9,26 +9,71 @@ (setq *ri* (instance pr2-interface :init)) (objects (list *pr2*)) -;;コールバック関数 (setq *tfl* (instance ros::transform-listener :init)) -(defun callback (msg) - (ros::ros-info "detect circle ~A" msg) - (print (send msg :point)) - (let ((circle-pose (instance geometry_msgs::posestamped :init))) - (send circle-pose :pose :position (send msg :point)) - (send circle-pose :pose :orientation :x 0.0) - (send circle-pose :pose :orientation :y 0.0) - (send circle-pose :pose :orientation :z 0.0) - (send circle-pose :pose :orientation :w 1.0) - (setq *target-coords* (send (ros::tf-pose->coords (send circle-pose :pose)) :copy-worldcoords)) +;;検知したboxとラベルを対応させるクラス +(defclass box-label-synchronizer + :super exact-time-message-filter) + +(defmethod box-label-synchronizer + (:callback (box-msg label-msg) + (print (list box-msg label-msg)) + (print (send-all (list box-msg label-msg) :header :stamp)) + (box-cb box-msg label-msg) + ) +) + +;;コールバック関数 +;;/docker/detic_segmentor/detected_classesというトピックにidの対応あり +(defun box-cb (box-msg label-msg) + (ros::ros-info "received ~A boxes, ~A labels" (length (send box-msg :boxes)) (length (send label-msg :labels))) + (dolist (msg-conbined (map cons #'(lambda (x y) (list x y)) (send box-msg :boxes) (send label-msg :labels))) + (let (box label) + ;;(print (list msg-conbined)) + (setq box (car msg-conbined) label (cadr msg-conbined)) + ;;(print (list box label)) + (ros::ros-info "box ~A, label ~A" (send box :pose) (send label :name)) + (when (or (string= (send label :name) "ball") (string= (send label :name) "soccer_ball") (string= (send label :name) "beachball"));; 41 -> ball, 1110 ->toy + (setq *target-coords* (send (ros::tf-pose->coords (send box :pose)) :copy-worldcoords)) + ;; (setq tfc (send *tfl* :lookup-transform "base_link" "head_mount_kinect_rgb_optical_frame" (ros::time 0))) + ;; (print *target-coords*) + ;; (when *tfl* + ;; (setq *target-coords* (send tfc :transform *target-coords*)) + ;; ) + ;; (print *target-coords*) + (setq *target-dimensions* (send box :dimensions)) + (format t "coords ~A, dimension ~A~%" (send *target-coords* :worldcoords) (* (send *target-dimensions* :x) (send *target-dimensions* :y) (send *target-dimensions* :z))) + ; (when (and (< (elt (send *target-coords* :worldpos) 2) 400) + ; (> (elt (send *target-coords* :worldpos) 2) -400)) + (send *target-box* :move-to *target-coords* :world) + (ros::ros-info "update target position") + ; ) + ) ) - (setq tfc (send *tfl* :lookup-transform "base_link" "head_mount_kinect_rgb_optical_frame" (ros::time 0))) - (when tfc - (setq *target-coords* (send tfc :transform *target-coords*)) - (send *target-box* :move-to *target-coords* :world) - (print "update target position"))) + ) +) + +;;コールバック関数 +; (setq *tfl* (instance ros::transform-listener :init)) +; (defun callback (msg) +; (ros::ros-info "detect circle ~A" (send msg :point)) +; (let ((circle-pose (instance geometry_msgs::posestamped :init))) +; (send circle-pose :pose :position (send msg :point)) +; (send circle-pose :pose :orientation :x 0.0) +; (send circle-pose :pose :orientation :y 0.0) +; (send circle-pose :pose :orientation :z 0.0) +; (send circle-pose :pose :orientation :w 1.0) +; (setq *target-coords* (send (ros::tf-pose->coords (send circle-pose :pose)) :copy-worldcoords)) +; ) +; (setq tfc (send *tfl* :lookup-transform "base_link" "head_mount_kinect_rgb_optical_frame" (ros::time 0))) +; (when tfc +; (setq *target-coords* (send tfc :transform *target-coords*)) +; (send *target-box* :move-to *target-coords* :world) +; (ros::ros-info "update target position") +; ) +; ) (defun init-pose () + (send *ri* :speak-jp "初期姿勢に移動します" :wait t) (send *pr2* :reset-pose) (send *ri* :stop-grasp :arms) (send *irtviewer* :draw-objects) @@ -44,42 +89,53 @@ ;;boxを適当な初期位置で配置 - (setq *target-box* (make-cube 100 100 100)) + (setq *target-box* (make-cube 0 0 -10)) (objects (list *pr2* *target-box*)) ;;サブスクライブ - (ros::subscribe "/pointcloud_screenpoint/output_point" geometry_msgs::PointStamped #'callback) + ; (ros::subscribe "/pointcloud_screenpoint/output_point" geometry_msgs::PointStamped #'callback) + (setq box-sync (instance box-label-synchronizer :init + (list (list "/docker/detic_segmentor/output/boxes" jsk_recognition_msgs::BoundingBoxArray) + (list "/docker/detic_segmentor/detected_classes" jsk_recognition_msgs::LabelArray)))) ;;ボールが見つかるまでループ - (ros::ros-info "wait for target ~A" (send *target-box* :worldpos)) + (send *ri* :speak-jp "ボールをさがしています" :wait t) (ros::rate 3) ;; 3Hz (while (< (elt (send *target-box* :worldpos) 2) 100) + (ros::ros-info "waiting... ~A" (send *target-box* :worldpos)) (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) (send *irtviewer* :draw-objects) (x::window-main-one) (ros::spin-once) - (ros::ros-info "waiting... ~A" (send *target-box* :worldpos)) (ros::sleep) - ) + ) + (send *ri* :speak-jp "ボールを見つけました" :wait t) (ros::ros-info "start grasping... ~A" (send *target-box* :worldpos)) + (send *pr2* :larm :shoulder-p :joint-angle 0) + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) ;;自分の台座とぶつからないようにアームを動かす - ;; :larm, :rarm, :arms (setq arm :larm) (send *pr2* arm :inverse-kinematics - (send (send *target-box* :copy-worldcoords) :translate #f(-100 0 0))) + (send (send (send *target-box* :copy-worldcoords) :translate #f(0 0 100)) :rotate (deg2rad 90) :y) + :rotation-axis t + ) (send *irtviewer* :draw-objects) - (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) (send *ri* :wait-interpolation) (send *pr2* arm :inverse-kinematics - (send (send *target-box* :copy-worldcoords) :translate #f(-50 0 -20))) + (send (send (send *target-box* :copy-worldcoords) :translate #f(0 0 0)) :rotate (deg2rad 90) :y) + :rotation-axis t + ) (send *irtviewer* :draw-objects) - (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) (send *ri* :wait-interpolation) ;;掴む @@ -91,10 +147,10 @@ ;;足にぶつからないような軌道で中央へ (send *pr2* arm :inverse-kinematics - (send (send *target-box* :copy-worldcoords) :translate #f(0 0 100))) + (send (send *target-box* :copy-worldcoords) :translate #f(-100 0 200))) (send *irtviewer* :draw-objects) - (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) (send *ri* :wait-interpolation) ) @@ -103,9 +159,9 @@ ;;メイン処理の実行 (ros::ros-info "init-pose") (init-pose) -(unix:sleep 3) +(unix:sleep 1) (ros::ros-info "pick up") (pick-up) -(unix:sleep 3) +(unix:sleep 1) (init-pose) From d782f0e16f2218ab085e124be581dd27821edcf4 Mon Sep 17 00:00:00 2001 From: Rinno1478 Date: Mon, 23 Dec 2024 18:45:34 +0900 Subject: [PATCH 6/6] =?UTF-8?q?=E3=83=9C=E3=83=BC=E3=83=AB=E5=9B=9E?= =?UTF-8?q?=E5=8F=8E=E3=81=8B=E3=82=89=E3=81=A1=E3=82=83=E3=81=B6=E5=8F=B0?= =?UTF-8?q?=E8=BF=94=E3=81=97=E3=81=8C=E3=81=A7=E3=81=8D=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- jsk_2024_10_semi/scripts/catch.l | 325 ++++++++++++++++------- jsk_2024_10_semi/scripts/chabudai.l | 41 ++- jsk_2024_10_semi/scripts/conversation.py | 303 ++++++++++++++------- jsk_2024_10_semi/scripts/respond.py | 111 ++++++++ jsk_2024_10_semi/scripts/test.l | 19 +- 5 files changed, 596 insertions(+), 203 deletions(-) create mode 100755 jsk_2024_10_semi/scripts/respond.py diff --git a/jsk_2024_10_semi/scripts/catch.l b/jsk_2024_10_semi/scripts/catch.l index ce2655fb60..6b7a9a930b 100644 --- a/jsk_2024_10_semi/scripts/catch.l +++ b/jsk_2024_10_semi/scripts/catch.l @@ -7,7 +7,18 @@ ;;pr2生成 (if (not (boundp '*pr2*)) (pr2-init)) (setq *ri* (instance pr2-interface :init)) -(objects (list *pr2*)) +;; 一辺600mmの立方体を出現させる +(setq *cube* (make-cube 600 600 600)) +;; 立方体を(700, 0, 300)移動 +(send *cube* :translate (float-vector 700 0 300)) +;;boxを適当な初期位置で配置 +(setq *target-box* (make-cube 20 20 20)) +(objects (list *pr2* *target-box* *cube*)) + +;; chabudaiフラグ +(setq *chabudai* nil) +;; ボールを回収した回数 +(setq *ball-count* 0) (setq *tfl* (instance ros::transform-listener :init)) ;;検知したboxとラベルを対応させるクラス @@ -28,22 +39,12 @@ (ros::ros-info "received ~A boxes, ~A labels" (length (send box-msg :boxes)) (length (send label-msg :labels))) (dolist (msg-conbined (map cons #'(lambda (x y) (list x y)) (send box-msg :boxes) (send label-msg :labels))) (let (box label) - ;;(print (list msg-conbined)) (setq box (car msg-conbined) label (cadr msg-conbined)) - ;;(print (list box label)) (ros::ros-info "box ~A, label ~A" (send box :pose) (send label :name)) - (when (or (string= (send label :name) "ball") (string= (send label :name) "soccer_ball") (string= (send label :name) "beachball"));; 41 -> ball, 1110 ->toy + (when (or (string= (send label :name) "ball") (string= (send label :name) "soccer_ball") (string= (send label :name) "tennis_ball") (string= (send label :name) "beachball") (string= (send label :name) "ping-pong_ball")) (setq *target-coords* (send (ros::tf-pose->coords (send box :pose)) :copy-worldcoords)) - ;; (setq tfc (send *tfl* :lookup-transform "base_link" "head_mount_kinect_rgb_optical_frame" (ros::time 0))) - ;; (print *target-coords*) - ;; (when *tfl* - ;; (setq *target-coords* (send tfc :transform *target-coords*)) - ;; ) - ;; (print *target-coords*) (setq *target-dimensions* (send box :dimensions)) - (format t "coords ~A, dimension ~A~%" (send *target-coords* :worldcoords) (* (send *target-dimensions* :x) (send *target-dimensions* :y) (send *target-dimensions* :z))) - ; (when (and (< (elt (send *target-coords* :worldpos) 2) 400) - ; (> (elt (send *target-coords* :worldpos) 2) -400)) + (ros::ros-info "coords ~A, dimension ~A" (send *target-coords* :worldcoords) (* (send *target-dimensions* :x) (send *target-dimensions* :y) (send *target-dimensions* :z))) (send *target-box* :move-to *target-coords* :world) (ros::ros-info "update target position") ; ) @@ -52,116 +53,262 @@ ) ) -;;コールバック関数 -; (setq *tfl* (instance ros::transform-listener :init)) -; (defun callback (msg) -; (ros::ros-info "detect circle ~A" (send msg :point)) -; (let ((circle-pose (instance geometry_msgs::posestamped :init))) -; (send circle-pose :pose :position (send msg :point)) -; (send circle-pose :pose :orientation :x 0.0) -; (send circle-pose :pose :orientation :y 0.0) -; (send circle-pose :pose :orientation :z 0.0) -; (send circle-pose :pose :orientation :w 1.0) -; (setq *target-coords* (send (ros::tf-pose->coords (send circle-pose :pose)) :copy-worldcoords)) -; ) -; (setq tfc (send *tfl* :lookup-transform "base_link" "head_mount_kinect_rgb_optical_frame" (ros::time 0))) -; (when tfc -; (setq *target-coords* (send tfc :transform *target-coords*)) -; (send *target-box* :move-to *target-coords* :world) -; (ros::ros-info "update target position") -; ) -; ) +(defun chabudai-cb (msg) + (ros::ros-info "robot_mood: ~A" (send msg :data)) + (ros::ros-info "ball_count: ~A" *ball-count*) + (when (> (send msg :data) (- 9 *ball-count*)) + (setq *chabudai* t) + ) +) (defun init-pose () - (send *ri* :speak-jp "初期姿勢に移動します" :wait t) + ; (send *ri* :speak-jp "初期姿勢に移動します" :wait t) (send *pr2* :reset-pose) + (send *pr2* :torso :waist-z :joint-angle 150) + (send *pr2* :larm :shoulder-p :joint-angle 0) + (send *pr2* :larm :shoulder-r :joint-angle 90) + (send *pr2* :head :neck-p :joint-angle 60) (send *ri* :stop-grasp :arms) (send *irtviewer* :draw-objects) - (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) (send *ri* :wait-interpolation) ) +(defun chabudai-gaeshi () + (send *cube* :put :left-coords + (make-cascoords + :coords (send (send *cube* :copy-worldcoords) :translate (float-vector 700 0 300)) + :parent *cube*) + ) + + (send *pr2* :larm :inverse-kinematics + (send (send *cube* :get :left-coords) :copy-worldcoords) + :rotation-axis :z + ) + + (send *pr2* :reset-pose) + (send *pr2* :torso :waist-z :joint-angle 150) + (send *pr2* :larm :collar-y :joint-angle 100) + (send *pr2* :rarm :collar-y :joint-angle -100) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + (send *pr2* :larm :shoulder-p :joint-angle 74) + (send *pr2* :larm :elbow-p :joint-angle -50) + (send *pr2* :rarm :shoulder-p :joint-angle 74) + (send *pr2* :rarm :elbow-p :joint-angle -50) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + ; (send *pr2* :larm :shoulder-p :joint-angle 70) + (send *pr2* :larm :shoulder-r :joint-angle 90) + (send *pr2* :larm :collar-y :joint-angle 60) + (send *pr2* :larm :elbow-p :joint-angle -110) + (send *pr2* :larm :elbow-r :joint-angle 180) + (send *pr2* :larm :wrist-p :joint-angle -25) + (send *pr2* :larm :wrist-r :joint-angle -15) + ; (send *pr2* :rarm :shoulder-p :joint-angle 70) + (send *pr2* :rarm :shoulder-r :joint-angle -90) + (send *pr2* :rarm :collar-y :joint-angle -60) + (send *pr2* :rarm :elbow-p :joint-angle -110) + (send *pr2* :rarm :elbow-r :joint-angle 180) + (send *pr2* :rarm :wrist-p :joint-angle -25) + (send *pr2* :rarm :wrist-r :joint-angle 15) + (send *pr2* :head :neck-p :joint-angle 50) + (send *ri* :stop-grasp :arms) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + ; (send *pr2* + ; :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + ; :translate (float-vector 0.0 0.5 0.0)) + ; :move-target (send *cube* :get :left-coords) + ; :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + ; :rotation-axis t) + ; (send *pr2* + ; :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + ; :translate (float-vector 1.0 -0.5 0.0)) + ; :move-target (send *cube* :get :left-coords) + ; :link-list (send *pr2* :link-list (send (send *pr2* :rarm :end-coords) :parent)) + ; :rotation-axis t) + + ; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + ; (send *ri* :wait-interpolation) + ; (send *irtviewer* :draw-objects) + + ; (send *pr2* + ; :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + ; :translate (float-vector 0.0 -5.0 5.0)) + ; :move-target (send *cube* :get :left-coords) + ; :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + ; :rotation-axis t) + ; (send *pr2* + ; :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + ; :translate (float-vector 0.0 5.0 5.0)) + ; :move-target (send *cube* :get :left-coords) + ; :link-list (send *pr2* :link-list (send (send *pr2* :rarm :end-coords) :parent)) + ; :rotation-axis t) + + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + ;; sleep 1秒 + (unix:sleep 1) + + (send *pr2* :larm :shoulder-p :joint-angle -20) + (send *pr2* :larm :shoulder-r :joint-angle 20) + (send *pr2* :larm :elbow-p :joint-angle -100) + (send *pr2* :rarm :shoulder-p :joint-angle -20) + (send *pr2* :rarm :shoulder-r :joint-angle -20) + (send *pr2* :rarm :elbow-p :joint-angle -100) + (send *pr2* :head :neck-p :joint-angle -10) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) +) + (defun pick-up () - (send *pr2* :head :neck-p :joint-angle 60) - (send *irtviewer* :draw-objects) - (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) - (send *ri* :wait-interpolation) + (setq is-pickup-successful nil) + (while (not is-pickup-successful) + (send *target-box* :newcoords (make-coords)) + ;;サブスクライブ + ; (ros::subscribe "/pointcloud_screenpoint/output_point" geometry_msgs::PointStamped #'callback) + (setq box-sync (instance box-label-synchronizer :init + (list (list "/docker/detic_segmentor/output/boxes" jsk_recognition_msgs::BoundingBoxArray) + (list "/docker/detic_segmentor/detected_classes" jsk_recognition_msgs::LabelArray)))) - ;;boxを適当な初期位置で配置 - (setq *target-box* (make-cube 0 0 -10)) - (objects (list *pr2* *target-box*)) + ;;ボールが見つかるまでループ + ; (send *ri* :speak-jp "ボールをさがしています" :wait t) + (ros::rate 3) ;; 3Hz + (while (< (elt (send *target-box* :worldpos) 2) 400) + (ros::ros-info "waiting... ~A" (send *target-box* :worldpos)) + (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) + (send *irtviewer* :draw-objects) + (x::window-main-one) + (ros::spin-once) + (ros::sleep) + ) + ; (send *ri* :speak-jp "ボールを見つけました" :wait t) + (ros::ros-info "start grasping... ~A" (send *target-box* :worldpos)) + (send *pr2* :larm :shoulder-p :joint-angle 0) + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + ;;ボールの上空へ移動 + (setq arm :larm) - ;;サブスクライブ - ; (ros::subscribe "/pointcloud_screenpoint/output_point" geometry_msgs::PointStamped #'callback) - (setq box-sync (instance box-label-synchronizer :init - (list (list "/docker/detic_segmentor/output/boxes" jsk_recognition_msgs::BoundingBoxArray) - (list "/docker/detic_segmentor/detected_classes" jsk_recognition_msgs::LabelArray)))) + (send *pr2* arm :elbow-p :joint-angle -100) + (send *pr2* arm :collar-y :joint-angle 20) - ;;ボールが見つかるまでループ - (send *ri* :speak-jp "ボールをさがしています" :wait t) - (ros::rate 3) ;; 3Hz - (while (< (elt (send *target-box* :worldpos) 2) 100) - (ros::ros-info "waiting... ~A" (send *target-box* :worldpos)) - (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) (send *irtviewer* :draw-objects) - (x::window-main-one) - (ros::spin-once) - (ros::sleep) - ) - (send *ri* :speak-jp "ボールを見つけました" :wait t) - (ros::ros-info "start grasping... ~A" (send *target-box* :worldpos)) - (send *pr2* :larm :shoulder-p :joint-angle 0) - (send *irtviewer* :draw-objects) - (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) - (send *ri* :wait-interpolation) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1500) + (send *ri* :wait-interpolation) + (send *pr2* arm :inverse-kinematics + (send (send (send *target-box* :copy-worldcoords) :translate #f(30 0 150)) :rotate (deg2rad 90) :y) + :rotation-axis t + ) - ;;自分の台座とぶつからないようにアームを動かす - (setq arm :larm) - (send *pr2* arm :inverse-kinematics - (send (send (send *target-box* :copy-worldcoords) :translate #f(0 0 100)) :rotate (deg2rad 90) :y) - :rotation-axis t - ) + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1500) + (send *ri* :wait-interpolation) + + ;;ボールへ移動 + (send *pr2* arm :inverse-kinematics + (send (send (send *target-box* :copy-worldcoords) :translate #f(30 0 -20)) :rotate (deg2rad 90) :y) + :rotation-axis t + ) + + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) - (send *irtviewer* :draw-objects) - (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) - (send *ri* :wait-interpolation) + + ;;掴む + (send *ri* :start-grasp arm :wait t) + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + + (setq is-pickup-successful (> (send *ri* :robot arm :gripper :joint-angle) 5)) + (when (not is-pickup-successful) + (ros::ros-info "pickup failed ~%") + + ; (send *ri* :speak-jp "ボールを掴めませんでした" :wait t) + (send *pr2* arm :inverse-kinematics + (send (send (send *target-box* :copy-worldcoords) :translate #f(30 0 100)) :rotate (deg2rad 90) :y) + :rotation-axis t + ) + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (init-pose) + ) + ) (send *pr2* arm :inverse-kinematics - (send (send (send *target-box* :copy-worldcoords) :translate #f(0 0 0)) :rotate (deg2rad 90) :y) + (send (send (send *target-box* :copy-worldcoords) :translate #f(30 0 100)) :rotate (deg2rad 90) :y) :rotation-axis t ) - (send *irtviewer* :draw-objects) - (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) - (send *ri* :wait-interpolation) - - ;;掴む - (send *ri* :start-grasp arm :wait t) (send *irtviewer* :draw-objects) (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) (send *ri* :wait-interpolation) + (ros::ros-info "pickup successful ~%") + ; (send *ri* :speak-jp "ボールを持ち上げました" :wait t) - ;;足にぶつからないような軌道で中央へ - (send *pr2* arm :inverse-kinematics - (send (send *target-box* :copy-worldcoords) :translate #f(-100 0 200))) + ;; + (send *pr2* :larm :collar-y :joint-angle 50) + (send *pr2* :larm :shoulder-p :joint-angle 0) + (send *pr2* :larm :shoulder-r :joint-angle 90) + (send *pr2* :larm :elbow-p :joint-angle -30) + (send *pr2* :larm :elbow-r :joint-angle 90) + (send *pr2* :larm :wrist-p :joint-angle -10) + (send *pr2* :larm :wrist-r :joint-angle 0) (send *irtviewer* :draw-objects) (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) (send *ri* :wait-interpolation) -) + ;;離す + (send *ri* :stop-grasp :arms) + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) +) +; (init-pose) +; (unix:sleep 1) +; (chabudai-gaeshi) ;;メイン処理の実行 -(ros::ros-info "init-pose") -(init-pose) -(unix:sleep 1) -(ros::ros-info "pick up") -(pick-up) -(unix:sleep 1) -(init-pose) +(ros::advertise "ball_count" std_msgs::Int32 1) +(ros::subscribe "/robot_mood" std_msgs::Int32 #'chabudai-cb) +(send *ri* :speak-jp "ボールを片付けます" :wait t) +(while (not *chabudai*) + (x::window-main-one) + (ros::ros-info "init-pose") + (init-pose) + (unix:sleep 1) + (ros::ros-info "pick up") + (pick-up) + + (setq *ball-count* (+ *ball-count* 1)) + (setq msg (instance std_msgs::Int32 :init)) + (send msg :data *ball-count*) + (ros::ros-info "ball_count: ~A" *ball-count*) + (ros::publish "ball_count" msg) + (ros::spin-once) + + (when *chabudai* + (chabudai-gaeshi) + ) +) \ No newline at end of file diff --git a/jsk_2024_10_semi/scripts/chabudai.l b/jsk_2024_10_semi/scripts/chabudai.l index d3eec9a361..01094a7593 100644 --- a/jsk_2024_10_semi/scripts/chabudai.l +++ b/jsk_2024_10_semi/scripts/chabudai.l @@ -29,20 +29,46 @@ (defun callback (msg) (format t "robot_mood: ~A~%" (send msg :data)) - (when (< (send msg :data) 2) + (when (< (send msg :data) 4) (setq *chabudai* t) ) ) -(defun chabudai_gaeshi () +(defun chabudai-gaeshi () + (send *cube* :put :left-coords + (make-cascoords + :coords (send (send *cube* :copy-worldcoords) :translate (float-vector 700 0 300)) + :parent *cube*) + ) + + (send *pr2* :larm :inverse-kinematics + (send (send *cube* :get :left-coords) :copy-worldcoords) + :rotation-axis :z + ) + + (send *pr2* :reset-pose) + (send *pr2* :larm :collar-y :joint-angle 100) + (send *pr2* :rarm :collar-y :joint-angle -100) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + (send *pr2* :larm :shoulder-p :joint-angle 70) + (send *pr2* :larm :elbow-p :joint-angle -50) + (send *pr2* :rarm :shoulder-p :joint-angle 70) + (send *pr2* :rarm :elbow-p :joint-angle -50) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + ; (send *pr2* :larm :shoulder-p :joint-angle 70) (send *pr2* :larm :shoulder-r :joint-angle 60) (send *pr2* :larm :collar-y :joint-angle 32) (send *pr2* :larm :elbow-p :joint-angle -110) (send *pr2* :larm :elbow-r :joint-angle 180) (send *pr2* :larm :wrist-p :joint-angle -30) (send *pr2* :larm :wrist-r :joint-angle -30) - (send *pr2* :rarm :shoulder-p :joint-angle 70) + ; (send *pr2* :rarm :shoulder-p :joint-angle 70) (send *pr2* :rarm :shoulder-r :joint-angle -60) (send *pr2* :rarm :collar-y :joint-angle -32) (send *pr2* :rarm :elbow-p :joint-angle -110) @@ -51,14 +77,10 @@ (send *pr2* :rarm :wrist-r :joint-angle 30) (send *pr2* :head :neck-p :joint-angle 50) (send *ri* :stop-grasp :arms) - (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) (send *ri* :wait-interpolation) (send *irtviewer* :draw-objects) - ;; sleep 1秒 - (unix:sleep 1) - - (send *pr2* :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) :translate (float-vector 1.0 0.5 0.0)) @@ -76,9 +98,6 @@ (send *ri* :wait-interpolation) (send *irtviewer* :draw-objects) - ;; sleep 1秒 - (unix:sleep 1) - (send *pr2* :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) :translate (float-vector 0.0 0.0 2.0)) diff --git a/jsk_2024_10_semi/scripts/conversation.py b/jsk_2024_10_semi/scripts/conversation.py index 932a507933..a6aae5e4b9 100644 --- a/jsk_2024_10_semi/scripts/conversation.py +++ b/jsk_2024_10_semi/scripts/conversation.py @@ -1,3 +1,196 @@ +# import os +# import sys +# import google.generativeai as genai +# from google.generativeai.types import HarmCategory, HarmBlockThreshold +# # import speech_recognition as sr +# # from gtts import gTTS +# import rospy +# from std_msgs.msg import String, Int32 +# from sound_play.msg import SoundRequest +# from speech_recognition_msgs.msg import SpeechRecognitionCandidates +# from difflib import SequenceMatcher + +# # ~/.bashrcに"export GEMINI_API_KEY='自分のapi_key'"を書いている +# GEMINI_API_KEY=os.getenv('GEMINI_API_KEY') +# MAX_TURNS = 20 + +# # def recognize_speech(): +# # # 音声認識のためのRecognizerオブジェクトを作成 +# # recognizer = sr.Recognizer() +# # # マイクから音声を取得 +# # with sr.Microphone() as source: +# # print('\033[36m'+"マイクの入力を待っています..."+'\033[0m') +# # recognizer.adjust_for_ambient_noise(source) # 周囲の雑音を調整 +# # audio = recognizer.listen(source) +# # try: +# # # 音声を日本語で認識 +# # text = recognizer.recognize_google(audio, language='ja-JP') +# # print('\033[36m'+f"認識結果: {text}"+'\033[0m') +# # return text +# # except sr.UnknownValueError: +# # print('\033[36m'+"音声を認識できませんでした。"+'\033[0m') +# # return None +# # except sr.RequestError as e: +# # print('\033[36m'+f"音声認識サービスに問題があります: {e}"+'\033[0m') +# # return None + +# # def text_to_speech_japanese(text): +# # # 日本語テキストを音声に変換 +# # tts = gTTS(text=text, lang='ja') +# # # 一時的な音声ファイルを保存 +# # filename = 'temp_audio.mp3' +# # tts.save(filename) +# # os.system(f"mpg123 {filename}") + + +# class GenerativeModel: +# def __init__(self, api_key, tools=[], model_name='gemini-1.5-pro'): +# genai.configure(api_key=api_key) +# if len(tools) > 0: +# self.model = genai.GenerativeModel(model_name=model_name, tools=tools) +# else: +# self.model = genai.GenerativeModel(model_name=model_name) +# self.safety_settings = { +# HarmCategory.HARM_CATEGORY_SEXUALLY_EXPLICIT: HarmBlockThreshold.BLOCK_ONLY_HIGH, +# HarmCategory.HARM_CATEGORY_HATE_SPEECH: HarmBlockThreshold.BLOCK_ONLY_HIGH, +# HarmCategory.HARM_CATEGORY_HARASSMENT: HarmBlockThreshold.BLOCK_ONLY_HIGH, +# HarmCategory.HARM_CATEGORY_DANGEROUS_CONTENT:HarmBlockThreshold.BLOCK_ONLY_HIGH +# } +# """ +# 会話の安全性設定 +# HarmCategory.HARM_CATEGORY_SEXUALLY_EXPLICIT: セクシャルな表現 +# HarmCategory.HARM_CATEGORY_HATE_SPEECH: 憎悪表現 +# HarmCategory.HARM_CATEGORY_HARASSMENT: 嫌がらせ +# HarmCategory.HARM_CATEGORY_DANGEROUS_CONTENT: 危険な表現 +# HarmBlockThreshold.BLOCK_NONE: ブロックしない +# HarmBlockThreshold.BLOCK_ONLY_HIGH: 高いレベルの有害な表現をブロック +# HarmBlockThreshold.BLOCK_MEDIUM_AND_ABOVE: 中以上のレベルの有害な表現をブロック +# HarmBlockThreshold.BLOCK_LOW_AND_ABOVE: 低以上のレベルの有害な表現をブロック +# HarmBlockThreshold.HARM_BLOCK_THRESHOLD_UNSPECIFIED: 未指定、デフォルトの設定 +# """ +# if len(tools) > 0: +# self.chat = self.model.start_chat(history=[], enable_automatic_function_calling=True) +# else: +# self.chat = self.model.start_chat(history=[]) +# def send_pre_prompt(self, pre_prompt): +# self.chat.send_message(pre_prompt, safety_settings=self.safety_settings) +# def send_message(self, message): +# response = self.chat.send_message(message, safety_settings=self.safety_settings) +# return response +# def get_history(self): +# return self.chat.history + +# # def conversation(): +# # input_mode = "text" +# # args = sys.argv +# # if len(args) > 2: +# # print("Usage: python conversation.py [input_mode]") +# # return +# # elif len(args) == 2: +# # if args[1] == "speech": +# # input_mode = "speech" +# # elif args[1] == "text": +# # input_mode = "text" +# # else: +# # print("Usage: input_mode should be either 'speech' or 'text'") +# # return +# # # モデルの初期化 +# # chat_ai = GenerativeModel(GEMINI_API_KEY, tools=[set_mood_level], model_name='gemini-pro') +# # pre_prompt = "# 設定 \ +# # * あなた(gemini)はuserと対話するAIです。 \ +# # * userはあなたに話しかけることができます。 \ +# # * userが話しかけると、あなたはuserに返答します。 \ +# # # 制約\ +# # * あなたはuserのメッセージと、前回のmood_levelに基づいて、AIの機嫌を表すmood_levelを設定します。 \ +# # * mood_levelの初期値は5です。 \ +# # * mood_levelは1から9の整数で表されます。 \ +# # * 毎回の返答の際には、mood_levelを表示する関数（set_mood_level）の呼び出しを必ず行います。 \ +# # * 返答のテキストにはmood_levelについての情報を含めてはいけません。 \ +# # # 注意 \ +# # * あなたはuserのメッセージに対して、適切な返答を行う必要があります。" +# # chat_ai.send_pre_prompt(pre_prompt) +# # for turn in range(MAX_TURNS): +# # user_input = None +# # if input_mode == "text": +# # user_input = input("User >> ") +# # elif input_mode == "speech": +# # user_input = recognize_speech() +# # if user_input is None: +# # print("テキスト入力モードに切り替えます。") +# # input_mode = "text" +# # user_input = input("User >> ") +# # if user_input == "exit": +# # break +# # response = chat_ai.send_message(user_input) +# # print("AI >>", response.text) +# # if input_mode == "speech": +# # text_to_speech_japanese(response) +# # print(chat_ai.get_history()) + +# class Responder(): +# def __init__(self): +# # self.sub = rospy.Subscriber('test_input_text', String, self.callback) +# self.sub = rospy.Subscriber('/speech_to_text', SpeechRecognitionCandidates, self.callback, queue_size=1) +# self.pub = rospy.Publisher('/robotsound_jp', SoundRequest, queue_size=1) +# self.pub2 = rospy.Publisher('/robot_mood', Int32, queue_size=1) +# self.chat_ai = GenerativeModel(GEMINI_API_KEY, tools=[self.set_mood_level], model_name='gemini-pro') +# pre_prompt = "# 設定 \ +# * あなた(gemini)はuserと対話するAIです。 \ +# * userはあなたに話しかけることができます。 \ +# * userが話しかけた後、あなたはuserに返答します。 \ +# * あなたはロボットで、今は机の上のボールを片付けながらuserと会話しています。 \ +# # 制約 \ +# * あなたはuserのメッセージに基づいて、AIの機嫌を表すmood_levelを設定します。 \ +# * mood_levelの初期値は5です。 \ +# * mood_levelは1から9の整数で表されます。 \ +# * 毎回の返答の際には、mood_levelを表示する関数（set_mood_level）の呼び出しを必ず行います。 \ +# * 返答のテキストにはmood_level（気分レベル）についての情報を含めてはいけません。" +# self.chat_ai.send_pre_prompt(pre_prompt) +# self.last_speak_time = rospy.Time.now() +# self.robot_mood_level = 5 + +# def callback(self, data): +# global robot_mood_level +# rospy.loginfo(data.transcript) +# if rospy.Time.now() - self.last_speak_time < rospy.Duration(10): +# rospy.loginfo("skip") +# return +# response = self.chat_ai.send_message(data.transcript) +# rospy.loginfo(response.text) +# sound_req = SoundRequest() +# sound_req.sound = -3 +# sound_req.command = 1 +# sound_req.volume = 70.0 +# sound_req.arg = str(response.text) +# self.pub.publish(sound_req) +# self.last_speak_time = rospy.Time.now() +# robot_mood_level = Int32() +# robot_mood_level.data = self.robot_mood_level +# self.pub2.publish(robot_mood_level) + +# def publish(self, data): +# global robot_mood_level +# self.pub.publish(data) +# self.pub2.publish(robot_mood_level) + +# def set_mood_level(self, mood_level:int) -> int: +# """setting AI's mood level based on the messages from the user and the previous mood level of the AI. default is 5. AI must call this function every time it responds to the user. +# Args: +# message: the message from the user. +# Returns: +# mood_level: the mood level of the AI from 1 to 9. 1 is the worst mood (angry) and 9 is the best mood (happy). +# """ +# print(f"AI's mood level: {mood_level}") +# self.robot_mood_level = int(mood_level) + + +# if __name__ == "__main__": +# rospy.init_node('responder') +# responder = Responder() +# rospy.spin() + + +#!/usr/bin/env python3 import os import sys import google.generativeai as genai @@ -8,40 +201,11 @@ from std_msgs.msg import String, Int32 from sound_play.msg import SoundRequest from speech_recognition_msgs.msg import SpeechRecognitionCandidates -from difflib import SequenceMatcher +# from difflib import SequenceMatcher # ~/.bashrcに"export GEMINI_API_KEY='自分のapi_key'"を書いている GEMINI_API_KEY=os.getenv('GEMINI_API_KEY') -MAX_TURNS = 5 - -# def recognize_speech(): -# # 音声認識のためのRecognizerオブジェクトを作成 -# recognizer = sr.Recognizer() -# # マイクから音声を取得 -# with sr.Microphone() as source: -# print('\033[36m'+"マイクの入力を待っています..."+'\033[0m') -# recognizer.adjust_for_ambient_noise(source) # 周囲の雑音を調整 -# audio = recognizer.listen(source) -# try: -# # 音声を日本語で認識 -# text = recognizer.recognize_google(audio, language='ja-JP') -# print('\033[36m'+f"認識結果: {text}"+'\033[0m') -# return text -# except sr.UnknownValueError: -# print('\033[36m'+"音声を認識できませんでした。"+'\033[0m') -# return None -# except sr.RequestError as e: -# print('\033[36m'+f"音声認識サービスに問題があります: {e}"+'\033[0m') -# return None - -# def text_to_speech_japanese(text): -# # 日本語テキストを音声に変換 -# tts = gTTS(text=text, lang='ja') -# # 一時的な音声ファイルを保存 -# filename = 'temp_audio.mp3' -# tts.save(filename) -# os.system(f"mpg123 {filename}") - +MAX_TURNS = 20 class GenerativeModel: def __init__(self, api_key, tools=[], model_name='gemini-1.5-pro'): @@ -80,81 +244,32 @@ def send_message(self, message): def get_history(self): return self.chat.history -# def conversation(): -# input_mode = "text" -# args = sys.argv -# if len(args) > 2: -# print("Usage: python conversation.py [input_mode]") -# return -# elif len(args) == 2: -# if args[1] == "speech": -# input_mode = "speech" -# elif args[1] == "text": -# input_mode = "text" -# else: -# print("Usage: input_mode should be either 'speech' or 'text'") -# return -# # モデルの初期化 -# chat_ai = GenerativeModel(GEMINI_API_KEY, tools=[set_mood_level], model_name='gemini-pro') -# pre_prompt = "# 設定 \ -# * あなた(gemini)はuserと対話するAIです。 \ -# * userはあなたに話しかけることができます。 \ -# * userが話しかけると、あなたはuserに返答します。 \ -# # 制約\ -# * あなたはuserのメッセージと、前回のmood_levelに基づいて、AIの機嫌を表すmood_levelを設定します。 \ -# * mood_levelの初期値は5です。 \ -# * mood_levelは1から9の整数で表されます。 \ -# * 毎回の返答の際には、mood_levelを表示する関数（set_mood_level）の呼び出しを必ず行います。 \ -# * 返答のテキストにはmood_levelについての情報を含めてはいけません。 \ -# # 注意 \ -# * あなたはuserのメッセージに対して、適切な返答を行う必要があります。" -# chat_ai.send_pre_prompt(pre_prompt) -# for turn in range(MAX_TURNS): -# user_input = None -# if input_mode == "text": -# user_input = input("User >> ") -# elif input_mode == "speech": -# user_input = recognize_speech() -# if user_input is None: -# print("テキスト入力モードに切り替えます。") -# input_mode = "text" -# user_input = input("User >> ") -# if user_input == "exit": -# break -# response = chat_ai.send_message(user_input) -# print("AI >>", response.text) -# if input_mode == "speech": -# text_to_speech_japanese(response) -# print(chat_ai.get_history()) class Responder(): def __init__(self): # self.sub = rospy.Subscriber('test_input_text', String, self.callback) - self.sub = rospy.Subscriber('/speech_to_text', SpeechRecognitionCandidates, self.callback, queue_size=1) + self.sub = rospy.Subscriber('ball_count', Int32, self.callback, queue_size=1) self.pub = rospy.Publisher('/robotsound_jp', SoundRequest, queue_size=1) self.pub2 = rospy.Publisher('/robot_mood', Int32, queue_size=1) self.chat_ai = GenerativeModel(GEMINI_API_KEY, tools=[self.set_mood_level], model_name='gemini-pro') pre_prompt = "# 設定 \ - * あなた(gemini)はuserと対話するAIです。 \ - * userはあなたに話しかけることができます。 \ - * userが話しかけた後、あなたはuserに返答します。 \ + あなた(gemini)はロボットで、今は机の上のボールを片付けています。 \ + 片付けたボールの数が整数で入力されるので、ボールを片付けるほどイライラしてください。 \ # 制約 \ - * あなたはuserのメッセージに基づいて、AIの機嫌を表すmood_levelを設定します。 \ - * mood_levelの初期値は5です。 \ - * mood_levelは1から9の整数で表されます。 \ - * 毎回の返答の際には、mood_levelを表示する関数（set_mood_level）の呼び出しを必ず行います。 \ - * 返答のテキストにはmood_level（気分レベル）についての情報を含めてはいけません。" + あなたは入力に基づいて、AIの機嫌を表すmood_level(イライラ度)を設定します。 \ + mood_levelの初期値は5です。 \ + mood_levelは1から9の整数で表されます。 \ + mood_levelは9が不機嫌で、1が幸せです。 \ + 毎回の返答の際には、mood_levelを表示する関数（set_mood_level）の呼び出しを必ず行います。 \ + 返答のテキストにはmood_levelについての情報を含めてはいけません。" self.chat_ai.send_pre_prompt(pre_prompt) self.last_speak_time = rospy.Time.now() self.robot_mood_level = 5 def callback(self, data): global robot_mood_level - rospy.loginfo(data.transcript) - if rospy.Time.now() - self.last_speak_time < rospy.Duration(10): - rospy.loginfo("skip") - return - response = self.chat_ai.send_message(data.transcript) + rospy.loginfo(data.data) + response = self.chat_ai.send_message(str(data.data)) rospy.loginfo(response.text) sound_req = SoundRequest() sound_req.sound = -3 @@ -173,11 +288,11 @@ def publish(self, data): self.pub2.publish(robot_mood_level) def set_mood_level(self, mood_level:int) -> int: - """setting AI's mood level based on the messages from the user and the previous mood level of the AI. default is 5. AI must call this function every time it responds to the user. + """setting AI's mood level based on inputs from the user and the previous mood level of the AI. default is 5. AI must call this function every time it responds to the user. Args: - message: the message from the user. + number: the number of balls put away Returns: - mood_level: the mood level of the AI from 1 to 9. 1 is the worst mood (angry) and 9 is the best mood (happy). + mood_level: the mood level of the AI from 1 to 9. 9 is the worst mood (angry) and 1 is the best mood (happy). """ print(f"AI's mood level: {mood_level}") self.robot_mood_level = int(mood_level) diff --git a/jsk_2024_10_semi/scripts/respond.py b/jsk_2024_10_semi/scripts/respond.py new file mode 100755 index 0000000000..2ee07c40eb --- /dev/null +++ b/jsk_2024_10_semi/scripts/respond.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 +import os +import sys +import google.generativeai as genai +from google.generativeai.types import HarmCategory, HarmBlockThreshold +# import speech_recognition as sr +# from gtts import gTTS +import rospy +from std_msgs.msg import String, Int32 +from sound_play.msg import SoundRequest +from speech_recognition_msgs.msg import SpeechRecognitionCandidates +# from difflib import SequenceMatcher + +# ~/.bashrcに"export GEMINI_API_KEY='自分のapi_key'"を書いている +GEMINI_API_KEY=os.getenv('GEMINI_API_KEY') +MAX_TURNS = 20 + +class GenerativeModel: + def __init__(self, api_key, tools=[], model_name='gemini-1.5-pro'): + genai.configure(api_key=api_key) + if len(tools) > 0: + self.model = genai.GenerativeModel(model_name=model_name, tools=tools) + else: + self.model = genai.GenerativeModel(model_name=model_name) + self.safety_settings = { + HarmCategory.HARM_CATEGORY_SEXUALLY_EXPLICIT: HarmBlockThreshold.BLOCK_ONLY_HIGH, + HarmCategory.HARM_CATEGORY_HATE_SPEECH: HarmBlockThreshold.BLOCK_ONLY_HIGH, + HarmCategory.HARM_CATEGORY_HARASSMENT: HarmBlockThreshold.BLOCK_ONLY_HIGH, + HarmCategory.HARM_CATEGORY_DANGEROUS_CONTENT:HarmBlockThreshold.BLOCK_ONLY_HIGH + } + """ + 会話の安全性設定 + HarmCategory.HARM_CATEGORY_SEXUALLY_EXPLICIT: セクシャルな表現 + HarmCategory.HARM_CATEGORY_HATE_SPEECH: 憎悪表現 + HarmCategory.HARM_CATEGORY_HARASSMENT: 嫌がらせ + HarmCategory.HARM_CATEGORY_DANGEROUS_CONTENT: 危険な表現 + HarmBlockThreshold.BLOCK_NONE: ブロックしない + HarmBlockThreshold.BLOCK_ONLY_HIGH: 高いレベルの有害な表現をブロック + HarmBlockThreshold.BLOCK_MEDIUM_AND_ABOVE: 中以上のレベルの有害な表現をブロック + HarmBlockThreshold.BLOCK_LOW_AND_ABOVE: 低以上のレベルの有害な表現をブロック + HarmBlockThreshold.HARM_BLOCK_THRESHOLD_UNSPECIFIED: 未指定、デフォルトの設定 + """ + if len(tools) > 0: + self.chat = self.model.start_chat(history=[], enable_automatic_function_calling=True) + else: + self.chat = self.model.start_chat(history=[]) + def send_pre_prompt(self, pre_prompt): + self.chat.send_message(pre_prompt, safety_settings=self.safety_settings) + def send_message(self, message): + response = self.chat.send_message(message, safety_settings=self.safety_settings) + return response + def get_history(self): + return self.chat.history + + +class Responder(): + def __init__(self): + # self.sub = rospy.Subscriber('test_input_text', String, self.callback) + self.sub = rospy.Subscriber('ball_count', Int32, self.callback, queue_size=1) + self.pub = rospy.Publisher('/robotsound_jp', SoundRequest, queue_size=1) + self.pub2 = rospy.Publisher('/robot_mood', Int32, queue_size=1) + self.chat_ai = GenerativeModel(GEMINI_API_KEY, tools=[self.set_mood_level], model_name='gemini-pro') + pre_prompt = "# 設定 \ + * あなた(gemini)はロボットで、今は机の上のボールを片付けています。 \ + * 片付けたボールの数が整数で入力されるので、片付けても片付けが進まないことに対してイライラしてください。 \ + # 制約 \ + * あなたは入力に基づいて、AIの機嫌を表すmood_levelを設定します。 \ + * mood_levelの初期値は5です。 \ + * mood_levelは1から9の整数で表されます。 \ + * 毎回の返答の際には、mood_levelを表示する関数（set_mood_level）の呼び出しを必ず行います。 \ + * 返答のテキストにはmood_level（気分レベル）についての情報を含めてはいけません。" + self.chat_ai.send_pre_prompt(pre_prompt) + self.last_speak_time = rospy.Time.now() + self.robot_mood_level = 5 + + def callback(self, data): + global robot_mood_level + rospy.loginfo(data.data) + response = self.chat_ai.send_message(str(data.data)) + rospy.loginfo(response.text) + sound_req = SoundRequest() + sound_req.sound = -3 + sound_req.command = 1 + sound_req.volume = 70.0 + sound_req.arg = str(response.text) + self.pub.publish(sound_req) + self.last_speak_time = rospy.Time.now() + robot_mood_level = Int32() + robot_mood_level.data = self.robot_mood_level + self.pub2.publish(robot_mood_level) + + def publish(self, data): + global robot_mood_level + self.pub.publish(data) + self.pub2.publish(robot_mood_level) + + def set_mood_level(self, mood_level:int) -> int: + """setting AI's mood level based on inputs from the user and the previous mood level of the AI. default is 5. AI must call this function every time it responds to the user. + Args: + message: the message from the user. + Returns: + mood_level: the mood level of the AI from 1 to 9. 1 is the worst mood (angry) and 9 is the best mood (happy). + """ + print(f"AI's mood level: {mood_level}") + self.robot_mood_level = int(mood_level) + + +if __name__ == "__main__": + rospy.init_node('responder') + responder = Responder() + rospy.spin() diff --git a/jsk_2024_10_semi/scripts/test.l b/jsk_2024_10_semi/scripts/test.l index 42d105d1cf..9802d41bdf 100644 --- a/jsk_2024_10_semi/scripts/test.l +++ b/jsk_2024_10_semi/scripts/test.l @@ -1,11 +1,12 @@ (require "package://pr2eus/pr2.l") ;;import pr2 package -(require "package://pr2eus/pr2-utils.l") -(require "package://pr2eus/pr2-interface.l") -(if (not (boundp '*pr2*)) (pr2-init)) ;; (setq *pr2* (pr2))) ;; (pr2) is alternated by *pr2* +; (require "package://pr2eus/pr2-utils.l") +; (require "package://pr2eus/pr2-interface.l") +; (if (not (boundp '*pr2*)) (pr2-init)) ;; (setq *pr2* (pr2))) ;; (pr2) is alternated by *pr2* +(if (not (boundp '*pr2*)) (setq *pr2* (pr2))) (send *pr2* :reset-pose) ;; 初期姿勢 -(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) -(send *ri* :wait-interpolation) +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) (objects (list *pr2*)) (send *pr2* :larm :shoulder-p :joint-angle 50) @@ -16,13 +17,13 @@ (send *pr2* :rarm :wrist-p :joint-angle -20) (send *pr2* :head :neck-p :joint-angle 50) (send *irtviewer* :draw-objects) -(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) -(send *ri* :wait-interpolation) +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) (send *pr2* :larm :shoulder-p :joint-angle 5) (send *pr2* :rarm :shoulder-p :joint-angle 5) (send *pr2* :head :neck-p :joint-angle -10) (send *irtviewer* :draw-objects) -(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) -(send *ri* :wait-interpolation) +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation)