2023年度ゼミ

[k-okada]

@keitayoneda ロボットの認識・行動判断に興味がある。Spotを使って細いところを通り抜けるのを見てみたい。 @yoshiki-447 理学療法。介護ロボット。人を持ち上げたり、あるいは人との会話。アドバイスや寄り添い。 @KIMJUNMO1106 月で建築作業をするロボット。人間のような機構、パワーを出す、筋電を使う。 @nockoy ロボットかっこいいな。かわいいな。

https://github.com/jsk-ros-pkg/jsk_demos/tree/jsk_2023_10_semi

[keitayoneda]

@sktometometo throttled_rateを2.0等の大きい値にすると画像のようにrosbag_record.launchを立ち上げたときに process has diedとなってしまうのですがどうすればいいでしょうか？

[keitayoneda]

throttled_rate=1でも起きてました。発生条件がthrottled_rateというわけではなさそうです

[sktometometo]

@keitayoneda

https://qiita.com/Shunmo17/items/3382e9b9dffb93806cba みたいにreindexしてみるとどうかな

[keitayoneda]

原因はrosbagの引数にrosbag=~/{path of bagfile}と書いていたのが原因でした、しょうもないミスで申し訳ないです ~を$HOMEにしたところちゃんと動きました

[sktometometo]

どんなバグ報告も誰かのためになりうるという意味で、githubのissueに上げるのにしょうもないものは無いのでどんどん書きましょう

[keitayoneda]

あと今後の方針について少し相談したいです。 現状の目標はspotの人追従をなめらかにしたいというもので、問題点は

• 遠ざかると認識できなくなってしまう
• 周囲に複数の人がいるときに目標の人からトラッキングが外れてしまう

というものでした。 昨日はdepth等の情報を使ってカメラで認識できない情報を取り込めたらいいということでspotのdepthから色付き点群を出せるようにしていただいたのですが、今日見た感想だと、人以外にも床や壁、その他置かれているものなどいわゆる不要な点群も含まれていてそれらから人を区別するのはかなり難易度が高いのではないかと感じました。 そこで点群ではなく、カメラ画像のみから人の位置推定を行う方針に切り替えようと思うのですがどうでしょうか？今のcoral_usbに入っているモデルを改良するか、新しいモデルに差し替えることで認識できる距離やトラッキング精度をあげることに取り組むほうがいいのではないかと考えています

[sktometometo]

解像度と画角の話をのぞけば、パノラマとボディカメラの違いはRGBの3チャンネルかグレースケール+距離画像の2チャンネルみたいな感じのはずで、人の部分を抽出する問題そのものが本質的に変わる気はちょっとしないけど、画角と解像度とサンプリングレート的には有利だと思うのでその面で使うのはありだと思います。

ただトラッキングの性能突き詰めるとそれこそ自動運転の人たちが使ってるであろうLidarベースのトラッキングアルゴリズム持ってきてファインチューンするのと質的に変わらない話だし、ただトラッキングするじゃなくて人との伴走とか人と一緒にずっと活動するロボットならではのトラッキングの観点（運動とかの傾向の認識とか意図の推定とか？興味次第でいろいろありそう）を組み合わせて何かできるかみたいな話を取り込んでも面白いかなと思います。

[keitayoneda]

まさにサンプリングレートと解像度のことを考えると画像のほうが時間あたりに得られる情報量が多く、扱いやすいのかなと思いました。 確かにトラッキングについてはいろいろ方向性がありそうですよね... もう少し考えてみます。

[sktometometo]

研究室でロボットをなるべくすぐ動かせるように整備してるのはちょっとした実験がすぐにできるようにするためなので、どんなことでもちょっと試したくなったら是非ロボット動かしてみてください。

実際にロボット動かしながらじゃないとわからないこととかアイディアもあると思うので。

[k-okada]

