【Chrono Engine学习总结】5-sensor-5.2-导出lidar数据的方法与原理探究

由于Chrono的官方教程在一些细节方面解释的并不清楚,自己做了一些尝试,做学习总结。

1、Sensor数据生成流程回顾

Chrono里面,sensor的数据生成、可视化、以及保存,都需要单独进行设置才能实现。sensor数据的采集流程如下https://api.projectchrono.org/sensor_overview.html
在这里插入图片描述
可以看出,sensor数据更新是另一个线程中进行,当场景/物体发生变化后,获得sensor的原始数据,之后应用各种滤波器,最后将滤波后的数据输出,可以在主线程中进行调用/保存。这就是sensor数据采集的整体流程。

2、数据输出的两种方式

2.1 通过滤波器实现

在官方的demo_SEN_gator和demo_SEN_HMMWV中,给出了通过滤波器方式实现数据输出的例子。关键核心步骤如下:

// 1. 创建一个sensor的manager
auto manager = chrono_types::make_shared<ChSensorManager>(gator.GetSystem());
// 2. 创建一个sensor
auto vlp16 = chrono::sensor::Sensor::CreateFromJSON(GetChronoDataFile("sensor/json/Velodyne/HDL-32E.json"), gator.GetChassisBody(), lidar_extrinsics);
// 3. 添加“保存点云”滤波器。只有加上这一行,才能够在后续仿真中自动输出
vlp16->PushFilter(chrono_types::make_shared<ChFilterSavePtCloud>("C:/Users/larrydong/Desktop/lidar_output/"));  // 包含这个filter,在每次Apply对应的filter时(update中),会执行保存到文件,并输出Beam count
// 4. 添加sensor到管理器
manager->AddSensor(vlp16);

// 循环部分
manager->Update();  
gator.Synchronize(time, driver_inputs, terrain);
gator.Advance(step_size);

程序运行之后,将会在滤波器给出的输出路径,输出第N帧数据的csv文件,文件内为点云数据。

2.2 通过直接读取Buffer实现

官方的tutorial中给出了一个读取数据的方式https://api.projectchrono.org/lidar_sensor.html
在这里插入图片描述
但并没有详细解释。进行了探究,发现,只需要在主循环中,判断读到了Buffer之后,即可输出数据,从这里写入文件即可。但这里存在一个问题,Buffer的大小是多少,即有多少个lidar点?这个问题在下一小节进行探讨。

3、详细探究lidar数据更新过程

3.1 关于Buffer读取的探究

在2.2中,给出了通过直接读取lidar的Buffer的方式读取数据。我们查看GetMostRecentBuffer函数,调用了ChSensor中的GetMostRecentBufferHelper,进一步调用了ChFilterAccess中的GetBuffer()。从这个最终的函数中,可以看出获取了buffer的宽、高、时间戳、和更新次数(LaunchedCount),因此在2.2中获取Buffer之后,可以通过数据的宽和高(对于lidar来说是Nx1的),获取lidar数据。

例如,20Hz的lidar,我们在main循环中,每步都输出Buffer的基本信息。代码:
在这里插入图片描述输出如下:
在这里插入图片描述即当前点云有24272个点,对应时间是3.8s和3.85s,对应第76和77帧。但我们发现,Buffer虽然没有更新,但依然是存在的,因此不能单纯通过Buffer是否存在,判断有无更新,而是需要通过launchCount这个数进行判断。但这时,我们仍不清楚lidar的数据在什么时刻“输出”的。我们马上会说到这个问题。

3.2 采用Filter方式输出

例子当中,我们把lidar这个sensor绑定到一个车辆vehicle当中。这部分其实很容易实现,只需要在创建一个sensor时指定attach的实体,给上我们的vehicle即可。这里的vehicle用的是Gator。

auto vlp16 = chrono::sensor::Sensor::CreateFromJSON(
      GetChronoDataFile("sensor/json/Velodyne/HDL-32E.json"), 
      gator.GetChassisBody(), 					// 绑定到Gator的底盘上,Gator的底盘本质上还是一个ChBody
      lidar_extrinsics);							// 相对于底盘的位置参数,即lidar外参

