3DLiDARを追加して欲しい

[tmori]

基本Spec

• 周波数：5Hz
• 水平：-90° 〜 90°
• 垂直：-30° 〜 30°
• 分解能：1°

[tmori]

データ型

PointCloud2 を使う。

std_msgs/Header header

# 2D structure of the point cloud. If the cloud is unordered, height is
# 1 and width is the length of the point cloud.
uint32 height
uint32 width

# Describes the channels and their layout in the binary data blob.
PointField[] fields

bool    is_bigendian # Is this data bigendian?
uint32  point_step   # Length of a point in bytes
uint32  row_step     # Length of a row in bytes
uint8[] data         # Actual point data, size is (row_step*height)

bool is_dense        # True if there are no invalid points

[tmori]

• height: 61
• width: 181
• filedsの配列長：4
• is_bigendian：false(リトルエンディアン)
• point_step：16bytes
• row_step：181x16bytes
• dataの配列長：61x181x16

filedsの内訳：

[
  {name: "x", offset: 0, datatype: FLOAT32, count: 1},
  {name: "y", offset: 4, datatype: FLOAT32, count: 1},
  {name: "z", offset: 8, datatype: FLOAT32, count: 1},
  {name: "intensity", offset: 12, datatype: FLOAT32, count: 1}
]

[tmori]

こんな感じでUnityのインスペクタビューでパラメータ定義できるようにしてやった。

[tmori]

以下で対応。

https://github.com/toppers/hakoniwa-unity-simasset-plugin/commit/623b0f8d813a21d5a44cfc554bd80cd12a35be72

[tmori]

動作確認プログラム

def parse_lidarData(data):

    # reshape array of floats to array of [X,Y,Z]
    points = numpy.array(data.point_cloud, dtype=numpy.dtype('f4'))
    points = numpy.reshape(points, (int(points.shape[0]/3), 3))

    return points

def main():
    if len(sys.argv) != 2:
        print(f"Usage: {sys.argv[0]} <config_path>")
        return 1

    # connect to the HakoSim simulator
    client = hakosim.MultirotorClient(sys.argv[1])
    client.confirmConnection()
    client.enableApiControl(True)
    client.armDisarm(True)
    client.takeoff(3)

    lidarData = client.getLidarData()
    if (len(lidarData.point_cloud) < 3):
        print("\tNo points received from Lidar data")
    else:
        print(f"len: {len(lidarData.point_cloud)}")
        points = parse_lidarData(lidarData)
        print("\tReading: time_stamp: %d number_of_points: %d" % (lidarData.time_stamp, len(points)))
        print("\t\tlidar position: %s" % (pprint.pformat(lidarData.pose.position)))
        print("\t\tlidar orientation: %s" % (pprint.pformat(lidarData.pose.orientation)))

        condition = numpy.logical_and(points <= 3, points > 0)
        filtered_points = points[numpy.any(condition, axis=1)]
        print(filtered_points)