传感器数据模块

This content is not available in your language yet.

传感器数据模块

【本模块仅供旗舰款调用，基础款用户无需查阅】

本页面包含了传感器相关的话题和服务的相关文档协议，以提供系统的传感器方面的数据。

本模块位于 orin 开发板上，http 后端监听的端口为 56422。

各个传感器数据的分辨率和帧率如下：

传感器类型	位置	数据流	分辨率	帧率
鱼眼相机 (Senyun ISX031C-H190XA)	胸部左 (chest_left)	彩色图像 (color)	640x480	30 FPS
鱼眼相机 (Senyun ISX031C-H190XA)	胸部右 (chest_right)	彩色图像 (color)	640x480	30 FPS
RGB-D 相机 (Realsense D415)	头部前 (head_front)	彩色图像 (color)	1280x720	15 FPS
RGB-D 相机 (Realsense D415)	头部前 (head_front)	深度图像 (depth)	1280x720	15 FPS
RGB-D 相机 (Orbbec)	腰部前 (waist_front)	彩色图像 (color)	640x480	10 FPS
RGB-D 相机 (Orbbec)	腰部前 (waist_front)	深度图像 (depth)	640x400	10 FPS
激光雷达 (Mid360)	颈部 (neck)	点云数据 (pcd_option)	-	10 FPS
激光雷达 (Mid360)	颈部 (neck)	IMU 数据 (imu_option)	-	200 FPS
IMU (YISENSE YI 320)	腰部中心 (waist_center)	IMU 数据 (imu_option)	-	1000FPS
交互相机 (Senyun ISX031C-H100F1A)	胸部中间 (waist_center)	彩色图像 (color)	1920x1536	30 FPS

Channel 接口

话题名	话题描述	订阅 or 发布	消息类型	备注	通信后端
`/aima/hal/fish_eye_camera/chest_left/color`	胸部左鱼眼相机rgb图像	发布	sensor_msgs::msg::Image		ros2
`/aima/hal/fish_eye_camera/chest_right/color`	胸部右鱼眼相机rgb图像	发布	sensor_msgs::msg::Image		ros2
`/aima/hal/rgbd_camera/head_front/color`	头部相机rgb图像	发布	sensor_msgs::msg::Image		ros2
`/aima/hal/rgbd_camera/head_front/depth`	头部相机depth图像	发布	sensor_msgs::msg::Image		ros2
`/aima/hal/rgbd_camera/waist_front/color`	腰部相机rgb图像	发布	sensor_msgs::msg::Image		ros2
`/aima/hal/rgbd_camera/waist_front/depth`	腰部相机depth图像	发布	sensor_msgs::msg::Image		ros2
`/aima/hal/lidar/neck/pointcloud`	脖颈部雷达点云	发布	sensor_msgs::msg::PointCloud2		ros2
`/aima/hal/lidar/neck/imu`	颈部雷达imu	发布	sensor_msgs::msg::Imu		ros2
`/body_drive/imu/data`	腰部中心imu	发布	sensor_msgs::msg::Imu		ros2
`/aima/hal/camera/interactive/color`	胸部中间交互相机rgb图像	发布	sensor_msgs::msg::Image	仅 P1 机型支持，非常规 ros2 topic，使用共享内存通信，需要使用 AimRT（V1.0.0 版本及以上）进行获取，已提供相关示例；机型可通过 `cat /etc/bsp_version` 文件来判断，该文件存在则为 P1 机型，否则为 T3 机型。	iceoryx

RPC 接口

接口名	接口描述	请求消息类型	答复消息类型	备注	通信后端
`pb:/aimdk.protocol.CamerasIntrinsicService/GetCamerasIntrinsic`	获取相机的内参	aimdk::protocol::GetCamerasIntrinsicRequest	aimdk::protocol::GetCamerasIntrinsicResponse	请注意，获取相机内参时，在 P1 机型上鱼眼相机和交互相机由 camera 模块单独管理，http 端口号需要改为 59324；机型可通过 `cat /etc/bsp_version` 文件来判断，该文件存在则为 P1 机型，否则为 T3 机型。	http

Protobuf 消息类型

`aimdk::protocol::DistortionType`

畸变类型

