Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
009038e5
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,发现更多精彩内容 >>
提交
009038e5
编写于
7月 24, 2017
作者:
Z
Zhang Liangliang
提交者:
Jiangtao Hu
7月 25, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
added trajectory stitcher
上级
f25484e9
变更
5
隐藏空白更改
内联
并排
Showing
5 changed file
with
192 addition
and
0 deletion
+192
-0
modules/common/vehicle_state/vehicle_state.h
modules/common/vehicle_state/vehicle_state.h
+6
-0
modules/planning/common/trajectory/discretized_trajectory.h
modules/planning/common/trajectory/discretized_trajectory.h
+4
-0
modules/planning/trajectory_stitcher/BUILD
modules/planning/trajectory_stitcher/BUILD
+22
-0
modules/planning/trajectory_stitcher/trajectory_stitcher.cc
modules/planning/trajectory_stitcher/trajectory_stitcher.cc
+116
-0
modules/planning/trajectory_stitcher/trajectory_stitcher.h
modules/planning/trajectory_stitcher/trajectory_stitcher.h
+44
-0
未找到文件。
modules/common/vehicle_state/vehicle_state.h
浏览文件 @
009038e5
...
...
@@ -77,6 +77,8 @@ class VehicleState {
*/
double
z
()
const
;
double
kappa
()
const
;
/**
* @brief Get the heading of vehicle position, which is the angle
* between the vehicle's heading direction and the x-axis.
...
...
@@ -135,6 +137,8 @@ class VehicleState {
*/
void
set_heading
(
const
double
heading
);
void
set_kappa
(
const
double
kappa
)
{
kappa_
=
kappa
;
}
/**
* @brief Set the vehicle's linear velocity.
* @param linear_velocity The value to set the vehicle's linear velocity.
...
...
@@ -181,6 +185,8 @@ class VehicleState {
double
y_
=
0.0
;
double
z_
=
0.0
;
double
heading_
=
0.0
;
// TODO: check the setting of kappa_
double
kappa_
=
0.0
;
double
linear_v_
=
0.0
;
double
angular_v_
=
0.0
;
double
linear_a_
=
0.0
;
...
...
modules/planning/common/trajectory/discretized_trajectory.h
浏览文件 @
009038e5
...
...
@@ -61,6 +61,10 @@ class DiscretizedTrajectory : public Trajectory {
uint32_t
num_of_points
()
const
;
const
std
::
vector
<
apollo
::
common
::
TrajectoryPoint
>
trajectory_points
()
const
{
return
_trajectory_points
;
}
protected:
std
::
vector
<
apollo
::
common
::
TrajectoryPoint
>
_trajectory_points
;
};
...
...
modules/planning/trajectory_stitcher/BUILD
0 → 100644
浏览文件 @
009038e5
load
(
"//tools:cpplint.bzl"
,
"cpplint"
)
package
(
default_visibility
=
[
"//visibility:public"
])
cc_library
(
name
=
"trajectory_stitcher"
,
srcs
=
[
"trajectory_stitcher.cc"
,
],
hdrs
=
[
"trajectory_stitcher.h"
,
],
deps
=
[
"//modules/common:log"
,
"//modules/common/proto:path_point_proto"
,
"//modules/common/vehicle_state"
,
"//modules/planning/common:frame"
,
"//modules/planning/common:planning_gflags"
,
],
)
cpplint
()
modules/planning/trajectory_stitcher/trajectory_stitcher.cc
0 → 100644
浏览文件 @
009038e5
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
/**
* @file trajectory_stitcher.cc
**/
#include "modules/planning/trajectory_stitcher/trajectory_stitcher.h"
#include <algorithm>
#include "modules/common/log.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/math/double.h"
namespace
apollo
{
namespace
planning
{
using
VehicleState
=
apollo
::
common
::
vehicle_state
::
VehicleState
;
std
::
vector
<
TrajectoryPoint
>
TrajectoryStitcher
::
compute_stitching_trajectory
(
const
VehicleState
&
vehicle_state
,
const
Frame
*
const
prev_frame
)
{
auto
compute_reinit_stitching_trajectory
=
[](
const
VehicleState
&
vehicle_state
)
{
TrajectoryPoint
init_point
;
init_point
.
mutable_path_point
()
->
set_x
(
vehicle_state
.
x
());
init_point
.
mutable_path_point
()
->
set_y
(
vehicle_state
.
y
());
init_point
.
set_v
(
vehicle_state
.
linear_velocity
());
init_point
.
set_a
(
vehicle_state
.
linear_acceleration
());
init_point
.
mutable_path_point
()
->
set_theta
(
vehicle_state
.
heading
());
init_point
.
mutable_path_point
()
->
set_kappa
(
vehicle_state
.
kappa
());
// TODO: the time is not correct, it should be
// FLAGS_forward_predict_time.
init_point
.
set_relative_time
(
0.0
);
// TODO: the init point should be some future point, not current vehicle
// state
// TODO: need vehicle bicycle model to compute the overhead trajectory.
return
std
::
vector
<
TrajectoryPoint
>
(
1
,
init_point
);
};
// no planning history
if
(
prev_frame
==
nullptr
)
{
return
compute_reinit_stitching_trajectory
(
vehicle_state
);
}
const
auto
&
prev_trajectory
=
prev_frame
->
planning_data
().
computed_trajectory
();
std
::
size_t
prev_trajectory_size
=
prev_trajectory
.
num_of_points
();
// previous planning is failed
if
(
prev_trajectory_size
==
0
)
{
AWARN
<<
"Projected trajectory at time ["
<<
prev_trajectory
.
header_time
()
<<
"] size is zero! Use origin car status instead."
;
return
compute_reinit_stitching_trajectory
(
vehicle_state
);
}
const
double
veh_rel_time
=
vehicle_state
.
timestamp
()
-
prev_trajectory
.
header_time
();
std
::
size_t
matched_index
=
prev_trajectory
.
query_nearest_point
(
veh_rel_time
);
// the previous trajectory is not long enough; something is seriously wrong.
if
(
matched_index
==
prev_trajectory_size
)
{
return
compute_reinit_stitching_trajectory
(
vehicle_state
);
}
// the previous trajectory doesn't cover current time;
if
(
matched_index
==
0
&&
veh_rel_time
<
prev_trajectory
.
start_point
().
relative_time
())
{
return
compute_reinit_stitching_trajectory
(
vehicle_state
);
}
auto
matched_point
=
prev_trajectory
.
trajectory_point_at
(
matched_index
);
double
position_diff
=
Eigen
::
Vector2d
(
matched_point
.
path_point
().
x
()
-
vehicle_state
.
x
(),
matched_point
.
path_point
().
y
()
-
vehicle_state
.
y
())
.
norm
();
// the distance between matched point and actual position is too large
if
(
position_diff
>
FLAGS_replan_distance_threshold
)
{
return
compute_reinit_stitching_trajectory
(
vehicle_state
);
}
double
forward_rel_time
=
veh_rel_time
+
FLAGS_forward_predict_time
;
std
::
size_t
forward_index
=
prev_trajectory
.
query_nearest_point
(
forward_rel_time
);
std
::
vector
<
TrajectoryPoint
>
stitching_trajectory
(
prev_trajectory
.
trajectory_points
().
begin
()
+
matched_index
,
prev_trajectory
.
trajectory_points
().
begin
()
+
forward_index
+
1
);
double
zero_time
=
prev_trajectory
.
trajectory_point_at
(
matched_index
).
relative_time
();
std
::
for_each
(
stitching_trajectory
.
begin
(),
stitching_trajectory
.
end
(),
[
&
zero_time
](
TrajectoryPoint
&
tp
)
{
tp
.
set_relative_time
(
tp
.
relative_time
()
-
zero_time
);
});
return
stitching_trajectory
;
}
}
// namespace planning
}
// namespace apollo
modules/planning/trajectory_stitcher/trajectory_stitcher.h
0 → 100644
浏览文件 @
009038e5
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
/**
* @file trajectory_stitcher.h
**/
#ifndef MODULES_PLANNING_TRAJECTORY_STITCHER_H_
#define MODULES_PLANNING_TRAJECTORY_STITCHER_H_
#include "modules/common/proto/path_point.pb.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/planning/common/frame.h"
namespace
apollo
{
namespace
planning
{
class
TrajectoryStitcher
{
public:
TrajectoryStitcher
()
=
delete
;
static
std
::
vector
<
TrajectoryPoint
>
compute_stitching_trajectory
(
const
common
::
vehicle_state
::
VehicleState
&
vehicle_state
,
const
Frame
*
const
prev_frame
);
};
}
// namespace planning
}
// namespace apollo
#endif // MODULES_PLANNING_TRAJECTORY_STITCHER_H_
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录