autoware foundationのtopicを可視化する

rosbagsでautoware foundationのtopicを可視化する方法
rosbagsではROSのパッケージをインストールしなくてもtopicを可視化できるので
rosbagを取得してjupyter環境(MAC)に転送して調査できる。

# 1.rosbagを取得してjupyter環境(MAC)に転送する。
autowareを起動してシュミレーションを起動する。
起動中にautowareの以下のtopicを取得してjupyter環境(MAC)に転送する。
- /tf
- /sensing/lidar/concatenated/pointcloud
- /perception/object_recognition/detection/objects

https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/rosbag-replay-simulation/


## 1.1.autoware起動
```
source ~/autoware/install/setup.bash
ros2 launch autoware_launch logging_simulator.launch.xml map_path:=$HOME/Downloads/sample-map-rosbag vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit
```

## 1.2.rosbagを取得
```Bash
source ~/autoware/install/setup.bash
ros2 bag record /tf /sensing/lidar/concatenated/pointcloud /perception/object_recognition/detection/objects
```

## 1.3.シュミレーションを起動
```Bash
source ~/autoware/install/setup.bash
ros2 bag record /tf /sensing/lidar/concatenated/pointcloud /perception/object_recognition/detection/objects
```

## 1.4.rosbagをjupyter環境のマシン(MAC)に転送


# 2.rosbagsの使い方
## 2.1.rosbagsインストール
```Bash
pip install rosbags
```

## 2.2.jupyterのコード
```Python
from rosbags.rosbag2 import Reader
from rosbags.serde import deserialize_cdr
from rosbags.typesys import get_types_from_idl, register_types

import numpy as np
import pandas as pd
import open3d as o3d
from pathlib import Path

rosbag_dir = 'rosbag2/rosbag2_2023_02_21-08_59_39'

def get_connectin(connections, topic):
    connections = [x for x in connections if x.topic == topic]
    return connections

```

### 2.2.1. /tfをdump
```Python
tf_msgs = []
with Reader(rosbag_dir) as reader:
    connections = get_connectin(reader.connections, '/tf')
    for connection, timestamp, rawdata in reader.messages(connections=connections):
        msg = deserialize_cdr(rawdata, connection.msgtype)
        tf_msgs.append(msg)
tf_msg = tf_msgs[0]
tf_msg
```
![tf.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/100239/51bc04e1-4ce6-cebf-0407-9660ee3565be.jpeg)

### 2.2.2. /sensing/lidar/concatenated/pointcloudをdump
```Python
pointcloud2_msgs = []
with Reader(rosbag_dir) as reader:
    connections = get_connectin(reader.connections, '/sensing/lidar/concatenated/pointcloud')
    for connection, timestamp, rawdata in reader.messages(connections=connections):
        msg = deserialize_cdr(rawdata, connection.msgtype)
        pointcloud2_msgs.append(msg)
pointcloud2_msg = pointcloud2_msgs[0]
pointcloud2_msg
```
![pointcloud2.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/100239/d102b017-efa4-aa74-c7de-c6ccf63ddd93.jpeg)