取ったbagファイルやそこから作成した動画を張り付けることが出来るかな． ー　近いところから遠いところまで追いかけたい人がいたときに，それぞれのカメラでどれぐらい見えているか． が確認できると良いと思います．また，各カメラでオリジナルでどれぐらいの解像度でrosbagに入っているのはどれぐらいの解像度，というのは確認しましょう． あと，https://support.bostondynamics.com/s/article/Moving-object-detection の機能を確認したくて，今の7階にあるSpotでLidarは治ったかな，それで見えるのか，8階のSpotのセットアップでないと有効になっていないのか．いずれもこれで，どこまで遠くに行って見つかるか，人込みののなかを歩くとどうなるか？を見てみたいです．

[keitayoneda]

@sktometometo 1) yoneda@StrelKa を作成、~/catkin_ws で spot-ros と jsk_robot をおいてコンパイル 2) ~/spot_driver_bringup.launch を作戦し、これを立ち上げて最低機能が動くことを確認 3) spot_driver/src/spot_driver の spot_wrapper.py / sport_ros.py あたりを見れば良いこと理解した

目標1) spot_driver でget_world_objects.py と同じような結果がprintされるようにする 目標2) 上記結果を（おそらく）TFでpublish できるようにする

[keitayoneda]

起動するとこんな感じでtf2のエラーが出るのですが、どうすればいでしょうか？

[keitayoneda]

printするところまではできました 下がdiffです

diff --git a/spot_driver/src/spot_driver/spot_ros.py b/spot_driver/src/spot_driver/spot_ros.py
index d097858..4ca2fd8 100644
--- a/spot_driver/src/spot_driver/spot_ros.py
+++ b/spot_driver/src/spot_driver/spot_ros.py
@@ -64,6 +64,7 @@ class SpotROS():
         self.callbacks["rear_image"] = self.RearImageCB
         self.callbacks["gripper_image"] = self.GripperImageCB
         self.callbacks["lidar_points"] = self.LidarPointCloudCB
+        self.callbacks["world_object"] = self.WorldObjectCB

     def RobotStateCB(self, results):
         """Callback for when the Spot Wrapper gets new robot state data.
@@ -127,6 +128,9 @@ class SpotROS():
             behavior_fault_state_msg = getBehaviorFaultsFromState(state, self.spot_wrapper)
             self.behavior_faults_pub.publish(behavior_fault_state_msg)

+
+
+
     def MetricsCB(self, results):
         """Callback for when the Spot Wrapper gets new metrics data.

@@ -280,6 +284,17 @@ class SpotROS():
             point_cloud_msg = GetLidarPointCloudMsg(data[0], self.spot_wrapper)
             self.lidar_point_cloud_pub.publish(point_cloud_msg)
             self.populate_lidar_static_transforms(data[0])
+            
+    def WorldObjectCB(self, results):
+        """Callback for when the Spot Wrapper gets new world_object data.
+
+        Args:
+            results: FutureWrapper object of AsyncPeriodicQuery callback
+        """
+        del results
+        data = self.spot_wrapper.world_object
+        print(data)
+        

     def handle_claim(self, req):
         """ROS service handler for the claim service"""
@@ -701,6 +716,7 @@ class SpotROS():
         rate = rospy.Rate(50)

         self.rates = rospy.get_param('~rates', {})
+        self.rates["world_object"] = 1.0
         self.username = rospy.get_param('~username', 'default_value')
         self.password = rospy.get_param('~password', 'default_value')
         self.hostname = rospy.get_param('~hostname', 'default_value')
diff --git a/spot_driver/src/spot_driver/spot_wrapper.py b/spot_driver/src/spot_driver/spot_wrapper.py
index 84e8a3f..091d064 100644
--- a/spot_driver/src/spot_driver/spot_wrapper.py
+++ b/spot_driver/src/spot_driver/spot_wrapper.py
@@ -9,6 +9,7 @@ from bosdyn.geometry import EulerZXY
 from bosdyn import geometry

 from bosdyn.client.robot_state import RobotStateClient
+from bosdyn.client.world_object import WorldObjectClient
 from bosdyn.client.robot_command import RobotCommandClient, RobotCommandBuilder
 from bosdyn.client.graph_nav import GraphNavClient
 from bosdyn.client.recording import GraphNavRecordingServiceClient