在main当中,我们有三个关键函数:sensor manager的update(),gator的Synchronize,以及gator的Advance()。经测试标明,在lidar输出了数据之后,Advance()执行时,会输出lidar数据到指定路径。具体的实现是调用了ChFilterSavePtCloud的Apply函数实现的,如下:

在这里插入图片描述

3.3 函数详解

需要提醒的是,调用GetMostRecentBuffer和Filter两种方式输出并不冲突。前者是我们自己读取Buffer获取,后者是仿真里面“例行公事”完成的,例行公事的函数就是这个Advance。只不过,我并不是很清楚,什么时候lidar的Buffer中更新了数据、什么时候执行的Advance。

提醒的第2点,sensor绑定vehicle后,更新的部分之和vehicle的函数有关,哪怕是sensor的可视化,也与irrlicht等可视化模块无关。只有当sensor的filter添加了可视化的filter之后,才能显示sensor的数据,并且窗口中数据的更新是跟vehicle的,并不是跟vis模块的。

在“认可”上面两条之后,我们探讨这部分的几个函数。

manager->Update()

在这里插入图片描述我们看sensor manager的Update,可以看到,包括:对所有光学传感器更新、非光学传感器更新两部分。前者依赖OptiX库,底层实现在 ChOptixEngine.cpp 中。对于光学的UpdateSensors,具体如下:
在这里插入图片描述在这个Update完成之后,可以认为lidar已经“采集了”一个新的数据,但这个数据并没有“传输”出来。此时,在执行Adance()函数时,会更新可视化窗口。即下图左侧的这个窗口,是由Sensor管理的,在sensor更新时才会更新,更新的频率和右侧的仿真窗口不同,右侧的是可视化例如irrlicht控制的。
在这里插入图片描述

gator.Advance()
这个Advance函数很复杂, 包括了vehicle和各种子系统的更新。由于Gator是一个wheelvehicle,也是一个vehicle,所以Gator的advance在完成了Gator一些独特的操作之后,再次调用ChWheelVehicle的advance,完成wheel特有的操作之后,再调用了ChVehicle的advance。同时,vehicle包括一个物理系统(之前博客讲过,如果有vehicle之后,就不需要再定义仿真物理系统了),最终执行的是物理系统的DoStepDynamic
在这里插入图片描述到这一步往下就没有深究。但正如上面所说,具体advance时,执行了更新图像、保存数据等操作。

何时更新图形、何时保存数据?
我们来看最后一个问题,到底是什么时候保存的数据、什么时候更新的lidar显示?由于上面已经说明了,lidar的显示和输出,并不是同步的。

为了充分模拟传感器,chrono在场景变化后,lidar的采集数据是立刻变化的,可视化的窗口会进行更新,但此时的数据并没有输出出来。这个输出延时,就是对传感器设置时有一个lag参数,表示“从采集到数据,到数据输出”的时间延迟。

可以这么理解,仿真过程是这个样子:

  1. 当达到物理仿真频率时(很高,例如1ms),物理环境就发生变换:
  2. 当达到sensor的采集频率时(不高,例如20Hz的lidar是50ms), lidar传感器获得了一次完整的frame,这一步由manager->Update实现;但在这个过程中,采集过程是另一个线程实现的,因此数据是连续的,并不是单纯的在“这一个时刻”获得的;
  3. 在sensor有效的update之后,Advance时对可视化窗口进行了更新(但“传输”的数据还没有更新)
  4. 经过当sensor的lag之后,如果定义了获取数据的Filter(ChFilterXYZIAccess),此时输出数据获得了更新,即GetMostRecentBuffer 获取到的数据进行了更新;如果又定义了保存点云的Filter(ChFilterSavePtCloud),此时Advance时会输出csv文件到路径。

