Merge pull request #417 from toppers/develop

v2.8.0向けのコードの実験的採用
toppers · Nov 15, 2024 · 5bfff09 · 5bfff09
2 parents 39cdbf0 + 171a7bd
commit 5bfff09
Show file tree

Hide file tree

Showing 67 changed files with 1,741 additions and 1,752 deletions.
diff --git a/docs/manual/ubuntu22.04_install&operation_pythonAPI編.md b/docs/manual/ubuntu22.04_install&operation_pythonAPI編.md
@@ -311,9 +311,17 @@ $ python3 sample.py ../../../hakoniwa-unity-drone-model/DroneAppLinux/custom.jso
 
 Unityアプリ上のドローンをPS4のコントローラで操作させるためのプロポ用のアプリを起動します。PC本体にPS4のコントローラをUSBに接続します。
 
-1.4.1 箱庭コア機能の起動、1.4.2 Unityアプリの起動手順を実施後に、以下のプロポ用のアプリを起動します。
+箱庭コア機能、Unityアプリの起動手順、プロポ用のアプリの順番で起動します。
 
+- 箱庭コア機能の起動(プロポ用のアプリを起動の場合)
+``` bash
+$ cd ~/work/hakoniwa-px4sim/hakoniwa
+$ bash drone-app.bash ../../hakoniwa-unity-drone-model/DroneAppLinux ./config/rc
+```
+
+- 1.4.2 Unityアプリの起動手順
 