@@ -98,6 +99,28 @@ class AsyncRobotState(AsyncPeriodicQuery):
             callback_future.add_done_callback(self._callback)
             return callback_future

+class AsyncWorldObject(AsyncPeriodicQuery):
+    """Class to get world object at regular intervals.  get_world_object_async query sent to the robot at every tick.  Callback registered to defined callback function.
+
+        Attributes:
+            client: The Client to a service on the robot
+            logger: Logger object
+            rate: Rate (Hz) to trigger the query
+            callback: Callback function to call when the results of the query are available
+    """
+    def __init__(self, client, logger, rate, callback):
+        super(AsyncWorldObject, self).__init__("world_object", client, logger,
+                                           period_sec=1.0/max(rate, 1.0))
+        self._callback = None
+        if rate > 0.0:
+            self._callback = callback
+
+    def _start_query(self):
+        if self._callback:
+            callback_future = self._client.list_world_objects_async()
+            callback_future.add_done_callback(self._callback)
+            return callback_future
+
 class AsyncMetrics(AsyncPeriodicQuery):
     """Class to get robot metrics at regular intervals.  get_robot_metrics_async query sent to the robot at every tick.  Callback registered to defined callback function.

@@ -361,6 +384,7 @@ class SpotWrapper():
             # Clients
             try:
                 self._robot_state_client = self._robot.ensure_client(RobotStateClient.default_service_name)
+                self._world_object_client = self._robot.ensure_client(WorldObjectClient.default_service_name)
                 self._robot_command_client = self._robot.ensure_client(RobotCommandClient.default_service_name)
                 self._graph_nav_client = self._robot.ensure_client(GraphNavClient.default_service_name)
                 self._power_client = self._robot.ensure_client(PowerClient.default_service_name)
@@ -421,11 +445,14 @@ class SpotWrapper():
                     self._point_cloud_requests)
             self._idle_task = AsyncIdle(self._robot_command_client,
                     self._logger, 10.0, self)
+            self._world_object_task = AsyncWorldObject(self._world_object_client,
+                    self._logger, max(0.0, self._rates.get("world_object",
+                        0.0)), self._callbacks.get("world_object", lambda:None))

             async_task_list = [self._robot_state_task,
                     self._robot_metrics_task, self._lease_task,
                     self._front_image_task, self._side_image_task,
-                    self._rear_image_task, self._point_cloud_task, self._idle_task]
+                    self._rear_image_task, self._point_cloud_task, self._idle_task, self._world_object_task]

             if self._robot.has_arm:
                 rospy.loginfo("Robot has arm so adding gripper async camera task")
@@ -467,6 +494,12 @@ class SpotWrapper():
         """Return latest proto from the _robot_state_task"""
         return self._robot_state_task.proto

+    @property
+    def world_object(self):
+        """Return latest proto from the _world_object_task"""
+        print("in property world_object")
+        return self._world_object_task.proto
+
     @property
     def metrics(self):
         """Return latest proto from the _robot_metrics_task"""

[keitayoneda]

今日は用事があるのでここで終わりますが、次はproto内にどんなデータが入っているかを見て、ros_msgにどういう形で出すか考えるところをやろうと思います

[keitayoneda]

https://github.com/boston-dynamics/spot-sdk/blob/d5e68145ac936495b2787e09b00b948e453b1ef2/protos/bosdyn/api/world_object.proto#L47 おそらくここにframeの情報は入っていそうというところまで調べました

[keitayoneda]

tf出せました！

[k-okada]

素晴らしい．ぜひ動画にもしてみましょう．こういう時にどういうシーンにしたらよいか，というのを考えるのが，将来とっても大切です．

• DockとかB2のドアのマーカが見えている状態で，ロボットがドックから離れて，廊下にでて５ｍぐらい歩く
• その間，ロボットの横？後ろを一緒に歩く人と，廊下では向かいのかなり遠くからジョギングして走ってすれ違う人がいる
• これのＴＦだけ表示して（ロボットのＴＦは消してよい），上のシーンが想像できれば，認識機能として使い物になりそう．

[sktometometo]

https://github.com/sktometometo/spot_ros-arm/pull/2 上記の @keitayoneda 君の変更