通过对一个完整的过程打断点与输出,验证了上面的理解:
在这里插入图片描述

  • “Width xxx” 这一行输出,是仿真每次循环(每1ms)时输出一次;
  • “Beam count” 这一行输出是Advance中更新图像窗口时的输出,精准的每50次执行一次;
  • 在0.1s和0.15s之间,可以看出点云的数据Buffer发生了变化,也就是这个时刻,lidar已经update的数据“传输”了、GetMostRecentBuffer发生了更新,因此此时Advance时能够保存点云到路径;
  • 在每次Update之后,经过了20次loop,更新的buff和输出点云,这个20次就是sensor的lag参数控制的。我设置成了0.02s,因此经过1ms的循环20次,“传输”了。

4、完整代码

这个代码太长了,主要是用来测试上面的内容。

#include "chrono/core/ChRealtimeStep.h"
#include "chrono/utils/ChUtilsInputOutput.h"
#include "chrono/physics/ChBodyEasy.h"
#include "chrono_vehicle/ChVehicleModelData.h"
#include "chrono_vehicle/terrain/RigidTerrain.h"
#include "chrono_vehicle/driver/ChInteractiveDriverIRR.h"
#include "chrono_vehicle/wheeled_vehicle/ChWheeledVehicleVisualSystemIrrlicht.h"
#include "chrono_models/vehicle/gator/Gator.h"
#include "chrono_thirdparty/filesystem/path.h"
#include "chrono_sensor/sensors/Sensor.h"
#include "chrono_sensor/sensors/ChCameraSensor.h"
#include "chrono_sensor/sensors/ChLidarSensor.h"
#include "chrono_sensor/ChSensorManager.h"
#include "chrono_sensor/filters/ChFilterAccess.h"
#include "chrono_sensor/filters/ChFilterPCfromDepth.h"
#include "chrono_sensor/filters/ChFilterVisualize.h"
#include "chrono_sensor/filters/ChFilterSave.h"
#include "chrono_sensor/filters/ChFilterSavePtCloud.h"
#include "chrono_sensor/filters/ChFilterVisualizePointCloud.h"
#include "chrono_sensor/filters/ChFilterAccess.h"
#include "chrono_sensor/filters/ChFilterVisualize.h"
#include "chrono_sensor/sensors//ChSensorBuffer.h"
#include <iostream>

using namespace chrono;
using namespace chrono::irrlicht;
using namespace chrono::vehicle;
using namespace chrono::vehicle::gator;
using namespace chrono::sensor;


// =============================================================================

// Initial vehicle location and orientation
ChVector<> initLoc(0, 0, 0.5);
ChQuaternion<> initRot(1, 0, 0, 0);

// Visualization type for vehicle parts (PRIMITIVES, MESH, or NONE)
VisualizationType chassis_vis_type = VisualizationType::PRIMITIVES;
VisualizationType suspension_vis_type = VisualizationType::PRIMITIVES;
VisualizationType steering_vis_type = VisualizationType::PRIMITIVES;
VisualizationType wheel_vis_type = VisualizationType::PRIMITIVES;
VisualizationType tire_vis_type = VisualizationType::PRIMITIVES;

// Collision type for chassis (PRIMITIVES, HULLS, or NONE)
CollisionType chassis_collision_type = CollisionType::NONE;

// Type of tire model (RIGID, TMEASY)
TireModelType tire_model = TireModelType::TMEASY;

// Rigid terrain
RigidTerrain::PatchType terrain_model = RigidTerrain::PatchType::BOX;

// Contact method
ChContactMethod contact_method = ChContactMethod::NSC;

// Simulation step sizes
double step_size = 1e-3;
double tire_step_size = step_size;

// Time interval between two render frames
double render_step_size = 1.0 / 50;  // FPS = 50

// SENSOR PARAMETERS
// Save sensor data
bool sensor_save = false;

// Visualize sensor data
bool sensor_vis = true;


// =============================================================================