+- プロポ用のアプリを起動
 ```bash
 $ cd ~/work/hakoniwa-px4sim
 $ cd drone_api/sample

diff --git a/drone_api/assets/config/area.json b/drone_api/assets/config/area.json
@@ -3,8 +3,22 @@
       {
         "area_id": "area_1",
         "bounds": {
-          "min": { "x": 10, "y": 10, "z": 0 },
-          "max": { "x": 20, "y": 20, "z": 10 }
+          "min": { "x": 1, "y": -5, "z": 0 },
+          "max": { "x": 5, "y":  5, "z": 20 }
+        }
+      },
+      {
+        "area_id": "area_2",
+        "bounds": {
+          "min": { "x": 1, "y": -10, "z": 0 },
+          "max": { "x": 5, "y":  -5, "z": 20 }
+        }
+      },
+      {
+        "area_id": "area_3",
+        "bounds": {
+          "min": { "x": 1, "y":  5, "z": 0 },
+          "max": { "x": 5, "y": 10, "z": 20 }
         }
       }
     ]

diff --git a/drone_api/assets/config/area_property.json b/drone_api/assets/config/area_property.json
@@ -3,22 +3,22 @@
       {
         "area_id": "area_1",
         "properties": {
-          "wind_velocity": [1.0, 1.0, 0.0],
+          "wind_velocity": [0.0, 20.0, 0.0],
           "temperature": 22.5
         }
       },
       {
         "area_id": "area_2",
         "properties": {
           "wind_velocity": [2.5, 1.0, 0.0],
-          "temperature": 19.0
+          "temperature": -15.0
         }
       },
       {
         "area_id": "area_3",
         "properties": {
           "wind_velocity": [0.0, 0.0, 0.0],
-          "temperature": 25.0
+          "temperature": 55.0
         }
       },
       {

diff --git a/drone_api/assets/hako_env_event.py b/drone_api/assets/hako_env_event.py
@@ -51,6 +51,10 @@ def on_manual_timing_control(context):
                 pdu_data['d_wind']['value']['x'] = wind[0]
                 pdu_data['d_wind']['value']['y'] = wind[1]
                 pdu_data['d_wind']['value']['z'] = wind[2]
+            temperature = property_info.get_temperature()
+            if temperature is not None:
+                print(f"{hakopy.simulation_time()} temperature ==> {property_info.get_temperature()}")
+                pdu_data['d_temp']['value'] = temperature
         else:
             print(f"{hakopy.simulation_time()}: No wind")
 

diff --git a/drone_api/libs/hakosim.py b/drone_api/libs/hakosim.py
@@ -20,19 +20,25 @@ def __init__(self, name):
         self.camera_move_cmd_request_id = 1
 
 class MultirotorClient:
-    def __init__(self, config_path):
+    def __init__(self, config_path, default_drone_name = None):
         self.pdu_manager = None
         self.config_path = config_path
         self.pdudef = self._load_json(config_path)
         self.vehicles = {}
         default_drone_set = False
-        for entry in self.pdudef['robots']:
-            entry_name = entry['name']
-            if len(entry['shm_pdu_readers']) > 0 or len(entry['shm_pdu_writers']) > 0 or len(entry['rpc_pdu_readers']) > 0 or len(entry['rpc_pdu_writers']) > 0:
-                self.vehicles[entry_name] = HakoDrone(entry_name)
-                if not default_drone_set:
-                    self.default_drone_name = entry_name
-                    default_drone_set = True
+        if default_drone_name is None:
+            for entry in self.pdudef['robots']:
+                entry_name = entry['name']
+                if len(entry['shm_pdu_readers']) > 0 or len(entry['shm_pdu_writers']) > 0 or len(entry['rpc_pdu_readers']) > 0 or len(entry['rpc_pdu_writers']) > 0:
+                    self.vehicles[entry_name] = HakoDrone(entry_name)
+                    if not default_drone_set:
+                        self.default_drone_name = entry_name
+                        default_drone_set = True
+            print("default drone name: ", default_drone_name)
+        else:
+            print("default drone name: ", default_drone_name)
+            self.default_drone_name = default_drone_name
+            self.vehicles[default_drone_name] = HakoDrone(default_drone_name)
 
     def _load_json(self, path):
         try:

diff --git a/drone_api/libs/pdu_info.py b/drone_api/libs/pdu_info.py
@@ -7,11 +7,12 @@
 HAKO_AVATOR_CHANNEL_ID_CMD_MOVE    = 6
 HAKO_AVATOR_CHANNEL_ID_CMD_LAND    = 7
 HAKO_AVATOR_CHANNEL_ID_GAME_CTRL   = 8
-HAKO_AVATOR_CHANNEL_ID_CMD_CAMERA  = 9
-HAKO_AVATOR_CHANNEL_ID_CAMERA_DATA = 10
-HAKO_AVATOR_CHANNEL_ID_CMD_CAMERA_MOVE  = 11
-HAKO_AVATOR_CHANNEL_ID_CAMERA_INFO = 12
-HAKO_AVATOR_CHANNEL_ID_CMD_MAG     = 13
-HAKO_AVATOR_CHANNEL_ID_STAT_MAG    = 14
-HAKO_AVATOR_CHANNEL_ID_LIDAR_DATA  = 15
-HAKO_AVATOR_CHANNEL_ID_LIDAR_POS   = 16
+HAKO_AVATOR_CHANNEL_ID_BATTERY_STAT= 0
+HAKO_AVATOR_CHANNEL_ID_CMD_CAMERA  = 10
+HAKO_AVATOR_CHANNEL_ID_CAMERA_DATA = 11
+HAKO_AVATOR_CHANNEL_ID_CMD_CAMERA_MOVE  = 12
+HAKO_AVATOR_CHANNEL_ID_CAMERA_INFO = 13
+HAKO_AVATOR_CHANNEL_ID_CMD_MAG     = 14
+HAKO_AVATOR_CHANNEL_ID_STAT_MAG    = 15
+HAKO_AVATOR_CHANNEL_ID_LIDAR_DATA  = 16
+HAKO_AVATOR_CHANNEL_ID_LIDAR_POS   = 17
diff --git a/drone_api/sample/camera.py b/drone_api/sample/camera.py
@@ -41,7 +41,7 @@ def main():
         print(f"Usage: {sys.argv[0]} <config_path>")
         return 1
 
-    client = hakosim.MultirotorClient(sys.argv[1])
+    client = hakosim.MultirotorClient(sys.argv[1], "DroneTransporter")
     client.confirmConnection()
     client.enableApiControl(True)
     client.armDisarm(True)

diff --git a/drone_api/sample/custom.json b/drone_api/sample/custom.json
@@ -1,5 +1,43 @@
 {
   "robots": [
+    {
+      "name": "Baggage1",
+      "rpc_pdu_readers": [],
+      "rpc_pdu_writers": [],
+      "shm_pdu_readers": [],
+      "shm_pdu_writers": [
+        {
+          "type": "geometry_msgs/Twist",
+          "org_name": "pos",
+          "name": "Baggage1_pos",
+          "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
+          "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
+          "channel_id": 0,
+          "pdu_size": 112,
+          "write_cycle": 1,
+          "method_type": "SHM"
+        }
+      ]
+    },
+    {
+      "name": "Baggage2",
+      "rpc_pdu_readers": [],
+      "rpc_pdu_writers": [],
+      "shm_pdu_readers": [],
+      "shm_pdu_writers": [
+        {
+          "type": "geometry_msgs/Twist",
+          "org_name": "pos",
+          "name": "Baggage2_pos",
+          "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
+          "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
+          "channel_id": 0,
+          "pdu_size": 112,
+          "write_cycle": 1,
+          "method_type": "SHM"
+        }
+      ]
+    },
     {
       "name": "DroneTransporter",
       "rpc_pdu_readers": [],
@@ -12,7 +50,7 @@
           "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReader",
           "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReaderConverter",
           "channel_id": 0,
-          "pdu_size": 88,
+          "pdu_size": 112,
           "write_cycle": 1,
           "method_type": "SHM"
         },
@@ -23,7 +61,7 @@
           "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReader",
           "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReaderConverter",
           "channel_id": 1,
-          "pdu_size": 48,
+          "pdu_size": 72,
           "write_cycle": 1,
           "method_type": "SHM"
         },
@@ -34,7 +72,18 @@
           "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
           "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
           "channel_id": 3,
-          "pdu_size": 56,
+          "pdu_size": 80,
+          "write_cycle": 1,
+          "method_type": "SHM"
+        },
+        {
+          "type": "hako_msgs/Disturbance",
+          "org_name": "drone_disturbance",
+          "name": "DroneTransporter_drone_disturbance",
+          "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReader",
+          "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReaderConverter",
+          "channel_id": 4,
+          "pdu_size": 88,
           "write_cycle": 1,
           "method_type": "SHM"
         },
@@ -45,7 +94,7 @@
           "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
           "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
           "channel_id": 5,
-          "pdu_size": 32,
+          "pdu_size": 64,
           "write_cycle": 1,
           "method_type": "SHM"
         },
@@ -56,7 +105,7 @@
           "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
           "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
           "channel_id": 6,
-          "pdu_size": 48,
+          "pdu_size": 80,
           "write_cycle": 1,
           "method_type": "SHM"
         },
@@ -67,18 +116,29 @@
           "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
           "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
           "channel_id": 7,
-          "pdu_size": 32,
+          "pdu_size": 64,
           "write_cycle": 1,
           "method_type": "SHM"
         },
         {
-          "type": "hako_msgs/HakoCmdMagnetHolder",
-          "org_name": "hako_cmd_magnet_holder",
-          "name": "DroneTransporter_hako_cmd_magnet_holder",
+          "type": "hako_msgs/GameControllerOperation",
+          "org_name": "hako_cmd_game",
+          "name": "DroneTransporter_hako_cmd_game",
           "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
           "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
           "channel_id": 8,
-          "pdu_size": 16,
+          "pdu_size": 136,
+          "write_cycle": 1,
+          "method_type": "SHM"
+        },
+        {
+          "type": "hako_msgs/HakoBatteryStatus",
+          "org_name": "hako_battery",
+          "name": "DroneTransporter_hako_battery",
+          "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReader",
+          "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReaderConverter",
+          "channel_id": 9,
+          "pdu_size": 56,
           "write_cycle": 1,
           "method_type": "SHM"
         },
@@ -89,7 +149,29 @@
           "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
           "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
           "channel_id": 10,
-          "pdu_size": 20,
+          "pdu_size": 44,
+          "write_cycle": 1,
+          "method_type": "SHM"
+        },
+        {
+          "type": "hako_msgs/HakoCmdCameraMove",
+          "org_name": "hako_cmd_camera_move",
+          "name": "DroneTransporter_hako_cmd_camera_move",
+          "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
+          "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
+          "channel_id": 12,
+          "pdu_size": 64,
+          "write_cycle": 1,
+          "method_type": "SHM"
+        },
+        {
+          "type": "hako_msgs/HakoCmdMagnetHolder",
+          "org_name": "hako_cmd_magnet_holder",
+          "name": "DroneTransporter_hako_cmd_magnet_holder",
+          "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
+          "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
+          "channel_id": 14,
+          "pdu_size": 40,
           "write_cycle": 1,
           "method_type": "SHM"
         }
@@ -102,18 +184,29 @@
           "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
           "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
           "channel_id": 2,
-          "pdu_size": 280,
+          "pdu_size": 304,
           "write_cycle": 1,
           "method_type": "SHM"
         },
         {
-          "type": "hako_msgs/Disturbance",
-          "org_name": "drone_disturbance",
-          "name": "DroneTransporter_drone_disturbance",
+          "type": "hako_msgs/HakoCameraData",
+          "org_name": "hako_camera_data",
+          "name": "DroneTransporter_hako_camera_data",
           "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
           "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
-          "channel_id": 4,
-          "pdu_size": 8,
+          "channel_id": 11,
+          "pdu_size": 102968,
+          "write_cycle": 1,
+          "method_type": "SHM"
+        },
+        {
+          "type": "hako_msgs/HakoCameraInfo",
+          "org_name": "hako_cmd_camera_info",
+          "name": "DroneTransporter_hako_cmd_camera_info",
+          "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
+          "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
+          "channel_id": 13,
+          "pdu_size": 56,
           "write_cycle": 1,
           "method_type": "SHM"
         },
@@ -123,20 +216,31 @@
           "name": "DroneTransporter_hako_status_magnet_holder",
           "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
           "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
-          "channel_id": 9,
-          "pdu_size": 8,
+          "channel_id": 15,
+          "pdu_size": 32,
           "write_cycle": 1,
           "method_type": "SHM"
         },
         {
-          "type": "hako_msgs/HakoCameraData",
-          "org_name": "hako_camera_data",
-          "name": "DroneTransporter_hako_camera_data",
+          "type": "sensor_msgs/PointCloud2",
+          "org_name": "lidar_points",
+          "name": "DroneTransporter_lidar_points",
           "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
           "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
-          "channel_id": 11,
-          "pdu_size": 102668,
-          "write_cycle": 1,
+          "channel_id": 16,
+          "pdu_size": 177400,
+          "write_cycle": 5,
+          "method_type": "SHM"
+        },
+        {
+          "type": "geometry_msgs/Twist",
+          "org_name": "lidar_pos",
+          "name": "DroneTransporter_lidar_pos",
+          "class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
+          "conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
+          "channel_id": 17,
+          "pdu_size": 72,
+          "write_cycle": 5,
           "method_type": "SHM"
         }
       ]