Name	Number	Description
DistortionType_UNDEFINED	0	未定义
DistortionType_MODIFIED_BROWN_CONRADY	1	修改的布朗-康拉迪
DistortionType_INVERSE_BROWN_CONRADY	2	逆布朗-康拉迪
DistortionType_FTHETA	3	F-θ
DistortionType_BROWN_CONRADY	4	布朗-康拉迪
DistortionType_KANNALA_BRANDT4	5	卡纳拉-布兰特4
DistortionType_COUNT	6	计数

`aimdk::protocol::StreamType`

流类型

Name	Number	Description
StreamType_UNDEFINED	0	未定义
StreamType_DEPTH	1	深度
StreamType_COLOR	2	彩色
StreamType_IR	3	红外

`aimdk::protocol::CameraIntrinsicInfo`

相机内参信息

Field	Type	Description
id	string	相机ID
name	string	相机名称
stream_type	aimdk::protocol::StreamType	流类型
width	int32	图像宽度
height	int32	图像高度
ppx	float	主点X坐标
ppy	float	主点Y坐标
fx	float	焦距X
fy	float	焦距Y
model	aimdk::protocol::DistortionType	畸变模型
coeffs	float[]	畸变系数

`aimdk::protocol::GetCamerasIntrinsicRequest`

获取相机内参请求

Field	Type	Description
header	aimdk::protocol::RequestHeader	请求头

`aimdk::protocol::GetCamerasIntrinsicResponse`

获取相机内参的响应结果

Field	Type	Description
info	aimdk::protocol::CameraIntrinsicInfo[]	相机内参信息列表

标定参数

相机内外参标定结果的保存目录（位于orin开发板）：

/agibot/data/param/calibration/

文件夹中包含以下文件：

外参：
- extrinsic_baselink_T_chest_left.txt：表示base_link到左目鱼眼的外参（x, y, z, qw, qx, qy ,qz）
- extrinsic_chest_right_T_chest_left.txt：表示左目相机到右目相机的外参（x, y, z, qw, qx, qy ,qz）
- extrinsic_baselink_T_head_front.txt：base_link到头部相机的外参（x, y, z, qw, qx, qy ,qz）
- estrinsic_waist_front_T_baselink.txt：表示base_link到胯部相机的外参（x, y, z, qw, qx, qy ,qz）
- estrinsic_baselink_T_lidar.txt：雷达到base_link的外参（x, y, z, qw, qx, qy ,qz）
- gravity_T_imu.txt：表示imu到base_link的旋转（qw, qx, qy ,qz）
内参：
- intrinsic_chest_left.yaml：左目鱼眼相机内参
- intrinsic_chest_right.yaml：右目鱼眼相机内参
- intrinsic_head_front.yaml：头部相机内参
- intrinsic_wasit_front.yaml：胯部相机内参

注：x, y, z表示平移；qw, qx, qy, qz为四元数（Hamilton表达），表示旋转。

相机内参读取

内参建议使用接口 pb:/aimdk.protocol.CamerasIntrinsicService/GetCamerasIntrinsic 进行获取，该接口从相机 SDK 中获取对应内参更为准确，调用示例如下：

# 获取相机内参，P1 机型上鱼眼相机和交互相机由 camera 模块单独管理，需要 http 端口号需要改为 59324
# 机型可通过 `cat /etc/bsp_version` 文件来判断，该文件存在则为 P1 机型，否则为 T3 机型。
curl --request POST \
  --url http://192.168.100.110:56422/rpc/aimdk.protocol.CamerasIntrinsicService/GetCamerasIntrinsic \
  --header 'content-type: application/json' \
  --data '{}'

针孔相机内参读取

针孔相机内参（intrinsic_head_front.yaml）示例：

%YAML:1.0
---
model_type: PINHOLE
camera_name: ""
image_width: 1280
image_height: 720
distortion_parameters:
  k1: 8.4721332081814996e-02
  k2: -2.2328724059363472e-01
  p1: 1.4980714426687237e-02
  p2: 5.8573350999184884e-03
projection_parameters:
  fx: 9.4067882179688206e+02
  fy: 9.3408163057525098e+02
  cx: 6.3075913699847933e+02
  cy: 3.7669033617329160e+02

fx, fy, cx, cy 对应相机内参矩阵 (correspond to the camera intrinsic matrix), k1, k2, p1, p2 对应畸变系数 (correspond to distortion coefficients).

