Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
f4838009
A
apollo
项目概览
Pinoxchio
/
apollo
与 Fork 源项目一致
从无法访问的项目Fork
通知
2
Star
0
Fork
0
代码
文件
提交
分支
Tags
贡献者
分支图
Diff
Issue
0
列表
看板
标记
里程碑
合并请求
0
Wiki
0
Wiki
分析
仓库
DevOps
项目成员
Pages
A
apollo
项目概览
项目概览
详情
发布
仓库
仓库
文件
提交
分支
标签
贡献者
分支图
比较
Issue
0
Issue
0
列表
看板
标记
里程碑
合并请求
0
合并请求
0
Pages
分析
分析
仓库分析
DevOps
Wiki
0
Wiki
成员
成员
收起侧边栏
关闭侧边栏
动态
分支图
创建新Issue
提交
Issue看板
体验新版 GitCode,发现更多精彩内容 >>
提交
f4838009
编写于
2月 09, 2018
作者:
U
unacao
提交者:
Jiangtao Hu
2月 14, 2018
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
display point cloud in dreamview
上级
b432bb79
变更
26
展开全部
隐藏空白更改
内联
并排
Showing
26 changed file
with
444 addition
and
272 deletion
+444
-272
modules/dreamview/backend/dreamview.cc
modules/dreamview/backend/dreamview.cc
+2
-1
modules/dreamview/backend/point_cloud/BUILD
modules/dreamview/backend/point_cloud/BUILD
+1
-0
modules/dreamview/backend/point_cloud/point_cloud_updater.cc
modules/dreamview/backend/point_cloud/point_cloud_updater.cc
+57
-33
modules/dreamview/backend/point_cloud/point_cloud_updater.h
modules/dreamview/backend/point_cloud/point_cloud_updater.h
+8
-0
modules/dreamview/frontend/dist/6ffbb307e25c0d47616c.worker.js
...es/dreamview/frontend/dist/6ffbb307e25c0d47616c.worker.js
+2
-2
modules/dreamview/frontend/dist/6ffbb307e25c0d47616c.worker.js.map
...reamview/frontend/dist/6ffbb307e25c0d47616c.worker.js.map
+1
-1
modules/dreamview/frontend/dist/app.bundle.js
modules/dreamview/frontend/dist/app.bundle.js
+7
-7
modules/dreamview/frontend/dist/app.bundle.js.map
modules/dreamview/frontend/dist/app.bundle.js.map
+1
-1
modules/dreamview/frontend/gen_pbjs.sh
modules/dreamview/frontend/gen_pbjs.sh
+3
-0
modules/dreamview/frontend/proto_bundle/map_proto_bundle.json
...les/dreamview/frontend/proto_bundle/map_proto_bundle.json
+118
-130
modules/dreamview/frontend/proto_bundle/point_cloud_proto_bundle.json
...mview/frontend/proto_bundle/point_cloud_proto_bundle.json
+24
-0
modules/dreamview/frontend/proto_bundle/sim_world_proto_bundle.json
...eamview/frontend/proto_bundle/sim_world_proto_bundle.json
+1
-72
modules/dreamview/frontend/src/components/Dreamview.js
modules/dreamview/frontend/src/components/Dreamview.js
+2
-1
modules/dreamview/frontend/src/components/SideBar/Menu.js
modules/dreamview/frontend/src/components/SideBar/Menu.js
+6
-0
modules/dreamview/frontend/src/renderer/index.js
modules/dreamview/frontend/src/renderer/index.js
+15
-0
modules/dreamview/frontend/src/renderer/point_cloud.js
modules/dreamview/frontend/src/renderer/point_cloud.js
+91
-0
modules/dreamview/frontend/src/store/config/MenuData.js
modules/dreamview/frontend/src/store/config/MenuData.js
+1
-0
modules/dreamview/frontend/src/store/config/parameters.yml
modules/dreamview/frontend/src/store/config/parameters.yml
+1
-0
modules/dreamview/frontend/src/store/options.js
modules/dreamview/frontend/src/store/options.js
+1
-0
modules/dreamview/frontend/src/store/websocket/index.js
modules/dreamview/frontend/src/store/websocket/index.js
+7
-0
modules/dreamview/frontend/src/store/websocket/websocket_point_cloud.js
...iew/frontend/src/store/websocket/websocket_point_cloud.js
+62
-0
modules/dreamview/frontend/src/utils/webworker.js
modules/dreamview/frontend/src/utils/webworker.js
+14
-2
modules/dreamview/proto/BUILD
modules/dreamview/proto/BUILD
+12
-0
modules/dreamview/proto/point_cloud.proto
modules/dreamview/proto/point_cloud.proto
+7
-0
modules/tools/supervisord/dev.conf
modules/tools/supervisord/dev.conf
+0
-11
modules/tools/supervisord/release.conf
modules/tools/supervisord/release.conf
+0
-11
未找到文件。
modules/dreamview/backend/dreamview.cc
浏览文件 @
f4838009
...
...
@@ -84,6 +84,8 @@ Status Dreamview::Init() {
<<
"CompressedImageAdapter is not initialized."
;
CHECK
(
AdapterManager
::
GetImageShort
())
<<
"ImageShortAdapter is not initialized."
;
CHECK
(
AdapterManager
::
GetPointCloud
())
<<
"PointCloudAdapter is not initialized."
;
// Initialize and run the web server which serves the dreamview htmls and
// javascripts and handles websocket requests.
...
...
@@ -101,7 +103,6 @@ Status Dreamview::Init() {
image_
.
reset
(
new
ImageHandler
());
websocket_
.
reset
(
new
WebSocketHandler
());
<<<<<<<
HEAD
map_ws_
.
reset
(
new
WebSocketHandler
());
point_cloud_ws_
.
reset
(
new
WebSocketHandler
());
map_service_
.
reset
(
new
MapService
());
...
...
modules/dreamview/backend/point_cloud/BUILD
浏览文件 @
f4838009
...
...
@@ -18,6 +18,7 @@ cc_library(
"//modules/common/adapters:adapter_manager"
,
"//modules/dreamview/backend/common:dreamview_gflags"
,
"//modules/dreamview/backend/handlers:websocket"
,
"//modules/dreamview/proto:point_cloud_proto"
,
"//third_party/json"
,
"@com_google_protobuf//:protobuf"
,
"@pcl//:pcl"
,
...
...
modules/dreamview/backend/point_cloud/point_cloud_updater.cc
浏览文件 @
f4838009
...
...
@@ -18,6 +18,7 @@
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/log.h"
#include "modules/common/time/time.h"
#include "modules/dreamview/backend/common/dreamview_gflags.h"
#include "pcl_conversions/pcl_conversions.h"
#include "pcl/point_cloud.h"
...
...
@@ -28,26 +29,61 @@ namespace apollo {
namespace
dreamview
{
using
apollo
::
common
::
adapter
::
AdapterManager
;
using
apollo
::
common
::
time
::
Clock
;
using
sensor_msgs
::
PointCloud2
;
using
Json
=
nlohmann
::
json
;
constexpr
int
PointCloudUpdater
::
kDownsampleRate
;
PointCloudUpdater
::
PointCloudUpdater
(
WebSocketHandler
*
websocket
)
:
websocket_
(
websocket
)
{
RegisterMessageHandlers
();
}
void
PointCloudUpdater
::
RegisterMessageHandlers
()
{
// Send current point_cloud status to the new client.
websocket_
->
RegisterConnectionReadyHandler
(
[
this
](
WebSocketHandler
::
Connection
*
conn
)
{
Json
response
;
response
[
"type"
]
=
"PointCloudStatus"
;
response
[
"enabled"
]
=
enabled_
;
websocket_
->
SendData
(
conn
,
response
.
dump
());
});
websocket_
->
RegisterMessageHandler
(
"RequestPointCloud"
,
[
this
](
const
Json
&
json
,
WebSocketHandler
::
Connection
*
conn
)
{
std
::
string
to_send
;
// If there is no point_cloud data for more than 2 seconds, reset.
if
((
point_cloud_
.
num_size
()
>
0
)
&&
(
Clock
::
NowInSeconds
()
-
last_receive_time_
>
2.0
))
{
point_cloud_
.
clear_num
();
boost
::
unique_lock
<
boost
::
shared_mutex
>
writer_lock
(
mutex_
);
point_cloud_
.
SerializeToString
(
&
point_cloud_str_
);
}
{
// Pay the price to copy the data instead of sending data over the
// wire while holding the lock.
boost
::
shared_lock
<
boost
::
shared_mutex
>
lock
(
mutex_
);
boost
::
shared_lock
<
boost
::
shared_mutex
>
reader_lock
(
mutex_
);
to_send
=
point_cloud_str_
;
}
websocket_
->
SendData
(
conn
,
to_send
,
true
);
websocket_
->
SendBinaryData
(
conn
,
to_send
,
true
);;
});
websocket_
->
RegisterMessageHandler
(
"TogglePointCloud"
,
[
this
](
const
Json
&
json
,
WebSocketHandler
::
Connection
*
conn
)
{
auto
enable
=
json
.
find
(
"enable"
);
if
(
enable
!=
json
.
end
()
&&
enable
->
is_boolean
())
{
if
(
*
enable
)
{
enabled_
=
true
;
}
else
{
enabled_
=
false
;
}
if
(
websocket_
)
{
Json
response
;
response
[
"type"
]
=
"PointCloudStatus"
;
response
[
"enabled"
]
=
enabled_
;
// Sync the point_cloud status across all the clients.
websocket_
->
BroadcastData
(
response
.
dump
());
}
}
});
}
...
...
@@ -57,42 +93,30 @@ void PointCloudUpdater::Start() {
}
void
PointCloudUpdater
::
UpdatePointCloud
(
const
PointCloud2
&
point_cloud
)
{
// transform from ros to pcl
pcl
::
PointCloud
<
pcl
::
PointXYZ
>
pcl_data
;
pcl
::
fromROSMsg
(
point_cloud
,
pcl_data
);
if
(
pcl_data
.
size
()
==
0
)
{
point_cloud_str_
=
"[]"
;
if
(
!
enabled_
)
{
return
;
}
std
::
stringstream
stream
;
std
::
string
data
(
"["
);
for
(
size_t
idx
=
0
;
idx
<
pcl_data
.
size
();
idx
+=
2
)
{
last_receive_time_
=
Clock
::
NowInSeconds
();
// transform from ros to pcl
pcl
::
PointCloud
<
pcl
::
PointXYZ
>
pcl_data
;
pcl
::
fromROSMsg
(
point_cloud
,
pcl_data
);
point_cloud_
.
Clear
();
for
(
size_t
idx
=
0
;
idx
<
pcl_data
.
size
();
idx
+=
kDownsampleRate
)
{
pcl
::
PointXYZ
&
pt
=
pcl_data
.
points
[
idx
];
if
(
!
isnan
(
pt
.
x
)
&&
!
isnan
(
pt
.
y
)
&&
!
isnan
(
pt
.
z
))
{
data
.
append
(
"["
);
stream
.
str
(
std
::
string
());
stream
<<
std
::
fixed
<<
std
::
setprecision
(
4
)
<<
pt
.
x
;
data
.
append
(
stream
.
str
());
data
.
append
(
","
);
stream
.
str
(
std
::
string
());
stream
<<
std
::
fixed
<<
std
::
setprecision
(
4
)
<<
pt
.
y
;
data
.
append
(
stream
.
str
());
data
.
append
(
","
);
stream
.
str
(
std
::
string
());
stream
<<
std
::
fixed
<<
std
::
setprecision
(
4
)
<<
pt
.
z
;
data
.
append
(
stream
.
str
());
data
.
append
(
"]"
);
if
((
idx
+
1
)
==
pcl_data
.
size
())
{
data
.
append
(
"]"
);
}
else
{
data
.
append
(
","
);
}
point_cloud_
.
add_num
(
pt
.
x
);
point_cloud_
.
add_num
(
pt
.
y
);
// TODO(unacao): velodyne height should be updated by hmi store
// upon vehicle change.
point_cloud_
.
add_num
(
pt
.
z
+
1.91
);
}
}
boost
::
shared_lock
<
boost
::
shared_mutex
>
lock
(
mutex_
);
point_cloud_str_
=
data
;
{
boost
::
unique_lock
<
boost
::
shared_mutex
>
writer_lock
(
mutex_
);
point_cloud_
.
SerializeToString
(
&
point_cloud_str_
);
}
}
}
// namespace dreamview
...
...
modules/dreamview/backend/point_cloud/point_cloud_updater.h
浏览文件 @
f4838009
...
...
@@ -29,6 +29,7 @@
#include "modules/common/log.h"
#include "modules/common/util/string_util.h"
#include "modules/dreamview/backend/handlers/websocket.h"
#include "modules/dreamview/proto/point_cloud.pb.h"
#include "sensor_msgs/PointCloud2.h"
/**
...
...
@@ -58,18 +59,25 @@ class PointCloudUpdater {
void
Start
();
private:
static
constexpr
int
kDownsampleRate
=
12
;
void
RegisterMessageHandlers
();
void
UpdatePointCloud
(
const
sensor_msgs
::
PointCloud2
&
point_cloud
);
WebSocketHandler
*
websocket_
;
bool
enabled_
=
false
;
// The PointCloud to be pushed to frontend.
std
::
string
point_cloud_str_
;
PointCloud
point_cloud_
;
// Mutex to protect concurrent access to point_cloud_str_.
// NOTE: Use boost until we have std version of rwlock support.
boost
::
shared_mutex
mutex_
;
double
last_receive_time_
=
0.0
;
};
}
// namespace dreamview
...
...
modules/dreamview/frontend/dist/
ce73b7d27f3389d3ec58
.worker.js
→
modules/dreamview/frontend/dist/
6ffbb307e25c0d47616c
.worker.js
浏览文件 @
f4838009
此差异已折叠。
点击以展开。
modules/dreamview/frontend/dist/
ce73b7d27f3389d3ec58
.worker.js.map
→
modules/dreamview/frontend/dist/
6ffbb307e25c0d47616c
.worker.js.map
浏览文件 @
f4838009
此差异已折叠。
点击以展开。
modules/dreamview/frontend/dist/app.bundle.js
浏览文件 @
f4838009
此差异已折叠。
点击以展开。
modules/dreamview/frontend/dist/app.bundle.js.map
浏览文件 @
f4838009
此差异已折叠。
点击以展开。
modules/dreamview/frontend/gen_pbjs.sh
浏览文件 @
f4838009
...
...
@@ -38,3 +38,6 @@ node_modules/protobufjs/bin/pbjs -t json $MAP_PROTOS \
../../common/proto/header.proto
\
../../common/proto/error_code.proto
\
-o
proto_bundle/map_proto_bundle.json
node_modules/protobufjs/bin/pbjs
-t
json ../proto/point_cloud.proto
\
-o
proto_bundle/point_cloud_proto_bundle.json
modules/dreamview/frontend/proto_bundle/map_proto_bundle.json
浏览文件 @
f4838009
...
...
@@ -4,6 +4,124 @@
"nested"
:
{
"hdmap"
:
{
"nested"
:
{
"Projection"
:
{
"fields"
:
{
"proj"
:
{
"type"
:
"string"
,
"id"
:
1
}
}
},
"Header"
:
{
"fields"
:
{
"version"
:
{
"type"
:
"bytes"
,
"id"
:
1
},
"date"
:
{
"type"
:
"bytes"
,
"id"
:
2
},
"projection"
:
{
"type"
:
"Projection"
,
"id"
:
3
},
"district"
:
{
"type"
:
"bytes"
,
"id"
:
4
},
"generation"
:
{
"type"
:
"bytes"
,
"id"
:
5
},
"revMajor"
:
{
"type"
:
"bytes"
,
"id"
:
6
},
"revMinor"
:
{
"type"
:
"bytes"
,
"id"
:
7
},
"left"
:
{
"type"
:
"double"
,
"id"
:
8
},
"top"
:
{
"type"
:
"double"
,
"id"
:
9
},
"right"
:
{
"type"
:
"double"
,
"id"
:
10
},
"bottom"
:
{
"type"
:
"double"
,
"id"
:
11
},
"vendor"
:
{
"type"
:
"bytes"
,
"id"
:
12
}
}
},
"Map"
:
{
"fields"
:
{
"header"
:
{
"type"
:
"Header"
,
"id"
:
1
},
"crosswalk"
:
{
"rule"
:
"repeated"
,
"type"
:
"Crosswalk"
,
"id"
:
2
},
"junction"
:
{
"rule"
:
"repeated"
,
"type"
:
"Junction"
,
"id"
:
3
},
"lane"
:
{
"rule"
:
"repeated"
,
"type"
:
"Lane"
,
"id"
:
4
},
"stopSign"
:
{
"rule"
:
"repeated"
,
"type"
:
"StopSign"
,
"id"
:
5
},
"signal"
:
{
"rule"
:
"repeated"
,
"type"
:
"Signal"
,
"id"
:
6
},
"yield"
:
{
"rule"
:
"repeated"
,
"type"
:
"YieldSign"
,
"id"
:
7
},
"overlap"
:
{
"rule"
:
"repeated"
,
"type"
:
"Overlap"
,
"id"
:
8
},
"clearArea"
:
{
"rule"
:
"repeated"
,
"type"
:
"ClearArea"
,
"id"
:
9
},
"speedBump"
:
{
"rule"
:
"repeated"
,
"type"
:
"SpeedBump"
,
"id"
:
10
},
"road"
:
{
"rule"
:
"repeated"
,
"type"
:
"Road"
,
"id"
:
11
}
}
},
"ClearArea"
:
{
"fields"
:
{
"id"
:
{
...
...
@@ -308,18 +426,6 @@
}
}
},
"MapMsg"
:
{
"fields"
:
{
"header"
:
{
"type"
:
"apollo.common.Header"
,
"id"
:
1
},
"map"
:
{
"type"
:
"apollo.hdmap.Map"
,
"id"
:
3
}
}
},
"LaneOverlapInfo"
:
{
"fields"
:
{
"startS"
:
{
...
...
@@ -432,124 +538,6 @@
}
}
},
"Projection"
:
{
"fields"
:
{
"proj"
:
{
"type"
:
"string"
,
"id"
:
1
}
}
},
"Header"
:
{
"fields"
:
{
"version"
:
{
"type"
:
"bytes"
,
"id"
:
1
},
"date"
:
{
"type"
:
"bytes"
,
"id"
:
2
},
"projection"
:
{
"type"
:
"Projection"
,
"id"
:
3
},
"district"
:
{
"type"
:
"bytes"
,
"id"
:
4
},
"generation"
:
{
"type"
:
"bytes"
,
"id"
:
5
},
"revMajor"
:
{
"type"
:
"bytes"
,
"id"
:
6
},
"revMinor"
:
{
"type"
:
"bytes"
,
"id"
:
7
},
"left"
:
{
"type"
:
"double"
,
"id"
:
8
},
"top"
:
{
"type"
:
"double"
,
"id"
:
9
},
"right"
:
{
"type"
:
"double"
,
"id"
:
10
},
"bottom"
:
{
"type"
:
"double"
,
"id"
:
11
},
"vendor"
:
{
"type"
:
"bytes"
,
"id"
:
12
}
}
},
"Map"
:
{
"fields"
:
{
"header"
:
{
"type"
:
"Header"
,
"id"
:
1
},
"crosswalk"
:
{
"rule"
:
"repeated"
,
"type"
:
"Crosswalk"
,
"id"
:
2
},
"junction"
:
{
"rule"
:
"repeated"
,
"type"
:
"Junction"
,
"id"
:
3
},
"lane"
:
{
"rule"
:
"repeated"
,
"type"
:
"Lane"
,
"id"
:
4
},
"stopSign"
:
{
"rule"
:
"repeated"
,
"type"
:
"StopSign"
,
"id"
:
5
},
"signal"
:
{
"rule"
:
"repeated"
,
"type"
:
"Signal"
,
"id"
:
6
},
"yield"
:
{
"rule"
:
"repeated"
,
"type"
:
"YieldSign"
,
"id"
:
7
},
"overlap"
:
{
"rule"
:
"repeated"
,
"type"
:
"Overlap"
,
"id"
:
8
},
"clearArea"
:
{
"rule"
:
"repeated"
,
"type"
:
"ClearArea"
,
"id"
:
9
},
"speedBump"
:
{
"rule"
:
"repeated"
,
"type"
:
"SpeedBump"
,
"id"
:
10
},
"road"
:
{
"rule"
:
"repeated"
,
"type"
:
"Road"
,
"id"
:
11
}
}
},
"BoundaryEdge"
:
{
"fields"
:
{
"curve"
:
{
...
...
modules/dreamview/frontend/proto_bundle/point_cloud_proto_bundle.json
0 → 100644
浏览文件 @
f4838009
{
"nested"
:
{
"apollo"
:
{
"nested"
:
{
"dreamview"
:
{
"nested"
:
{
"PointCloud"
:
{
"fields"
:
{
"num"
:
{
"rule"
:
"repeated"
,
"type"
:
"float"
,
"id"
:
1
,
"options"
:
{
"packed"
:
false
}
}
}
}
}
}
}
}
}
}
\ No newline at end of file
modules/dreamview/frontend/proto_bundle/sim_world_proto_bundle.json
浏览文件 @
f4838009
...
...
@@ -2466,83 +2466,12 @@
}
}
},
"CloudReferenceLinePoint"
:
{
"fields"
:
{
"x"
:
{
"type"
:
"double"
,
"id"
:
1
},
"y"
:
{
"type"
:
"double"
,
"id"
:
2
},
"z"
:
{
"type"
:
"double"
,
"id"
:
3
},
"theta"
:
{
"type"
:
"double"
,
"id"
:
4
},
"kappa"
:
{
"type"
:
"double"
,
"id"
:
5
},
"laneS"
:
{
"type"
:
"double"
,
"id"
:
6
},
"dkappa"
:
{
"type"
:
"double"
,
"id"
:
7
}
}
},
"CloudReferenceLineSegment"
:
{
"fields"
:
{
"laneId"
:
{
"type"
:
"string"
,
"id"
:
1
},
"point"
:
{
"rule"
:
"repeated"
,
"type"
:
"CloudReferenceLinePoint"
,
"id"
:
2
}
}
},
"CloudReferenceLineResponse"
:
{
"fields"
:
{
"segment"
:
{
"rule"
:
"repeated"
,
"type"
:
"CloudReferenceLineSegment"
,
"id"
:
1
}
}
},
"NavigationPath"
:
{
"fields"
:
{
"pathPoint"
:
{
"rule"
:
"repeated"
,
"type"
:
"apollo.common.PathPoint"
,
"id"
:
1
},
"pathPriority"
:
{
"type"
:
"uint32"
,
"id"
:
2
}
}
},
"NavigationInfo"
:
{
"fields"
:
{
"header"
:
{
"type"
:
"apollo.common.Header"
,
"type"
:
"apollo.common.Path"
,
"id"
:
1
},
"navigationPath"
:
{
"rule"
:
"repeated"
,
"type"
:
"NavigationPath"
,
"id"
:
2
}
}
}
...
...
modules/dreamview/frontend/src/components/Dreamview.js
浏览文件 @
f4838009
...
...
@@ -7,7 +7,7 @@ import MainView from "components/Layouts/MainView";
import
ToolView
from
"
components/Layouts/ToolView
"
;
import
PNCMonitor
from
"
components/PNCMonitor
"
;
import
SideBar
from
"
components/SideBar
"
;
import
WS
,
{
MAP_WS
}
from
"
store/websocket
"
;
import
WS
,
{
MAP_WS
,
POINT_CLOUD_WS
}
from
"
store/websocket
"
;
@
inject
(
"
store
"
)
@
observer
...
...
@@ -32,6 +32,7 @@ export default class Dreamview extends React.Component {
componentDidMount
()
{
WS
.
initialize
();
MAP_WS
.
initialize
();
POINT_CLOUD_WS
.
initialize
();
window
.
addEventListener
(
"
resize
"
,
()
=>
{
this
.
props
.
store
.
updateDimension
();
});
...
...
modules/dreamview/frontend/src/components/SideBar/Menu.js
浏览文件 @
f4838009
...
...
@@ -12,6 +12,8 @@ import decisionIcon from "assets/images/menu/Decision.png";
import
planningIcon
from
"
assets/images/menu/Planning.png
"
;
import
cameraIcon
from
"
assets/images/menu/PointOfView.png
"
;
import
{
POINT_CLOUD_WS
}
from
"
store/websocket
"
;
const
MenuIconMapping
=
{
perception
:
perceptionIcon
,
prediction
:
predictionIcon
,
...
...
@@ -22,6 +24,7 @@ const MenuIconMapping = {
};
const
MenuIdOptionMapping
=
{
perceptionPointCloud
:
'
showPointCloud
'
,
perceptionVehicle
:
'
showObstaclesVehicle
'
,
perceptionPedestrian
:
'
showObstaclesPedestrian
'
,
perceptionBicycle
:
'
showObstaclesBicycle
'
,
...
...
@@ -54,6 +57,9 @@ class MenuItemCheckbox extends React.Component {
<
ul
>
<
li
id
=
{
id
}
onClick
=
{()
=>
{
options
.
toggle
(
MenuIdOptionMapping
[
id
]);
if
(
id
===
"
perceptionPointCloud
"
)
{
POINT_CLOUD_WS
.
togglePointCloud
(
options
.
showPointCloud
);
}
}}
>
<
div
className
=
"
switch
"
>
<
input
type
=
"
checkbox
"
name
=
{
id
}
className
=
"
toggle-switch
"
...
...
modules/dreamview/frontend/src/renderer/index.js
浏览文件 @
f4838009
...
...
@@ -15,6 +15,7 @@ import Prediction from "renderer/prediction.js";
import
Routing
from
"
renderer/routing.js
"
;
import
RoutingEditor
from
"
renderer/routing_editor.js
"
;
import
Gnss
from
"
renderer/gnss.js
"
;
import
PointCloud
from
"
renderer/point_cloud.js
"
;
const
_
=
require
(
'
lodash
'
);
...
...
@@ -70,6 +71,8 @@ class Renderer {
// The GNSS/GPS
this
.
gnss
=
new
Gnss
();
this
.
pointCloud
=
new
PointCloud
();
// The Performance Monitor
this
.
stats
=
null
;
if
(
PARAMETERS
.
debug
.
performanceMonitor
)
{
...
...
@@ -310,6 +313,11 @@ class Renderer {
this
.
scene
.
add
(
this
.
ground
.
mesh
);
}
if
(
this
.
pointCloud
.
initialized
===
false
)
{
this
.
pointCloud
.
initialize
();
this
.
scene
.
add
(
this
.
pointCloud
.
points
);
}
this
.
adjustCamera
(
this
.
adc
.
mesh
,
this
.
options
.
cameraAngle
);
this
.
renderer
.
render
(
this
.
scene
,
this
.
camera
);
}
...
...
@@ -357,6 +365,13 @@ class Renderer {
this
.
map
.
appendMapData
(
newData
,
this
.
coordinates
,
this
.
scene
);
}
updatePointCloud
(
pointCloud
)
{
if
(
!
this
.
coordinates
.
isInitialized
()
||
!
this
.
adc
.
mesh
)
{
return
;
}
this
.
pointCloud
.
update
(
pointCloud
,
this
.
adc
.
mesh
);
}
updateMapIndex
(
hash
,
elementIds
,
radius
)
{
if
(
!
this
.
routingEditor
.
isInEditingMode
()
||
this
.
routingEditor
.
EDITING_MAP_RADIUS
===
radius
)
{
...
...
modules/dreamview/frontend/src/renderer/point_cloud.js
0 → 100644
浏览文件 @
f4838009
import
*
as
THREE
from
"
three
"
;
const
MAX_POINTS
=
8400
;
const
HEIGHT_COLOR_MAPPING
=
{
0.5
:
0xFF0000
,
1.0
:
0xFF7F00
,
1.5
:
0xFFFF00
,
2.0
:
0x00FF00
,
2.5
:
0x0000FF
,
3.0
:
0x4B0082
,
10.0
:
0x9400D3
,
};
export
default
class
PointCloud
{
constructor
()
{
this
.
points
=
null
;
this
.
initialized
=
false
;
}
initialize
()
{
this
.
points
=
this
.
createPointCloud
(
HEIGHT_COLOR_MAPPING
[
0.5
]);
this
.
initialized
=
true
;
}
createPointCloud
(
hex_color
)
{
const
geometry
=
new
THREE
.
Geometry
();
const
colors
=
[];
for
(
let
i
=
0
;
i
<
MAX_POINTS
;
++
i
)
{
const
vertex
=
new
THREE
.
Vector3
();
vertex
.
set
(
0
,
0
,
-
10
);
geometry
.
vertices
.
push
(
vertex
);
colors
[
i
]
=
new
THREE
.
Color
(
hex_color
);
}
geometry
.
colors
=
colors
;
const
material
=
new
THREE
.
PointsMaterial
({
size
:
0.1
,
transparent
:
true
,
opacity
:
0.7
,
vertexColors
:
THREE
.
VertexColors
});
const
points
=
new
THREE
.
Points
(
geometry
,
material
);
points
.
frustumCulled
=
false
;
return
points
;
}
update
(
pointCloud
,
adcMesh
)
{
if
(
this
.
points
===
null
)
{
return
;
}
if
(
pointCloud
.
num
.
length
%
3
!==
0
)
{
console
.
warn
(
'
PointCloud length should be multiples of 3!
'
);
return
;
}
const
pointCloudSize
=
pointCloud
.
num
.
length
/
3
;
const
total
=
(
pointCloudSize
<
MAX_POINTS
)
?
pointCloudSize
:
MAX_POINTS
;
let
color_key
=
0.5
;
for
(
let
i
=
0
;
i
<
total
;
i
++
)
{
const
x
=
pointCloud
.
num
[
i
*
3
];
const
y
=
pointCloud
.
num
[
i
*
3
+
1
];
const
z
=
pointCloud
.
num
[
i
*
3
+
2
];
this
.
points
.
geometry
.
vertices
[
i
].
set
(
x
,
y
,
z
+
0.8
);
// Update color based on height.
if
(
z
<
0.5
)
{
color_key
=
0.5
;
}
else
if
(
z
<
1.0
)
{
color_key
=
1.0
;
}
else
if
(
z
<
1.5
)
{
color_key
=
1.5
;
}
else
if
(
z
<
2.0
)
{
color_key
=
2.0
;
}
else
if
(
z
<
2.5
)
{
color_key
=
2.5
;
}
else
if
(
z
<
3.0
)
{
color_key
=
3.0
;
}
else
{
color_key
=
10.0
;
}
this
.
points
.
geometry
.
colors
[
i
].
setHex
(
HEIGHT_COLOR_MAPPING
[
color_key
]);
}
// Hide unused points.
for
(
let
i
=
total
;
i
<
MAX_POINTS
;
++
i
)
{
this
.
points
.
geometry
.
vertices
[
i
].
set
(
0
,
0
,
-
10
);
}
this
.
points
.
geometry
.
verticesNeedUpdate
=
true
;
this
.
points
.
geometry
.
colorsNeedUpdate
=
true
;
this
.
points
.
position
.
set
(
adcMesh
.
position
.
x
,
adcMesh
.
position
.
y
,
adcMesh
.
position
.
z
);
this
.
points
.
rotation
.
set
(
0
,
0
,
adcMesh
.
rotation
.
y
);
}
}
modules/dreamview/frontend/src/store/config/MenuData.js
浏览文件 @
f4838009
...
...
@@ -15,6 +15,7 @@ export default [
title
:
'
Perception
'
,
type
:
'
checkbox
'
,
data
:
{
perceptionPointCloud
:
'
Point Cloud
'
,
perceptionVehicle
:
'
Vehicle
'
,
perceptionPedestrian
:
'
Pedestrian
'
,
perceptionBicycle
:
'
Bicycle
'
,
...
...
modules/dreamview/frontend/src/store/config/parameters.yml
浏览文件 @
f4838009
...
...
@@ -71,6 +71,7 @@ options:
showObstaclesVelocity
:
true
showObstaclesHeading
:
true
showObstaclesId
:
true
showPointCloud
:
true
showPositionGps
:
false
showPositionLocalization
:
true
cameraAngle
:
Default
...
...
modules/dreamview/frontend/src/store/options.js
浏览文件 @
f4838009
...
...
@@ -45,6 +45,7 @@ export default class Options {
PARAMETERS
.
options
.
defaults
.
showObstaclesHeading
;
@
observable
showObstaclesId
=
PARAMETERS
.
options
.
defaults
.
showObstaclesId
;
@
observable
showPointCloud
=
PARAMETERS
.
options
.
defaults
.
showPointCloud
;
@
observable
showPositionGps
=
PARAMETERS
.
options
.
defaults
.
showPositionGps
;
@
observable
showPositionLocalization
=
PARAMETERS
.
options
.
defaults
.
showPositionLocalization
;
...
...
modules/dreamview/frontend/src/store/websocket/index.js
浏览文件 @
f4838009
...
...
@@ -3,6 +3,7 @@ import PARAMETERS from "store/config/parameters.yml";
import
OfflinePlaybackWebSocketEndpoint
from
"
store/websocket/websocket_offline
"
;
import
RealtimeWebSocketEndpoint
from
"
store/websocket/websocket_realtime
"
;
import
MapDataWebSocketEndpoint
from
"
store/websocket/websocket_map
"
;
import
PointCloudWebSocketEndpoint
from
"
store/websocket/websocket_point_cloud
"
;
// Returns the websocket server address based on the web server address.
// Follows the convention that the websocket is served on the same host/port
...
...
@@ -18,6 +19,9 @@ function deduceWebsocketServerAddr(type) {
case
"
map
"
:
path
=
"
map
"
;
break
;
case
"
point_cloud
"
:
path
=
"
pointcloud
"
;
break
;
case
"
sim_world
"
:
path
=
OFFLINE_PLAYBACK
?
"
RosPlayBack
"
:
"
websocket
"
;
break
;
...
...
@@ -36,3 +40,6 @@ export default WS;
const
mapServerAddr
=
deduceWebsocketServerAddr
(
"
map
"
);
export
const
MAP_WS
=
new
MapDataWebSocketEndpoint
(
mapServerAddr
);
const
pointCloudServerAddr
=
deduceWebsocketServerAddr
(
"
point_cloud
"
);
export
const
POINT_CLOUD_WS
=
new
PointCloudWebSocketEndpoint
(
pointCloudServerAddr
);
modules/dreamview/frontend/src/store/websocket/websocket_point_cloud.js
0 → 100644
浏览文件 @
f4838009
import
STORE
from
"
store
"
;
import
RENDERER
from
"
renderer
"
;
const
Worker
=
require
(
'
worker-loader!utils/webworker.js
'
);
export
default
class
PointCloudWebSocketEndpoint
{
constructor
(
serverAddr
)
{
this
.
serverAddr
=
serverAddr
;
this
.
websocket
=
null
;
this
.
worker
=
new
Worker
();
}
initialize
()
{
try
{
this
.
websocket
=
new
WebSocket
(
this
.
serverAddr
);
this
.
websocket
.
binaryType
=
"
arraybuffer
"
;
}
catch
(
error
)
{
console
.
error
(
"
Failed to establish a connection:
"
+
error
);
setTimeout
(()
=>
{
this
.
initialize
();
},
1000
);
return
;
}
this
.
websocket
.
onmessage
=
event
=>
{
this
.
worker
.
postMessage
({
source
:
'
point_cloud
'
,
data
:
event
.
data
,
});
};
this
.
websocket
.
onclose
=
event
=>
{
console
.
log
(
"
WebSocket connection closed with code:
"
+
event
.
code
);
this
.
initialize
();
};
this
.
worker
.
onmessage
=
event
=>
{
if
(
event
.
data
.
type
===
"
PointCloudStatus
"
)
{
STORE
.
setOptionStatus
(
'
showPointCloud
'
,
event
.
data
.
enabled
);
}
else
if
(
STORE
.
options
.
showPointCloud
===
true
&&
event
.
data
.
num
!==
undefined
)
{
RENDERER
.
updatePointCloud
(
event
.
data
);
}
};
// Request point cloud every 100ms.
clearInterval
(
this
.
timer
);
this
.
timer
=
setInterval
(()
=>
{
if
(
this
.
websocket
.
readyState
===
this
.
websocket
.
OPEN
)
{
if
(
STORE
.
options
.
showPointCloud
===
true
)
{
this
.
websocket
.
send
(
JSON
.
stringify
({
type
:
"
RequestPointCloud
"
}));
}
else
{
RENDERER
.
updatePointCloud
({
num
:[]});
}
}
},
100
);
}
togglePointCloud
(
enable
)
{
this
.
websocket
.
send
(
JSON
.
stringify
({
type
:
"
TogglePointCloud
"
,
enable
:
enable
,
}));
}
}
\ No newline at end of file
modules/dreamview/frontend/src/utils/webworker.js
浏览文件 @
f4838009
const
protobuf
=
require
(
"
protobufjs/light
"
);
const
simWorldRoot
=
protobuf
.
Root
.
fromJSON
(
require
(
"
../../proto_bundle/sim_world_proto_bundle.json
"
)
require
(
"
../../proto_bundle/sim_world_proto_bundle.json
"
)
);
const
SimWorldMessage
=
simWorldRoot
.
lookupType
(
"
apollo.dreamview.SimulationWorld
"
);
const
mapRoot
=
protobuf
.
Root
.
fromJSON
(
require
(
"
../../proto_bundle/map_proto_bundle.json
"
));
const
mapMessage
=
mapRoot
.
lookupType
(
"
apollo.hdmap.Map
"
);
const
pointCloudRoot
=
protobuf
.
Root
.
fromJSON
(
require
(
"
../../proto_bundle/point_cloud_proto_bundle.json
"
)
);
const
pointCloudMessage
=
pointCloudRoot
.
lookupType
(
"
apollo.dreamview.PointCloud
"
);
self
.
addEventListener
(
"
message
"
,
event
=>
{
let
message
=
null
;
...
...
@@ -26,9 +30,17 @@ self.addEventListener("message", event => {
{
enums
:
String
});
message
.
type
=
"
MapData
"
;
break
;
case
"
point_cloud
"
:
if
(
typeof
data
===
"
string
"
)
{
message
=
JSON
.
parse
(
data
);
}
else
{
message
=
pointCloudMessage
.
toObject
(
pointCloudMessage
.
decode
(
new
Uint8Array
(
data
)),
{
arrays
:
true
});
}
break
;
}
if
(
message
)
{
self
.
postMessage
(
message
);
self
.
postMessage
(
message
);
}
});
modules/dreamview/proto/BUILD
浏览文件 @
f4838009
...
...
@@ -16,6 +16,18 @@ proto_library(
],
)
cc_proto_library
(
name
=
"point_cloud_proto"
,
deps
=
[
":point_cloud_proto_lib"
,
],
)
proto_library
(
name
=
"point_cloud_proto_lib"
,
srcs
=
[
"point_cloud.proto"
],
)
cc_proto_library
(
name
=
"hmi_config_proto"
,
deps
=
[
...
...
modules/dreamview/proto/point_cloud.proto
0 → 100644
浏览文件 @
f4838009
syntax
=
"proto2"
;
package
apollo
.
dreamview
;
message
PointCloud
{
repeated
float
num
=
1
;
}
modules/tools/supervisord/dev.conf
浏览文件 @
f4838009
...
...
@@ -115,17 +115,6 @@ autorestart=unexpected
redirect_stderr
=
true
stdout_logfile
=/
apollo
/
data
/
log
/
dreamview
.
out
[
program
:
sim_control
]
command
=/
apollo
/
bazel
-
bin
/
modules
/
dreamview
/
dreamview
--
flagfile
=/
apollo
/
modules
/
dreamview
/
conf
/
dreamview
.
conf
--
enable_sim_control
--
log_dir
=/
apollo
/
data
/
log
autostart
=
false
numprocs
=
1
exitcodes
=
0
stopsignal
=
SIGINT
startretries
=
10
autorestart
=
unexpected
redirect_stderr
=
true
stdout_logfile
=/
apollo
/
data
/
log
/
dreamview
.
out
[
program
:
monitor
]
command
=/
apollo
/
bazel
-
bin
/
modules
/
monitor
/
monitor
--
flagfile
=/
apollo
/
modules
/
monitor
/
conf
/
monitor
.
conf
--
log_dir
=/
apollo
/
data
/
log
autostart
=
false
...
...
modules/tools/supervisord/release.conf
浏览文件 @
f4838009
...
...
@@ -115,17 +115,6 @@ autorestart=unexpected
redirect_stderr
=
true
stdout_logfile
=/
apollo
/
data
/
log
/
dreamview
.
out
[
program
:
sim_control
]
command
=/
apollo
/
modules
/
dreamview
/
dreamview
--
flagfile
=/
apollo
/
modules
/
dreamview
/
conf
/
dreamview
.
conf
--
enable_sim_control
--
log_dir
=/
apollo
/
data
/
log
autostart
=
false
numprocs
=
1
exitcodes
=
0
stopsignal
=
SIGINT
startretries
=
10
autorestart
=
unexpected
redirect_stderr
=
true
stdout_logfile
=/
apollo
/
data
/
log
/
dreamview
.
out
[
program
:
monitor
]
command
=/
apollo
/
modules
/
monitor
/
monitor
--
flagfile
=/
apollo
/
modules
/
monitor
/
conf
/
monitor
.
conf
--
log_dir
=/
apollo
/
data
/
log
autostart
=
false
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录