int main(int argc, char* argv[]) {
    GetLog() << "Copyright (c) 2017 projectchrono.org\nChrono version: " << CHRONO_VERSION << "\n\n";
    chrono::SetChronoDataPath("E:/codeGit/chrono/chrono/build/data/");              // change the default data loading path.
    chrono::vehicle::SetDataPath("E:/codeGit/chrono/chrono/build/data/vehicle/");              // change the vehicle data path

    // --------------
    // Create vehicle
    // --------------
    Gator gator;
    gator.SetContactMethod(contact_method);
    gator.SetChassisCollisionType(chassis_collision_type);
    gator.SetChassisFixed(false);
    gator.SetInitPosition(ChCoordsys<>(initLoc, initRot));
    gator.SetTireType(tire_model);
    gator.SetTireStepSize(tire_step_size);
    gator.SetAerodynamicDrag(0.5, 5.0, 1.2);
    gator.Initialize();
    gator.SetChassisVisualizationType(chassis_vis_type);
    gator.SetSuspensionVisualizationType(suspension_vis_type);
    gator.SetSteeringVisualizationType(steering_vis_type);
    gator.SetWheelVisualizationType(wheel_vis_type);
    gator.SetTireVisualizationType(tire_vis_type);
    // Associate a collision system
    gator.GetSystem()->SetCollisionSystemType(ChCollisionSystem::Type::BULLET);

    // ------------------
    // Create the terrain
    // ------------------

    RigidTerrain terrain(gator.GetSystem());

    ChContactMaterialData minfo;
    minfo.mu = 0.9f;
    minfo.cr = 0.01f;
    minfo.Y = 2e7f;
    auto patch_mat = minfo.CreateMaterial(contact_method);
    std::shared_ptr<RigidTerrain::Patch> patch;
    patch = terrain.AddPatch(patch_mat, CSYSNORM, 100.0, 100.0);
    patch->SetTexture(vehicle::GetDataFile("terrain/textures/tile4.jpg"), 200, 200);
    patch->SetColor(ChColor(0.8f, 0.8f, 0.5f));
    terrain.Initialize();

    auto ground_body = patch->GetGroundBody();      // ground_body: ChBody 是地形的ChBody

    auto vis_mat1 = chrono_types::make_shared<ChVisualMaterial>();
    vis_mat1->SetKdTexture(vehicle::GetDataFile("terrain/textures/grass.jpg"));         // Kd, Ks, Ke:一些材质的反射、发光等设定。
    vis_mat1->SetWeightTexture(GetChronoDataFile("sensor/textures/weight1.png"));       // 两种地形材料的混合权重。weight1和2是互补的。
    vis_mat1->SetSpecularColor({ .0f, .0f, .0f });
    vis_mat1->SetRoughness(1.f);
    vis_mat1->SetUseSpecularWorkflow(false);

    auto vis_mat2 = chrono_types::make_shared<ChVisualMaterial>();
    vis_mat2->SetKdTexture(vehicle::GetDataFile("terrain/textures/dirt.jpg"));
    vis_mat2->SetWeightTexture(GetChronoDataFile("sensor/textures/weight2.png"));
    vis_mat2->SetSpecularColor({ .0f, .0f, .0f });
    vis_mat2->SetRoughness(1.f);
    vis_mat2->SetUseSpecularWorkflow(false);

    auto visual_shape = ground_body->GetVisualModel()->GetShape(0);
    visual_shape->SetMaterial(0, vis_mat1);         // 将第i个material设置为某个特定的material
    visual_shape->AddMaterial(vis_mat2);

    // -------------------------------------
    // Create the vehicle Irrlicht interface
    // -------------------------------------
    auto vis = chrono_types::make_shared<ChWheeledVehicleVisualSystemIrrlicht>();
    vis->SetWindowTitle("Gator Demo");
    vis->SetChaseCamera(ChVector<>(0.0, 0.0, 2.0), 5.0, 0);
    vis->Initialize();
    vis->AddTypicalLights();
    vis->AddSkyBox();
    vis->AddLogo();
    vis->AttachVehicle(&gator.GetVehicle());

    // ------------------------
    // Create the driver system
    // ------------------------
    // Create the interactive driver system
    ChInteractiveDriverIRR driver(*vis);
    // Set the time response for steering and throttle keyboard inputs.
    double steering_time = 1.0;  // time to go from 0 to +1 (or from 0 to -1)
    double throttle_time = 1.0;  // time to go from 0 to +1
    double braking_time = 0.3;   // time to go from 0 to +1
    driver.SetSteeringDelta(render_step_size / steering_time);
    driver.SetThrottleDelta(render_step_size / throttle_time);
    driver.SetBrakingDelta(render_step_size / braking_time);
    driver.Initialize();


    // 传感器部分
    auto manager = chrono_types::make_shared<ChSensorManager>(gator.GetSystem());
    manager->scene->AddPointLight({ 100, 100, 100 }, { 2, 2, 2 }, 5000);
    auto lidar_extrinsics = chrono::ChFrame<double>({ 0, 0, 1 }, Q_from_AngAxis(0, { 1, 0, 0 }));
    // Lidar from JSON file - Velodyne VLP-16
    auto vlp16 = chrono::sensor::Sensor::CreateFromJSON(GetChronoDataFile("sensor/json/Velodyne/HDL-32E.json"), gator.GetChassisBody(), lidar_extrinsics);
    vlp16->PushFilter(chrono_types::make_shared<ChFilterSavePtCloud>("C:/Users/larrydong/Desktop/lidar_output/"));  // 包含这个filter,在每次Apply对应的filter时(update中),会执行保存到文件,并输出Beam count
    manager->AddSensor(vlp16);

    // ---------------
    // Simulation loop
    // ---------------
    // output vehicle mass
    std::cout << "VEHICLE MASS: " << gator.GetVehicle().GetMass() << std::endl;

    // Number of simulation steps between miscellaneous events
    int render_steps = (int)std::ceil(render_step_size / step_size);

    // Initialize simulation frame counters
    int step_number = 0;
    int render_frame = 0;

    float orbit_radius = 10.f;
    float orbit_rate = 1;

    ChRealtimeStepTimer realtime_timer;         // Ch
    while (vis->Run()) {
        double time = gator.GetSystem()->GetChTime();

        chrono::sensor::UserXYZIBufferPtr data_ptr;
        data_ptr = vlp16->GetMostRecentBuffer< chrono::sensor::UserXYZIBufferPtr>();
        if (data_ptr->Buffer) {
            using namespace std;
            cout << "==>    Width: " << data_ptr->Width << ", height: " << data_ptr->Height << ",  ts: " << data_ptr->TimeStamp << ", launched count: " << data_ptr->LaunchedCount << endl;
        }

        manager->Update();

        // Render scene and output POV-Ray data
        if (step_number % render_steps == 0) {
            vis->BeginScene();
            vis->Render();              // 这一步更新的lidar?
            vis->EndScene();
            render_frame++;
        }

        // Get driver inputs
        DriverInputs driver_inputs = driver.GetInputs();

        // Update modules (process inputs from other modules)
        driver.Synchronize(time);
        terrain.Synchronize(time);
        gator.Synchronize(time, driver_inputs, terrain);
        vis->Synchronize(time, driver_inputs);

        // Advance simulation for one timestep for all modules
        driver.Advance(step_size);
        terrain.Advance(step_size);
        gator.Advance(step_size);
        vis->Advance(step_size);

        // Increment frame number
        step_number++;

        // Spin in place for real time to catch up
        realtime_timer.Spin(step_size);
    }
    return 0;
}

还是建议参考官方的两个demo:

在这里插入图片描述

5、小结

吐槽一下Chrono的官方文档,有些内容讲的真的不透彻;

个人建议:采用Filter的方式输出lidar数据,避免用Buffer的方式判断有无更新、而且效率也不一定高。Filter的输出在“sensor更新->延迟->Advance”后存储,文件名是“编号.csv”,可以通过采集频率确定每个编号对应的时间戳。

  • 14
    点赞
  • 28
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值