ApolloAuto / apollo

An open autonomous driving platform
Apache License 2.0
25.01k stars 9.67k forks source link

How to use dreamview to display custom point cloud? #13736

Closed TheGreatGalaxy closed 3 years ago

TheGreatGalaxy commented 3 years ago

We appreciate you go through Apollo documentations and search previous issues before creating an new one. If neither of the sources helped you with your issues, please report the issue using the following form. Please note missing info can delay the response time.

System information

Steps to reproduce the issue:

qwetqwe commented 3 years ago

you can add gflag --pointcloud_topic= YOUR POINT CLOUD TOPIC in https://github.com/ApolloAuto/apollo/blob/ecece0da431c82cd1b52129ec1b0bb58323331a2/modules/dreamview/conf/dreamview.conf#L1

TheGreatGalaxy commented 3 years ago

you can add gflag --pointcloud_topic= YOUR POINT CLOUD TOPIC in https://github.com/ApolloAuto/apollo/blob/ecece0da431c82cd1b52129ec1b0bb58323331a2/modules/dreamview/conf/dreamview.conf#L1

@qwetqwe If i have several lidars, how should i to configure:

  1. method 1 --pointcloud_topic= YOUR POINT CLOUD TOPIC_1 --pointcloud_topic= YOUR POINT CLOUD TOPIC_2
  2. method 2 --pointcloud_topic_1= YOUR POINT CLOUD TOPIC_1 --pointcloud_topic_2= YOUR POINT CLOUD TOPIC_2 And where is the code to subscribe those topics?
TheGreatGalaxy commented 3 years ago

@qwetqwe I wite a node and use the channel ""/apollo/sensor/lidar128/compensator/PointCloud2" which contained in a demo rec data。 I use cyber_record to play this data but black the channel. And use the node to send point cloud. But the dreamview still can't display point cloud. `#include "modules/drivers/proto/pointcloud.pb.h"

include "cyber/cyber.h"

include "cyber/time/rate.h"

include "cyber/time/time.h"

include "modules/common/adapters/adapter_gflags.h"

using apollo::cyber::Rate; using apollo::cyber::Time; using apollo::drivers::PointCloud; using apollo::drivers::PointXYZIT;

float GetRandomNoise() { std::srand((unsigned)std::time(NULL)); return std::rand() % 20 / 10.0; }

int main(int argc, char *argv[]) { google::ParseCommandLineFlags(&argc, &argv, true); FLAGS_alsologtostderr = true; // init cyber framework apollo::cyber::Init(argv[0]); // create talker node std::shared_ptr send_pointcloud_node( apollo::cyber::CreateNode("send_pointcloud_node222")); // create talker auto talker = send_pointcloud_node->CreateWriter( "/apollo/sensor/lidar128/compensator/PointCloud2"); Rate rate(10.0); int pnum = 20; static uint32_t seqnum = 1;

while (apollo::cyber::OK()) { std::shared_ptr pc_ptr(new PointCloud); pc_ptr->mutable_header()->set_timestamp_sec(Time::Now().ToSecond()); // pc_ptr->mutable_header()->set_module_name("SendPointCloudComponet"); pc_ptr->mutable_header()->set_sequence_num(seqnum++); pc_ptr->mutable_header()->set_lidar_timestamp(Time::Now().ToNanosecond()); pc_ptr->mutable_header()->set_version(1); pc_ptr->mutable_header()->mutable_status()->set_error_code( apollo::common::OK); pc_ptr->mutable_header()->set_frame_id("velodyne"); pc_ptr->set_frame_id("velodyne"); pc_ptr->set_is_dense(false); // pc_ptr->header.frame_id = ""; pc_ptr->mutable_point()->Reserve(pnum pnum pnum); for (int i = 0; i < pnum; ++i) { for (int j = 0; j < pnum; ++j) { for (int k = 0; k < pnum; ++k) { auto p = pc_ptr->add_point(); p->set_x(i + GetRandomNoise()); p->set_y(j + GetRandomNoise()); p->set_z(k + GetRandomNoise()); } } } pc_ptr->set_width( static_cast(pc_ptr->point_size())); AINFO << "size: " << pc_ptr->point_size() << " " << static_cast(pc_ptr->point_size()) << "\n"; pc_ptr->set_height(1); pc_ptr->set_measurement_time(pc_ptr->header().timestamp_sec() - 0.1); talker->Write(pc_ptr); AINFO << "talker sent a message!"; rate.Sleep(); } 16_2_pc.txt

return 0; }` This is the code. And i can see the send content got by _cyberchannel echo. it's same format as the lidar content from demo rec file. But mine can't display, the demo rec can display in Dreamview. Can you help me? How to debug this issue?

qwetqwe commented 3 years ago

you can add gflag --pointcloud_topic= YOUR POINT CLOUD TOPIC in https://github.com/ApolloAuto/apollo/blob/ecece0da431c82cd1b52129ec1b0bb58323331a2/modules/dreamview/conf/dreamview.conf#L1

@qwetqwe If i have several lidars, how should i to configure:

1. method 1
   --pointcloud_topic= YOUR POINT CLOUD TOPIC_1
   --pointcloud_topic= YOUR POINT CLOUD TOPIC_2

2. method 2
   --pointcloud_topic_1= YOUR POINT CLOUD TOPIC_1
   --pointcloud_topic_2= YOUR POINT CLOUD TOPIC_2
   And where is the code to subscribe those topics?

you need to fusion multi lidar pointclouds into one point cloud. dreamview can only show one topic see https://github.com/ApolloAuto/apollo/blob/ecece0da431c82cd1b52129ec1b0bb58323331a2/modules/drivers/lidar/velodyne/conf/velodyne_fusion_conf.pb.txt#L3-L7

TheGreatGalaxy commented 3 years ago

Find the reason of point cloud can't be displayed. Dreamview is using timestamp to filter point cloud. https://github.com/ApolloAuto/apollo/blob/ecece0da431c82cd1b52129ec1b0bb58323331a2/modules/dreamview/backend/point_cloud/point_cloud_updater.cc#L93.