Pointcloud2のmsgでは点群の座標はdataの中に入っているがunit8で入っているためfloat32に変換する必要がある。Pointcloud2のdataをnumpyに変換する必要がある。
(参考:https://github.com/ros2/common_interfaces/blob/60df44894caafa67767da4c71d91f8c5eb7abf38/sensor_msgs_py/sensor_msgs_py/point_cloud2.py)

##### Pointcloud2のmsgをnumpyに変換する関数
```Python
_DATATYPES = {}
_DATATYPES[1] = np.dtype(np.int8)
_DATATYPES[2] = np.dtype(np.uint8)
_DATATYPES[3] = np.dtype(np.int16)
_DATATYPES[4] = np.dtype(np.uint16)
_DATATYPES[5] = np.dtype(np.int32)
_DATATYPES[6] = np.dtype(np.uint32)
_DATATYPES[7] = np.dtype(np.float32)
_DATATYPES[8] = np.dtype(np.float64)

def dtype_from_fields(fields):
    """
    Convert a Iterable of sensor_msgs.msg.PointField messages to a np.dtype.
    :param fields: The point cloud fields.
                   (Type: iterable of sensor_msgs.msg.PointField)
    :returns: NumPy datatype
    """
    # Create a lists containing the names, offsets and datatypes of all fields
    field_names = []
    field_offsets = []
    field_datatypes = []
    for i, field in enumerate(fields):
        # Datatype as numpy datatype
        datatype = _DATATYPES[field.datatype]
        # Name field
        if field.name == '':
            name = f'{DUMMY_FIELD_PREFIX}_{i}'
        else:
            name = field.name
        # Handle fields with count > 1 by creating subfields with a suffix consiting
        # of "_" followed by the subfield counter [0 -> (count - 1)]
        assert field.count > 0, "Can't process fields with count = 0."
        for a in range(field.count):
            # Add suffix if we have multiple subfields
            if field.count > 1:
                subfield_name = f'{name}_{a}'
            else:
                subfield_name = name
            assert subfield_name not in field_names, 'Duplicate field names are not allowed!'
            field_names.append(subfield_name)
            # Create new offset that includes subfields
            field_offsets.append(field.offset + a * datatype.itemsize)
            field_datatypes.append(datatype.str)
            

    # Create a tuple for each field containing name and data type
    return np.dtype({
        'names': field_names,
        'formats': field_datatypes,
        'offsets': field_offsets,
    })

def getpoints(msg):
    return np.ndarray(
        shape=(msg.width * msg.height, ),
        dtype=dtype_from_fields(msg.fields),
        buffer=msg.data)
```

##### Pointcloud2のmsgをPCDに変換

```Python
points = getpoints(pointcloud2_msg)
df_points = pd.DataFrame(data=points)
xyz= df_points[['x','y','z']].values

pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(xyz)
o3d.io.write_point_cloud("test_pointcloud0.pcd", pcd)
o3d.visualization.draw_geometries([pcd])
```

##### PCDを可視化
![pcd.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/100239/e0fa3234-4009-26cb-93c8-6c1ce432167f.png)

### 2.2.3. /perception/object_recognition/detection/objectsをdump
DetectedObjectsのmsgはautoware foundation独自のmsgのためrosbagsに定義がない。
以下の方法でcustom message typeを追加する必要がある。

##### 定義をgit clone
```Bash
# autoware_auto_msgsの定義(IDL)をgithubからclone
$ git clone https://github.com/tier4/autoware_auto_msgs.git
```

##### rosbagsにcusmtom message typeを追加
```Python
add_types = {}

for pathstr in [
    'autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObjects.idl',
    'autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObject.idl',
    'autoware_auto_msgs/autoware_auto_perception_msgs/msg/ObjectClassification.idl',
    'autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObjectKinematics.idl',
    'autoware_auto_msgs/autoware_auto_perception_msgs/msg/Shape.idl',
]:
    msgpath = Path(pathstr)
    msgdef = msgpath.read_text(encoding='utf-8')
    add_types.update(get_types_from_idl(msgdef))

register_types(add_types)
```

##### detectedobjectsを抽出
```Python
detectedbjects_msgs = []
with Reader(rosbag_dir) as reader:
    connections = get_connectin(reader.connections, '/perception/object_recognition/detection/objects')
    for connection, timestamp, rawdata in reader.messages(connections=connections):
        msg = deserialize_cdr(rawdata, connection.msgtype)
        detectedbjects_msgs.append(msg)
```

##### objectのlabelとpositionをprint
```Python
detectedbjects_msg = detectedbjects_msgs[0]
detected_objects = []
for object_ in detectedbjects_msg.objects:
    x = object_.kinematics.pose_with_covariance.pose.position.x
    y = object_.kinematics.pose_with_covariance.pose.position.y
    z = object_.kinematics.pose_with_covariance.pose.position.z
    position = [x,y,z]
    for classification in object_.classification:
        label = classification.label
    detected_object = {'label': label, 'position': position}
    detected_objects.append(detected_object)
detected_objects
```
![detected_objects.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/100239/1732f9f5-f48b-39a1-d866-d8983afc3080.png)