相应读取函数如下：

// 针孔相机内参读取函数 (Pinhole camera intrinsic reading function)
bool ReadFromYamlFile(const std::string& filename)
{
    cv::FileStorage fs(filename, cv::FileStorage::READ);

    if (!fs.isOpened())
    {
        return false;
    }

    if (!fs["model_type"].isNone())
    {
        std::string sModelType;
        fs["model_type"] >> sModelType;

        if (sModelType.compare("PINHOLE") != 0)
        {
            return false;
        }
    }

    std::string model_type_ = "PINHOLE";
    std::string camera_name_;
    fs["camera_name"] >> camera_name_;
    int image_width_  = static_cast<int>(fs["image_width"]);
    int image_height_ = static_cast<int>(fs["image_height"]);

    cv::FileNode n = fs["distortion_parameters"];
    double k1_            = static_cast<double>(n["k1"]);
    double k2_            = static_cast<double>(n["k2"]);
    double p1_            = static_cast<double>(n["p1"]);
    double p2_            = static_cast<double>(n["p2"]);

    n   = fs["projection_parameters"];
    double fx_ = static_cast<double>(n["fx"]);
    double fy_ = static_cast<double>(n["fy"]);
    double cx_ = static_cast<double>(n["cx"]);
    double cy_ = static_cast<double>(n["cy"]);

    return true;
}

鱼眼相机内参读取

鱼眼相机内参（intrinsic_chest_left.yaml）示例：

%YAML:1.0
---
model_type: KANNALA_BRANDT
camera_name: ""
image_width: 640
image_height: 480
projection_parameters:
  k2: 4.0154631284881101e-02
  k3: -1.9502762632935614e-02
  k4: 8.9682432239682577e-04
  k5: -5.0758203706675745e-04
  mu: 1.9728857021424918e+02
  mv: 1.9697037865238804e+02
  u0: 3.1897257040247649e+02
  v0: 2.4312745081143589e+02

u0, v0, mu, mv 对应相机的内参矩阵 (correspond to the camera intrinsic matrix), k2, k3, k4, k5 对应畸变系数 (correspond to distortion coefficients).

相应读取函数如下：

// 鱼眼相机内参读取函数 (Fisheye camera intrinsic reading function)
bool ReadFromYamlFile(const std::string& filename)
{
    cv::FileStorage fs(filename, cv::FileStorage::READ);

    if (!fs.isOpened())
    {
        return false;
    }

    if (!fs["model_type"].isNone())
    {
        std::string sModelType;
        fs["model_type"] >> sModelType;

        if (sModelType.compare("KANNALA_BRANDT") != 0)
        {
            return false;
        }
    }

    std::string model_type_ = "KANNALA_BRANDT";
    std::string camera_name_;
    fs["camera_name"] >> camera_name_;
    int image_width_  = static_cast<int>(fs["image_width"]);
    int image_height_ = static_cast<int>(fs["image_height"]);

    cv::FileNode n = fs["projection_parameters"];
    double k2_            = static_cast<double>(n["k2"]);
    double k3_            = static_cast<double>(n["k3"]);
    double k4_            = static_cast<double>(n["k4"]);
    double k5_            = static_cast<double>(n["k5"]);
    double mu_            = static_cast<double>(n["mu"]);
    double mv_            = static_cast<double>(n["mv"]);
    double u0_            = static_cast<double>(n["u0"]);
    double v0_            = static_cast<double>(n["v0"]);

    return true;
}

相机外参读取

相机外参对应的数据格式：

x, y, z, qw, qx, qy, qz

读取函数如下：

// 外参读取函数 (Extrinsic parameter reading function)
bool ReadPoseFromFile(const std::string& file_name, SE3d& pose)
{
    if (!std::filesystem::exists(file_name))
    {
        LOG(WARNING) << "pose file don't exists!" << file_name;
        return false;
    }

    std::ifstream     pose_ifs(file_name);
    Eigen::Quaternion rot   = pose.unit_quaternion();
    Vec3d             trans = pose.translation();
    pose_ifs >> trans.x() >> trans.y() >> trans.z() >> rot.w() >> rot.x() >> rot.y() >> rot.z();
    pose = SE3d(rot, trans);
    pose_ifs.close();
    return true;
}