diff --git a/.gitignore b/.gitignore index b493dd467f172fd0e12a7ccaf6db610fb0f1bdda..56bd1b54a76aa0defcdf89fcf771b662271784a3 100644 --- a/.gitignore +++ b/.gitignore @@ -7,6 +7,7 @@ __pycache__ *.pyc *venv +<<<<<<< HEAD *.zip *.png *.log @@ -18,3 +19,6 @@ gan pyRoIS_higashi +======= +*.log +>>>>>>> dev diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000000000000000000000000000000000000..762c6e97cbd21146049a79e73285513f301267ac --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "route_guidance_ros/scripts/pyRoIS_higashi"] + path = route_guidance_ros/scripts/pyRoIS_higashi + url = https://inohira.mns.kyutech.ac.jp/git/higashi/pyRoIS_higashi.git diff --git a/README.md b/README.md index acf599d8e45e7d6436b0fe104e72201c614b2196..dbec7297f916b96197ebcfece71fa83e09bc65af 100644 --- a/README.md +++ b/README.md @@ -6,16 +6,19 @@ Gazebo (meta) package for RoIS ## 導入方法 -$ ROS 環境の構築 -$ `mkdir gazebo_RoIS_ws` -$ `cd gazebo_RoIS_ws` -$ `git clone https://inohira.mns.kyutech.ac.jp/git/tanacchi/gazebo_RoIS_tanacchi.git src` -$ `cd src` -$ `catkin_init_workspace` -$ `cd -` -$ `catkin_make` -$ `echo "source ${PWD}/devel/setup.bash" >> ~/.bashrc` # 必要ならば +ROS 環境の構築後以下を実行 +```shell +$ mkdir gazebo_RoIS_ws +$ cd gazebo_RoIS_ws +$ git clone https://inohira.mns.kyutech.ac.jp/git/tanacchi/gazebo_RoIS_tanacchi.git src +$ cd src +$ catkin_init_workspace +$ sh install_packages.sh +$ cd .. +$ catkin_make +$ echo "source ${PWD}/devel/setup.bash" >> ~/.bashrc` # 必要ならば +``` ## ディレクトリ構成 @@ -34,7 +37,7 @@ gazebo_RoIS_tanacchi // 当該パッケージ 1. teleop_twist_keyboard を起動． 操作方法は標準出力もしくはROS公式ドキュメントを参照のこと． - $ `roslaunch rois_gazebo teleop_keybaord.launch` + $ `roslaunch rois_gazebo teleop_keyboard.launch` 2. 別のターミナルで，Gazebo を起動 $ `roslaunch rois_gazebo setup_gazebo.launch` diff --git a/rois_2dnav_gazebo/map/guruguru/guruguru.pgm b/rois_2dnav_gazebo/map/guruguru/guruguru.pgm new file mode 100644 index 0000000000000000000000000000000000000000..21690410fc012ebe43e3e0c89f718e146733ebe2 --- /dev/null +++ b/rois_2dnav_gazebo/map/guruguru/guruguru.pgm @@ -0,0 +1,5 @@ +P5 +# CREATOR: map_saver.cpp 0.050 m/pix +4000 4000 +255 + \ No newline at end of file diff --git a/rois_2dnav_gazebo/map/guruguru/guruguru.yaml b/rois_2dnav_gazebo/map/guruguru/guruguru.yaml new file mode 100644 index 0000000000000000000000000000000000000000..c8258434f743f2769f19edb13659097dd583c344 --- /dev/null +++ b/rois_2dnav_gazebo/map/guruguru/guruguru.yaml @@ -0,0 +1,7 @@ +image: guruguru.pgm +resolution: 0.050000 +origin: [-100.000000, -100.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/rois_bringup/launch/navigation.launch b/rois_bringup/launch/navigation.launch index 6d2981a9199dd72627a1362f0610cd284e20fb46..bfb7e62277603ea27cb9cba5156bef482fa0f10e 100644 --- a/rois_bringup/launch/navigation.launch +++ b/rois_bringup/launch/navigation.launch @@ -9,6 +9,25 @@ name="rvizconfig" value="$(find rois_2dnav_gazebo)/rviz/multi_navigation.rviz" /> + + + + + + + + + + + + + + @@ -29,6 +69,12 @@ + + + value="$(arg robot2_init_x)" /> + value="$(arg robot2_init_y)" /> + value="$(arg robot3_init_x)" /> + value="$(arg robot3_init_y)" /> + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + 65535 + + + + + 100 + 50 + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 4.44925 -0.07264 0 0 -0 0 + + + + + 24.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 24.5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -23.55 2.25 0 0 -0 1.5708 + 0 + 0 + 0 + + + + + + 21.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 21.5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 0.874999 4.925 0 0 -0 0 + 0 + 0 + 0 + + + + + + 10.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 10.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 11.55 -0.375 0 0 -0 -1.5708 + 0 + 0 + 0 + + + + + + 17.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 17.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 3 -5.675 0 0 -0 3.14159 + 0 + 0 + 0 + + + + + + 7.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 7.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -5.55 -1.875 0 0 -0 1.5708 + 0 + 0 + 0 + + + + + + 12.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 12.5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 0.624999 1.925 0 0 -0 0 + 0 + 0 + 0 + + + + + + 4.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 4.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 6.8 -0.125 0 0 -0 -1.5708 + 0 + 0 + 0 + + + + + + 8.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 8.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 2.5 -2.175 0 0 -0 3.14159 + 0 + 0 + 0 + + + + + + 47.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 47.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -1e-06 14.425 0 0 -0 0 + 0 + 0 + 0 + + + + + + 29 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 29 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 23.55 0 0 0 -0 -1.5708 + 0 + 0 + 0 + + + + + + 41.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 41.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 3 -14.425 0 0 -0 3.14159 + 0 + 0 + 0 + + + + + + 24.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 24.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -17.55 -2.125 0 0 -0 1.5708 + 0 + 0 + 0 + + + + + + 34.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 34.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -0.500001 10.175 0 0 -0 0 + 0 + 0 + 0 + + + + + + 20.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 20.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 16.55 0.125 0 0 -0 -1.5708 + 0 + 0 + 0 + + + + + + 26.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 26.5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 3.375 -9.925 0 0 -0 3.14159 + 0 + 0 + 0 + + + + + + 15 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 15 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -9.8 -2.5 0 0 -0 1.5708 + 0 + 0 + 0 + + 1 + + + 305 168000000 + 306 621197083 + 1583827816 406143483 + 305168 + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 4.56178 -0.012511 0 0 0 -3.13034 + 1 1 1 + + 28.1356 -1.99747 0 0 0 -1.55954 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.74224 -4.94704 0 0 0 -3.13034 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.9917 0.232549 0 0 -0 1.58204 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.49814 5.62839 0 0 -0 0.011246 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0903 1.9248 0 0 0 -1.55954 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.95848 -1.94442 0 0 0 -3.13034 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.23919 0.035994 0 0 -0 1.58204 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.03748 2.13423 0 0 -0 0.011246 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.72404 -14.4366 0 0 0 -3.13034 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -18.9867 -0.277406 0 0 -0 1.58204 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.39972 14.3778 0 0 -0 0.011246 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 22.0868 2.30976 0 0 0 -1.55954 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.1762 -10.1812 0 0 0 -3.13034 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -11.9858 -0.323661 0 0 -0 1.58204 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.07536 9.8739 0 0 -0 0.011246 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 14.333 2.59756 0 0 0 -1.55954 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + -14.8492 -16.4672 70.1336 0 1.28843 0.034594 + orbit + perspective + + + + diff --git a/rois_gazebo/launch/setup_gazebo.launch b/rois_gazebo/launch/setup_gazebo.launch index 99fb925a5a4ef0d774e57621c90bc1ef27355eba..c144292cb2e76af85caa64926f839f13d7202c3b 100644 --- a/rois_gazebo/launch/setup_gazebo.launch +++ b/rois_gazebo/launch/setup_gazebo.launch @@ -8,6 +8,24 @@ + + + + + + + value="$(arg robot1_init_x)" /> + value="$(arg robot1_init_y)" /> @@ -34,10 +52,10 @@ value="robot2"/> + value="$(arg robot2_init_x)" /> + value="$(arg robot2_init_y)" /> @@ -46,10 +64,10 @@ value="robot3"/> + value="$(arg robot3_init_x)" /> + value="$(arg robot3_init_y)" /> diff --git a/route_guidance_ros/scripts/client_info_gazebo.json b/route_guidance_ros/scripts/client_info_gazebo.json new file mode 100644 index 0000000000000000000000000000000000000000..47b920dfcc8c498063fb808db6dc6787fd246390 --- /dev/null +++ b/route_guidance_ros/scripts/client_info_gazebo.json @@ -0,0 +1,22 @@ +{ + "urn:x-rois:def:HRIComponent:Kyutech:main:SystemInformation": { + "module": "System_Information_client", + "class":"System_Information_Client", + "url":"http://127.0.0.1:8002" + }, + "urn:x-rois:def:HRIComponent:Kyutech:main:PersonLocalization": { + "module": "Person_Localization_client", + "class":"Person_Localization_Client", + "url":"http://127.0.0.1:8043" + }, + "urn:x-rois:def:HRIComponent:Kyutech:main:SpeechRecognition": { + "module": "Speech_Recognition_client", + "class":"Speech_Recognition_Client", + "url":"http://127.0.0.1:8003" + }, + "urn:x-rois:def:HRIComponent:Kyutech:main:Navigation": { + "module": "Navigation_client", + "class":"Navigation_Client", + "url":"http://127.0.0.1:8001" + } +} \ No newline at end of file diff --git a/route_guidance_ros/scripts/client_info_gazebo_multi.json b/route_guidance_ros/scripts/client_info_gazebo_multi.json new file mode 100644 index 0000000000000000000000000000000000000000..3b093868e51be5edcdc03ac91e1d12c675d6e305 --- /dev/null +++ b/route_guidance_ros/scripts/client_info_gazebo_multi.json @@ -0,0 +1,77 @@ +{ + "urn:x-rois:def:HRIComponent:Kyutech:main:VirtualNavigation": { + "module": "VirtualNavigation_client", + "class":"VirtualNavigation_Client", + "url":"http://127.0.0.1:8041" + }, + "urn:x-rois:def:HRIComponent:Kyutech:main:PersonLocalization": { + "module": "Person_Localization_client", + "class":"Person_Localization_Client", + "url":"http://127.0.0.1:8043" + }, + + + "urn:x-rois:def:HRIEngine:Kyutech::robot1":{ + "module": "HRI_Engine_client_sample", + "class":"IF", + "url":"http://127.0.0.1:8010" + }, + "urn:x-rois:def:HRIComponent:Kyutech:robot1:Navigation": { + "module": "Navigation_client", + "class":"Navigation_Client", + "url":"http://127.0.0.1:8011" + }, + "urn:x-rois:def:HRIComponent:Kyutech:robot1:SystemInformation": { + "module": "System_Information_client", + "class":"System_Information_Client", + "url":"http://127.0.0.1:8012" + }, + "urn:x-rois:def:HRIComponent:Kyutech:robot1:SpeechRecognition": { + "module": "Speech_Recognition_client", + "class":"Speech_Recognition_Client", + "url":"http://127.0.0.1:8013" + }, + + + "urn:x-rois:def:HRIEngine:Kyutech::robot2":{ + "module": "HRI_Engine_client_sample", + "class":"IF", + "url":"http://127.0.0.1:8020" + }, + "urn:x-rois:def:HRIComponent:Kyutech:robot2:Navigation": { + "module": "Navigation_client", + "class":"Navigation_Client", + "url":"http://127.0.0.1:8021" + }, + "urn:x-rois:def:HRIComponent:Kyutech:robot2:SystemInformation": { + "module": "System_Information_client", + "class":"System_Information_Client", + "url":"http://127.0.0.1:8022" + }, + "urn:x-rois:def:HRIComponent:Kyutech:robot2:SpeechRecognition": { + "module": "Speech_Recognition_client", + "class":"Speech_Recognition_Client", + "url":"http://127.0.0.1:8023" + }, + + "urn:x-rois:def:HRIEngine:Kyutech::robot3":{ + "module": "HRI_Engine_client_sample", + "class":"IF", + "url":"http://127.0.0.1:8030" + }, + "urn:x-rois:def:HRIComponent:Kyutech:robot3:Navigation": { + "module": "Navigation_client", + "class":"Navigation_Client", + "url":"http://127.0.0.1:8031" + }, + "urn:x-rois:def:HRIComponent:Kyutech:robot3:SystemInformation": { + "module": "System_Information_client", + "class":"System_Information_Client", + "url":"http://127.0.0.1:8032" + }, + "urn:x-rois:def:HRIComponent:Kyutech:robot3:SpeechRecognition": { + "module": "Speech_Recognition_client", + "class":"Speech_Recognition_Client", + "url":"http://127.0.0.1:8033" + } +} \ No newline at end of file diff --git a/route_guidance_ros/scripts/engine_profile/engine_profile_gazebo.xml b/route_guidance_ros/scripts/engine_profile/engine_profile_gazebo.xml new file mode 100644 index 0000000000000000000000000000000000000000..99dd8844deb00d6825fb5d9005a1feb380bc3b04 --- /dev/null +++ b/route_guidance_ros/scripts/engine_profile/engine_profile_gazebo.xml @@ -0,0 +1,15 @@ + + +Robotic Interaction Service Framework Version 1.2 +<--> + + + urn:x-rois:def:HRIEngine:Kyutech::main + MainHRI + urn:x-rois:def:HRIComponent:Kyutech:main:Navigation + urn:x-rois:def:HRIComponent:Kyutech:main:SystemInformation + urn:x-rois:def:HRIComponent:Kyutech:main:SpeechRecognition + urn:x-rois:def:HRIComponent:Kyutech:main:PersonLocalization + \ No newline at end of file diff --git a/route_guidance_ros/scripts/engine_profile/engine_profile_gazebo_multi.xml b/route_guidance_ros/scripts/engine_profile/engine_profile_gazebo_multi.xml new file mode 100644 index 0000000000000000000000000000000000000000..944246c1fd604a794562d80d7fc983202de5943a --- /dev/null +++ b/route_guidance_ros/scripts/engine_profile/engine_profile_gazebo_multi.xml @@ -0,0 +1,34 @@ + + +Robotic Interaction Service Framework Version 1.2 +<--> + + + urn:x-rois:def:HRIEngine:Kyutech::main + main + urn:x-rois:def:HRIComponent:Kyutech:main:VirtualNavigation + urn:x-rois:def:HRIComponent:Kyutech:main:PersonLocalization + + urn:x-rois:def:HRIEngine:Kyutech::robot1 + robot1 + urn:x-rois:def:HRIComponent:Kyutech:robot1:Navigation + urn:x-rois:def:HRIComponent:Kyutech:robot1:SystemInformation + urn:x-rois:def:HRIComponent:Kyutech:robot1:SpeechRecognition + + + urn:x-rois:def:HRIEngine:Kyutech::robot2 + robot2 + urn:x-rois:def:HRIComponent:Kyutech:robot2:Navigation + urn:x-rois:def:HRIComponent:Kyutech:robot2:SystemInformation + urn:x-rois:def:HRIComponent:Kyutech:robot2:SpeechRecognition + + + urn:x-rois:def:HRIEngine:Kyutech::robot3 + robot3 + urn:x-rois:def:HRIComponent:Kyutech:robot3:Navigation + urn:x-rois:def:HRIComponent:Kyutech:robot3:SystemInformation + urn:x-rois:def:HRIComponent:Kyutech:robot3:SpeechRecognition + + \ No newline at end of file diff --git a/route_guidance_ros/scripts/pyRoIS_higashi b/route_guidance_ros/scripts/pyRoIS_higashi new file mode 160000 index 0000000000000000000000000000000000000000..5246a885b77b0159501d66e0b133942f2ac6cc1d --- /dev/null +++ b/route_guidance_ros/scripts/pyRoIS_higashi @@ -0,0 +1 @@ +Subproject commit 5246a885b77b0159501d66e0b133942f2ac6cc1d diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 51dc172ccdaff936e53545f1ebcce85b9aacc64a..f59a7377dcac0b1a9720df4ea12ec52f0b55e6ac 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -76,9 +76,9 @@ class Service(Service_Application_IF): pl_event_id = self.latest_event_id_table['person_localized'] status, results = self._proxy.get_event_detail(pl_event_id, "") print(colored("person localized successfully.", 'green')) - dest = results[2] + dest = results[3] print("Destination is {} ({})".format(dest, type(dest).__name__)) - (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") + (return_code, command_id) = self._proxy.set_parameter('urn:x-rois:def:HRIComponent:Kyutech:main:Navigation', [dest, "", ""]) status = RoIS_HRI.ReturnCode_t(return_code) if status == RoIS_HRI.ReturnCode_t.OUT_OF_RESOURCES: print(colored("There is no robot that is available.", 'red')) @@ -93,9 +93,9 @@ class Service(Service_Application_IF): sr_event_id = self.latest_event_id_table['speech_recognized'] status, results = self._proxy.get_event_detail(sr_event_id, "") print(colored("speech recognized successfully.", 'green')) - dest = results[1][0] + dest = results[2][0] print("Destination is {} ({})".format(dest, type(dest).__name__)) - (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") + (return_code, command_id) = self._proxy.set_parameter('urn:x-rois:def:HRIComponent:Kyutech:main:Navigation', [dest, "", ""]) status = RoIS_HRI.ReturnCode_t(return_code) if status == RoIS_HRI.ReturnCode_t.BAD_PARAMETER: print(colored("Bad destination detected.", 'yellow')) @@ -115,6 +115,17 @@ class Service(Service_Application_IF): def run(self): self._proxy = xmlrpc.client.ServerProxy(self._uri) time.sleep(3) + + self._proxy.connect() + self._proxy.get_profile("") + self._proxy.search("") + self._proxy.bind('urn:x-rois:def:HRIComponent:Kyutech:main:Navigation') + self._proxy.bind('urn:x-rois:def:HRIComponent:Kyutech:main:SystemInformation') + self._proxy.bind('urn:x-rois:def:HRIComponent:Kyutech:main:SpeechRecognition') + self._proxy.bind('urn:x-rois:def:HRIComponent:Kyutech:main:PersonLocalization') + + self._proxy.get_parameter('urn:x-rois:def:HRIComponent:Kyutech:main:SystemInformation') + self.Start() while True: time.sleep(5) diff --git a/route_guidance_ros/scripts/service_app_lv2_multi.py b/route_guidance_ros/scripts/service_app_lv2_multi.py new file mode 100644 index 0000000000000000000000000000000000000000..2e042bd7c75af0d0542dac1148970dd1bbbbd2a5 --- /dev/null +++ b/route_guidance_ros/scripts/service_app_lv2_multi.py @@ -0,0 +1,142 @@ +from pyrois.Service_Application_IF import Service_Application_IF +from pyrois import RoIS_Service, RoIS_HRI +from pyrois.RoIS_Common import Component_Status +from pyrois.RoIS_Service import Completed_Status + +import logging +import queue +import threading +import time +import enum +import xmlrpc +from termcolor import colored +from transitions import Machine +import sys + + +class Task(enum.Enum): + Init = 'Init' + GetUserPos = 'GetUserPos' + NaviToUser = 'NaviToUser' + ListenDest = 'ListenDest' + NaviToGuide = 'NaviToGuide' + End = 'End' + + +class Service(Service_Application_IF): + def __init__(self, uri="http://localhost:8000", logger=None): + super().__init__(uri) + # buid state machine + states = [Task.Init, Task.GetUserPos, Task.NaviToUser, Task.ListenDest, Task.NaviToGuide, Task.End] + transitions = [ + {'trigger': 'Start', 'source': Task.Init, 'dest': Task.GetUserPos, 'after': 'get_user_position' }, + {'trigger': 'PLSuccess', 'source': Task.GetUserPos, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, + {'trigger': 'NaviSuccess', 'source': Task.NaviToUser, 'dest': Task.ListenDest, 'after': 'listen_destination' }, + {'trigger': 'SRSuccess', 'source': Task.ListenDest, 'dest': Task.NaviToGuide, 'after': 'start_navi_to_guide'}, + {'trigger': 'InvalidDest', 'source': Task.NaviToGuide, 'dest': Task.ListenDest, 'after': 'listen_destination' }, + {'trigger': 'NaviFail', 'source': Task.NaviToUser, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, + {'trigger': 'NaviFail', 'source': Task.NaviToGuide, 'dest': Task.End, 'after': 'abort_service' }, + {'trigger': 'NaviSuccess', 'source': Task.NaviToGuide, 'dest': Task.End, 'after': 'complete_service' }, + {'trigger': 'NoRoboAvail', 'source': Task.NaviToUser, 'dest': Task.End, 'after': 'abort_service' }, + ] + self.machine = Machine(self, + states=states, + transitions=transitions, + initial=Task.Init, + ignore_invalid_triggers=True) + self.latest_event_id_table = {} + + def completed(self, command_id, completed_status): + completed_status = Completed_Status(completed_status) + if completed_status == Completed_Status.OK: + print(colored("robot reached goal successfully.", 'green')) + self.NaviSuccess() + elif completed_status == Completed_Status.ABORT: + print(colored("robot failed to reach goal.", 'red')) + self.NaviFail() + + def notify_error(self, error_id, error_type): + print('received error event {}({}) '.format( \ + error_id, RoIS_Service.ErrorType(error_type).name)) + + def notify_event(self, event_id, event_type, subscribe_id, expire): + if event_type == 'speech_recognized': + self.latest_event_id_table['speech_recognized'] = event_id + return_code = self._proxy.unsubscribe(subscribe_id) + self.SRSuccess() + if event_type == 'person_localized': + self.latest_event_id_table['person_localized'] = event_id + self.PLSuccess() + + def get_user_position(self): + return_code, subscribe_id = self._proxy.subscribe('person_localized', "") + print(colored("GetUserPos started.", 'cyan')) + + def start_navi_to_user(self): + pl_event_id = self.latest_event_id_table['person_localized'] + status, results = self._proxy.get_event_detail(pl_event_id, "") + print(colored("person localized successfully.", 'green')) + dest = results[3] + print("Destination is {} ({})".format(dest, type(dest).__name__)) + (return_code, command_id) = self._proxy.set_parameter('urn:x-rois:def:HRIComponent:Kyutech:main:Navigation', [dest, "", ""]) + status = RoIS_HRI.ReturnCode_t(return_code) + if status == RoIS_HRI.ReturnCode_t.OUT_OF_RESOURCES: + print(colored("There is no robot that is available.", 'red')) + self.NoRoboAvail() + print(colored("NaviToUser started.", 'cyan')) + + def listen_destination(self): + return_code, subscribe_id = self._proxy.subscribe('speech_recognized', "") + print(colored("ListenDest started.", 'cyan')) + + def start_navi_to_guide(self): + sr_event_id = self.latest_event_id_table['speech_recognized'] + status, results = self._proxy.get_event_detail(sr_event_id, "") + print(colored("speech recognized successfully.", 'green')) + dest = results[2][0] + print("Destination is {} ({})".format(dest, type(dest).__name__)) + (return_code, command_id) = self._proxy.set_parameter('urn:x-rois:def:HRIComponent:Kyutech:main:Navigation', [dest, "", ""]) + status = RoIS_HRI.ReturnCode_t(return_code) + if status == RoIS_HRI.ReturnCode_t.BAD_PARAMETER: + print(colored("Bad destination detected.", 'yellow')) + print(colored("Trying to ask again...", 'yellow')) + self.InvalidDest() + else: + print(colored("NaviToGuide started.", 'cyan')) + + def complete_service(self): + print(colored("Route Guidance Completed !", 'cyan')) + sys.exit(0) + + def abort_service(self): + print(colored("Route Guidance Aborted...", 'red')) + sys.exit(1) + + def run(self): + self._proxy = xmlrpc.client.ServerProxy(self._uri) + time.sleep(3) + + self._proxy.connect() + self._proxy.get_profile("") + self._proxy.search("") + self._proxy.bind('urn:x-rois:def:HRIComponent:Kyutech:main:VirtualNavigation') + self._proxy.bind('urn:x-rois:def:HRIComponent:Kyutech:main:PersonLocalization') + self._proxy.bind('urn:x-rois:def:HRIComponent:Kyutech:robot1:Navigation') + self._proxy.bind('urn:x-rois:def:HRIComponent:Kyutech:robot1:SystemInformation') + self._proxy.bind('urn:x-rois:def:HRIComponent:Kyutech:robot1:SpeechRecognition') + self._proxy.bind('urn:x-rois:def:HRIComponent:Kyutech:robot2:Navigation') + self._proxy.bind('urn:x-rois:def:HRIComponent:Kyutech:robot2:SystemInformation') + self._proxy.bind('urn:x-rois:def:HRIComponent:Kyutech:robot2:SpeechRecognition') + self._proxy.bind('urn:x-rois:def:HRIComponent:Kyutech:robot3:VirtualNavigation') + self._proxy.bind('urn:x-rois:def:HRIComponent:Kyutech:robot3:SystemInformation') + self._proxy.bind('urn:x-rois:def:HRIComponent:Kyutech:robot3:SpeechRecognition') + + self._proxy.get_parameter('urn:x-rois:def:HRIComponent:Kyutech:main:SystemInformation') + + self.Start() + + +if __name__ == '__main__': + service = Service() + print("Starting service..") + service.run() diff --git a/route_guidance_ros/scripts/service_app_with_eng_client.py b/route_guidance_ros/scripts/service_app_with_eng_client.py new file mode 100644 index 0000000000000000000000000000000000000000..841e879fc7bcf5e46e2be759746aff293937ed8b --- /dev/null +++ b/route_guidance_ros/scripts/service_app_with_eng_client.py @@ -0,0 +1,148 @@ +# from pyrois.Service_Application_IF import Service_Application_IF +from pyrois import RoIS_Service, RoIS_HRI +from pyrois.RoIS_Common import Component_Status +from pyrois.RoIS_Service import Completed_Status +import HRI_Engine_client_sample + +import logging +import queue +import threading +import time +import enum +# import xmlrpc +from termcolor import colored +from transitions import Machine +import sys + + +class Task(enum.Enum): + Init = 'Init' + GetUserPos = 'GetUserPos' + NaviToUser = 'NaviToUser' + ListenDest = 'ListenDest' + NaviToGuide = 'NaviToGuide' + End = 'End' + + +class Service(HRI_Engine_client_sample.IF): + def __init__(self, uri="http://127.0.0.1:8000", logger=None): + HRI_Engine_client_sample.IF.__init__(self, uri, logger=None, event_queue=None) + # buid state machine + states = [Task.Init, Task.GetUserPos, Task.NaviToUser, Task.ListenDest, Task.NaviToGuide, Task.End] + transitions = [ + {'trigger': 'Start', 'source': Task.Init, 'dest': Task.GetUserPos, 'after': 'get_user_position' }, + {'trigger': 'PLSuccess', 'source': Task.GetUserPos, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, + {'trigger': 'NaviSuccess', 'source': Task.NaviToUser, 'dest': Task.ListenDest, 'after': 'listen_destination' }, + {'trigger': 'SRSuccess', 'source': Task.ListenDest, 'dest': Task.NaviToGuide, 'after': 'start_navi_to_guide'}, + {'trigger': 'InvalidDest', 'source': Task.NaviToGuide, 'dest': Task.ListenDest, 'after': 'listen_destination' }, + {'trigger': 'NaviFail', 'source': Task.NaviToUser, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, + {'trigger': 'NaviFail', 'source': Task.NaviToGuide, 'dest': Task.End, 'after': 'abort_service' }, + {'trigger': 'NaviSuccess', 'source': Task.NaviToGuide, 'dest': Task.End, 'after': 'complete_service' }, + {'trigger': 'NoRoboAvail', 'source': Task.NaviToUser, 'dest': Task.End, 'after': 'abort_service' }, + ] + self.machine = Machine(self, + states=states, + transitions=transitions, + initial=Task.Init, + ignore_invalid_triggers=True) + self.latest_event_id_table = {} + self.logger.info("service_app_with_eng_client.Service(): complete init") + + def completed(self, command_id, completed_status): + completed_status = Completed_Status(completed_status) + if completed_status == Completed_Status.OK: + print(colored("robot reached goal successfully.", 'green')) + self.NaviSuccess() + elif completed_status == Completed_Status.ABORT: + print(colored("robot failed to reach goal.", 'red')) + self.NaviFail() + + def notify_error(self, error_id, error_type): + print('received error event {}({}) '.format( \ + error_id, RoIS_Service.ErrorType(error_type).name)) + + def notify_event(self, event_id, event_type, subscribe_id, expire): + if event_type == 'speech_recognized': + self.latest_event_id_table['speech_recognized'] = event_id + return_code = self.unsubscribe(subscribe_id) + self.SRSuccess() + if event_type == 'person_localized': + self.latest_event_id_table['person_localized'] = event_id + self.PLSuccess() + + def get_user_position(self): + return_code, subscribe_id = self.subscribe('person_localized', "") + print(colored("GetUserPos started.", 'cyan')) + + def start_navi_to_user(self): + pl_event_id = self.latest_event_id_table['person_localized'] + status, results = self.get_event_detail(pl_event_id, "") + print(colored("person localized successfully.", 'green')) + dest = results[3] + print("Destination is {} ({})".format(dest, type(dest).__name__)) + (return_code, command_id) = self.set_parameter('urn:x-rois:def:HRIComponent:Kyutech:main:Navigation', [dest, "", ""]) + status = RoIS_HRI.ReturnCode_t(return_code) + if status == RoIS_HRI.ReturnCode_t.OUT_OF_RESOURCES: + print(colored("There is no robot that is available.", 'red')) + self.NoRoboAvail() + print(colored("NaviToUser started.", 'cyan')) + + def listen_destination(self): + return_code, subscribe_id = self.subscribe('speech_recognized', "") + print(colored("ListenDest started.", 'cyan')) + + def start_navi_to_guide(self): + sr_event_id = self.latest_event_id_table['speech_recognized'] + status, results = self.get_event_detail(sr_event_id, "") + print(colored("speech recognized successfully.", 'green')) + dest = results[2][0] + print("Destination is {} ({})".format(dest, type(dest).__name__)) + (return_code, command_id) = self.set_parameter('urn:x-rois:def:HRIComponent:Kyutech:main:Navigation', [dest, "", ""]) + status = RoIS_HRI.ReturnCode_t(return_code) + if status == RoIS_HRI.ReturnCode_t.BAD_PARAMETER: + print(colored("Bad destination detected.", 'yellow')) + print(colored("Trying to ask again...", 'yellow')) + self.InvalidDest() + else: + print(colored("NaviToGuide started.", 'cyan')) + + def complete_service(self): + print(colored("Route Guidance Completed !", 'cyan')) + sys.exit(0) + + def abort_service(self): + print(colored("Route Guidance Aborted...", 'red')) + sys.exit(1) + + def start_service(self): + self.connect() + + self.get_profile("") + self.search("") + self.bind('urn:x-rois:def:HRIComponent:Kyutech:main:Navigation') + self.bind('urn:x-rois:def:HRIComponent:Kyutech:main:SystemInformation') + self.bind('urn:x-rois:def:HRIComponent:Kyutech:main:SpeechRecognition') + self.bind('urn:x-rois:def:HRIComponent:Kyutech:main:PersonLocalization') + + self.get_parameter('urn:x-rois:def:HRIComponent:Kyutech:main:SystemInformation') + + self.Start() + print(self.state) + # self.PLSuccess() + # print(self.state) + + +if __name__ == '__main__': + service = Service() + print("Starting service..") + service.start_service() + + + # print(Service.mro()) + """ + [, , , + , , , + , , , + , , , + ] + """ \ No newline at end of file diff --git a/route_guidance_ros/scripts/service_app_with_eng_client_easy.py b/route_guidance_ros/scripts/service_app_with_eng_client_easy.py new file mode 100644 index 0000000000000000000000000000000000000000..d780f9f9579713debd77ccd8c124a2ff922628eb --- /dev/null +++ b/route_guidance_ros/scripts/service_app_with_eng_client_easy.py @@ -0,0 +1,60 @@ +from pyrois import RoIS_Service, RoIS_HRI +from pyrois.RoIS_Common import Component_Status +from pyrois.RoIS_Service import Completed_Status +from pyRoIS_higashi import HRI_Engine_client_sample, Service_Application_IF_sample, HRI_Engine_sample + +import logging +import queue +import threading +import time +import enum +import xmlrpc +from termcolor import colored +from transitions import Machine +import sys + + +class Task(enum.Enum): + Init = 'Init' + GetUserPos = 'GetUserPos' + NaviToUser = 'NaviToUser' + ListenDest = 'ListenDest' + NaviToGuide = 'NaviToGuide' + End = 'End' + + +class test1(HRI_Engine_client_sample.IF): #vscode上ではクラス名が白文字のまま HRI_Engine_sampleも同様 多段階継承になるかどうか？ + def __init__(self, uri="http://127.0.0.1:8000", logger=None): + HRI_Engine_client_sample.IF.__init__(self, uri=uri, logger=None, event_queue=None) + # buid state machine + +class test2(Service_Application_IF_sample.Service_Application_IF): #緑字になる + def __init__(self, uri="http://127.0.0.1:8000", logger=None): + Service_Application_IF_sample.Service_Application_IF.__init__(self, uri=uri, logger=None, event_queue=None) + # buid state machine + + +class Service: #継承しなければうまくいく + def __init__(self, uri="http://127.0.0.1:8000", logger=None): + self.test = HRI_Engine_client_sample.IF(uri=uri, logger=None, event_queue=None) + # buid state machine + + def run(self): + self.test.connect() + self.test.get_profile("") + # self.search("") + # self.bind('urn:x-rois:def:HRIComponent:Kyutech:main:Navigation') + # self.bind('urn:x-rois:def:HRIComponent:Kyutech:main:SystemInformation') + # self.bind('urn:x-rois:def:HRIComponent:Kyutech:main:SpeechRecognition') + # self.bind('urn:x-rois:def:HRIComponent:Kyutech:main:PersonLocalization') + + # self.get_parameter('urn:x-rois:def:HRIComponent:Kyutech:main:SystemInformation') + + + time.sleep(10) + + +if __name__ == '__main__': + service = Service() + print("Starting service..") + service.run() diff --git a/route_guidance_ros/scripts/setup_comp_servers.py b/route_guidance_ros/scripts/setup_comp_servers.py new file mode 100644 index 0000000000000000000000000000000000000000..4db0b39a68119e438519e15e0cced6066baa1d45 --- /dev/null +++ b/route_guidance_ros/scripts/setup_comp_servers.py @@ -0,0 +1,118 @@ +import time +import subprocess +from subprocess import Popen +from multiprocessing import Process, Queue +import atexit +import sys + +def register_process_killer(process): + def kill_subprocess(pid): + subprocess.run(["kill", "-SIGKILL", str(pid)]) + print("process {} was killed.".format(pid)) + atexit.register(kill_subprocess, process.pid) + +class SubprocessWrapper(object): + def __init__(self, command): + self.process = Popen(command) + register_process_killer(self.process) + + # It is not guaranteed that __del__() methods are called when the interpreter exits. + # def __del__(self): + # self.process.kill() + + +class ComponentWrapper(SubprocessWrapper): + def __init__(self, component_name, robot_name, port): + super().__init__(["rosrun", "route_guidance_ros", component_name, str(port), robot_name]) + + +class NavigationComponent(ComponentWrapper): + def __init__(self, robot_name, port): + super().__init__("Navigation.py", robot_name, port) + + +class SysInfoComponent(ComponentWrapper): + def __init__(self, robot_name, port): + super().__init__("System_Information.py", robot_name, port) + + +class SpeechRecogComponent(ComponentWrapper): + def __init__(self, robot_name, port): + super().__init__("Dummy_Speech_Recognition.py", robot_name, port) + + +# class SubEngineWrapper(SubprocessWrapper): +# def __init__(self, engine_port): +# super().__init__(["python3", "sub_engine.py", str(engine_port)]) + + +# class MainEngineWrapper(SubprocessWrapper): +# def __init__(self): +# super().__init__(["python3", "main_engine.py"]) + + +def launch_components_and_subengines(**robot_port_table): + process = [] + for robot_name, engine_port in robot_port_table.items(): + process.append(NavigationComponent(robot_name, engine_port+1)) + process.append(SysInfoComponent(robot_name, engine_port+2)) + process.append(SpeechRecogComponent(robot_name, engine_port+3)) + print("Components Constructed.") + + command = [ + "rosrun", "route_guidance_ros", "Person_Localization.py", + "8043", "human1" + ] + process.append(SubprocessWrapper(command)) + print("Person Localization Component Constructed.") + time.sleep(10) + + # for engine_port in robot_port_table.values(): + # process.append(SubEngineWrapper(engine_port)) + # print("Sub Engine Constructed.") + # from pyRoIS_higashi import HRI_Engine_sample + # process.append(Process(target=HRI_Engine_sample.test_engine, args=(8000,"engine_profile_gazebo", "client_info_gazebo", None, None,))) + # time.sleep(5) + + return process + + +def setup_single_robot(): + return launch_components_and_subengines(robot1=8000) + + +def setup_multi_robot(): + process = launch_components_and_subengines(robot1=8010, robot2=8020, robot3=8030) + command = [ + "rosrun", "route_guidance_ros", "VirtualNavigation.py", + "8041", "robot1", "robot2", "robot3" + ] + process.append(SubprocessWrapper(command)) + print("Virtual Navigation Component Constructed.") + time.sleep(5) + # process.append(MainEngineWrapper()) + # print("Main Engine Constructed.") + # time.sleep(10) + # from pyRoIS_higashi import HRI_Engine_sample + # process.append(Process(target=HRI_Engine_sample.test_engine, args=(8000,"engine_profile_gazebo_multi", "client_info_gazebo_multi", None, None,))) + + + return process + + +if __name__ == "__main__": + args = sys.argv + if len(args) == 2: + print(args) + if args[1] == "single": + setup_single_robot() + elif args[1] == "multi": + setup_multi_robot() + else: + print("The argument is wrong. Please choose [single] or [multi].") + sys.exit() + else: + print("Please choose [single] or [multi] on command line.") + sys.exit() + +# python3 setup_comp_servers.py single \ No newline at end of file diff --git a/thesis_tanaka_2019.pdf b/thesis_tanaka_2019.pdf new file mode 100644 index 0000000000000000000000000000000000000000..5ed03467b20476ce1624a806620411cb32a59ecb Binary files /dev/null and b/thesis_tanaka_2019.pdf differ