Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
1560cbfb
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,发现更多精彩内容 >>
提交
1560cbfb
编写于
7月 18, 2017
作者:
J
Jiangtao Hu
提交者:
Dong Li
7月 18, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
add planning config and refactor to plugin houston code. (#29)
上级
2ebd4352
变更
18
显示空白变更内容
内联
并排
Showing
18 changed file
with
236 addition
and
336 deletion
+236
-336
modules/planning/BUILD
modules/planning/BUILD
+1
-2
modules/planning/common/planning_gflags.cc
modules/planning/common/planning_gflags.cc
+4
-0
modules/planning/common/planning_gflags.h
modules/planning/common/planning_gflags.h
+1
-0
modules/planning/conf/planning_config.pb.txt
modules/planning/conf/planning_config.pb.txt
+1
-0
modules/planning/main.cc
modules/planning/main.cc
+3
-16
modules/planning/planner/BUILD
modules/planning/planner/BUILD
+1
-0
modules/planning/planner_factory.cc
modules/planning/planner_factory.cc
+0
-35
modules/planning/planner_factory.h
modules/planning/planner_factory.h
+0
-58
modules/planning/planning.cc
modules/planning/planning.cc
+134
-7
modules/planning/planning.h
modules/planning/planning.h
+51
-8
modules/planning/planning_node.cc
modules/planning/planning_node.cc
+0
-129
modules/planning/planning_node.h
modules/planning/planning_node.h
+0
-74
modules/planning/planning_test.cc
modules/planning/planning_test.cc
+10
-7
modules/planning/proto/BUILD
modules/planning/proto/BUILD
+2
-0
modules/planning/proto/planning_config.proto
modules/planning/proto/planning_config.proto
+11
-0
modules/planning/testdata/conf/adapter.conf
modules/planning/testdata/conf/adapter.conf
+15
-0
modules/planning/testdata/conf/planning.conf
modules/planning/testdata/conf/planning.conf
+1
-0
modules/planning/testdata/conf/planning_config.pb.txt
modules/planning/testdata/conf/planning_config.pb.txt
+1
-0
未找到文件。
modules/planning/BUILD
浏览文件 @
1560cbfb
...
...
@@ -3,9 +3,7 @@ package(default_visibility = ["//visibility:public"])
cc_library
(
name
=
"lib_planning"
,
srcs
=
[
"planner_factory.cc"
,
"planning.cc"
,
"planning_node.cc"
,
],
hdrs
=
glob
([
"*.h"
,
...
...
@@ -14,6 +12,7 @@ cc_library(
"//modules/common"
,
"//modules/common:log"
,
"//modules/common/adapters:adapter_manager"
,
"//modules/common:apollo_app"
,
"//modules/common/proto:path_point_proto"
,
"//modules/common/vehicle_state"
,
"//modules/decision/proto:decision_proto"
,
...
...
modules/planning/common/planning_gflags.cc
浏览文件 @
1560cbfb
...
...
@@ -16,6 +16,10 @@
#include "modules/planning/common/planning_gflags.h"
DEFINE_string
(
planning_config_file
,
"modules/planning/conf/planning_config.pb.txt"
,
"planning config file"
);
DEFINE_int32
(
planning_loop_rate
,
5
,
"Loop rate for planning node"
);
DEFINE_string
(
rtk_trajectory_filename
,
"modules/planning/data/garage.csv"
,
...
...
modules/planning/common/planning_gflags.h
浏览文件 @
1560cbfb
...
...
@@ -19,6 +19,7 @@
#include "gflags/gflags.h"
DECLARE_string
(
planning_config_file
);
DECLARE_int32
(
planning_loop_rate
);
DECLARE_string
(
rtk_trajectory_filename
);
DECLARE_uint64
(
rtk_trajectory_backward
);
...
...
modules/planning/conf/planning_config.pb.txt
0 → 100644
浏览文件 @
1560cbfb
planner_type: RTK
modules/planning/main.cc
浏览文件 @
1560cbfb
...
...
@@ -14,20 +14,7 @@
* limitations under the License.
*****************************************************************************/
#include "modules/planning/planning_node.h"
#include "modules/common/apollo_app.h"
#include "modules/planning/planning.h"
#include "gflags/gflags.h"
#include "modules/common/log.h"
#include "ros/include/ros/ros.h"
int
main
(
int
argc
,
char
**
argv
)
{
google
::
InitGoogleLogging
(
argv
[
0
]);
google
::
ParseCommandLineFlags
(
&
argc
,
&
argv
,
true
);
ros
::
init
(
argc
,
argv
,
"planning"
);
::
apollo
::
planning
::
PlanningNode
planning_node
;
planning_node
.
Run
();
return
0
;
}
APOLLO_MAIN
(
::
apollo
::
planning
::
Planning
)
modules/planning/planner/BUILD
浏览文件 @
1560cbfb
...
...
@@ -13,6 +13,7 @@ cc_library(
"//modules/common:log"
,
"//modules/common/proto:path_point_proto"
,
"//modules/common/util"
,
"//modules/common/util:factory"
,
"//modules/common/vehicle_state"
,
"//modules/planning/common:lib_planning_common"
,
"@eigen//:eigen"
,
...
...
modules/planning/planner_factory.cc
已删除
100644 → 0
浏览文件 @
2ebd4352
/******************************************************************************
* 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.
*****************************************************************************/
#include "modules/planning/planner_factory.h"
#include "modules/planning/planner/rtk_replay_planner.h"
namespace
apollo
{
namespace
planning
{
std
::
unique_ptr
<
Planner
>
PlannerFactory
::
CreateInstance
(
const
PlannerType
&
planner_type
)
{
switch
(
planner_type
)
{
case
PlannerType
::
RTK_PLANNER
:
return
std
::
unique_ptr
<
Planner
>
(
new
RTKReplayPlanner
());
default:
return
nullptr
;
}
}
}
// namespace planning
}
// namespace apollo
modules/planning/planner_factory.h
已删除
100644 → 0
浏览文件 @
2ebd4352
/******************************************************************************
* 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.
*****************************************************************************/
#ifndef MODULES_PLANNING_PLANNER_FACTORY_H_
#define MODULES_PLANNING_PLANNER_FACTORY_H_
#include "planner/planner.h"
#include <memory>
/**
* @namespace apollo::planning
* @brief apollo::planning
*/
namespace
apollo
{
namespace
planning
{
/**
* @class PlanningType
* @brief PlanningType is a enum class used to specify a planner instance.
*/
enum
class
PlannerType
{
RTK_PLANNER
,
OTHER
};
/**
* @class PlannerFactory
* @brief PlannerFactory is a creator class that
* creates an instance of a specific planner.
*/
class
PlannerFactory
{
public:
PlannerFactory
()
=
delete
;
/**
* @brief Generate a planner instance.
* @param planner_type The specific type of planner,
* currently only supports PlannerType::RTK_PLANNER
* @return A unique pointer pointing to the planner instance.
*/
static
std
::
unique_ptr
<
Planner
>
CreateInstance
(
const
PlannerType
&
planner_type
);
};
}
// namespace planning
}
// namespace apollo
#endif
/* MODULES_PLANNING_PLANNER_FACTORY_H_ */
modules/planning/planning.cc
浏览文件 @
1560cbfb
...
...
@@ -16,20 +16,123 @@
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/time/time.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/planner_factory.h"
#include "modules/planning/planning.h"
#include "modules/planning/planner/rtk_replay_planner.h"
namespace
apollo
{
namespace
planning
{
using
apollo
::
common
::
TrajectoryPoint
;
using
apollo
::
common
::
vehicle_state
::
VehicleState
;
using
apollo
::
common
::
adapter
::
AdapterManager
;
using
apollo
::
common
::
time
::
Clock
;
using
TrajectoryPb
=
ADCTrajectory
;
using
apollo
::
common
::
Status
;
using
apollo
::
common
::
ErrorCode
;
Planning
::
Planning
()
{
ptr_planner_
=
PlannerFactory
::
CreateInstance
(
PlannerType
::
RTK_PLANNER
)
;
std
::
string
Planning
::
Name
()
const
{
return
"planning"
;
}
void
Planning
::
RegisterPlanners
()
{
planner_factory_
.
Register
(
PlanningConfig
::
RTK
,
[]()
->
Planner
*
{
return
new
RTKReplayPlanner
();
});
}
Status
Planning
::
Init
()
{
if
(
!
apollo
::
common
::
util
::
GetProtoFromFile
(
FLAGS_planning_config_file
,
&
config_
))
{
AERROR
<<
"failed to load planning config file "
<<
FLAGS_planning_config_file
;
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
"failed to load planning config file: "
+
FLAGS_planning_config_file
);
}
AdapterManager
::
Init
(
FLAGS_adapter_config_path
);
RegisterPlanners
();
planner_
=
planner_factory_
.
CreateObject
(
config_
.
planner_type
());
if
(
!
planner_
)
{
return
Status
(
ErrorCode
::
PLANNING_ERROR
,
"planning is not initialized with config : "
+
config_
.
DebugString
());
}
return
Status
::
OK
();
}
Status
Planning
::
Start
()
{
static
ros
::
Rate
loop_rate
(
FLAGS_planning_loop_rate
);
while
(
ros
::
ok
())
{
RunOnce
();
ros
::
spinOnce
();
loop_rate
.
sleep
();
}
return
Status
::
OK
();
}
void
Planning
::
RunOnce
()
{
AdapterManager
::
Observe
();
if
(
AdapterManager
::
GetLocalization
()
==
nullptr
)
{
AERROR
<<
"Localization is not available; skip the planning cycle"
;
return
;
}
if
(
AdapterManager
::
GetLocalization
()
->
Empty
())
{
AERROR
<<
"localization messages are missing; skip the planning cycle"
;
return
;
}
else
{
AINFO
<<
"Get localization message;"
;
}
if
(
AdapterManager
::
GetChassis
()
==
nullptr
)
{
AERROR
<<
"Chassis is not available; skip the planning cycle"
;
return
;
}
if
(
AdapterManager
::
GetChassis
()
->
Empty
())
{
AERROR
<<
"Chassis messages are missing; skip the planning cycle"
;
return
;
}
else
{
AINFO
<<
"Get localization message;"
;
}
AINFO
<<
"Start planning ..."
;
const
auto
&
localization
=
AdapterManager
::
GetLocalization
()
->
GetLatestObserved
();
VehicleState
vehicle_state
(
localization
);
const
auto
&
chassis
=
AdapterManager
::
GetChassis
()
->
GetLatestObserved
();
bool
is_on_auto_mode
=
chassis
.
driving_mode
()
==
chassis
.
COMPLETE_AUTO_DRIVE
;
double
planning_cycle_time
=
1.0
/
FLAGS_planning_loop_rate
;
// the execution_start_time is the estimated time when the planned trajectory
// will be executed by the controller.
double
execution_start_time
=
apollo
::
common
::
time
::
ToSecond
(
apollo
::
common
::
time
::
Clock
::
Now
())
+
planning_cycle_time
;
std
::
vector
<
TrajectoryPoint
>
planning_trajectory
;
bool
res_planning
=
Plan
(
vehicle_state
,
is_on_auto_mode
,
execution_start_time
,
&
planning_trajectory
);
if
(
res_planning
)
{
TrajectoryPb
trajectory_pb
=
ToTrajectoryPb
(
execution_start_time
,
planning_trajectory
);
AdapterManager
::
PublishPlanningTrajectory
(
trajectory_pb
);
AINFO
<<
"Planning succeeded"
;
}
else
{
AINFO
<<
"Planning failed"
;
}
}
void
Planning
::
Stop
()
{}
bool
Planning
::
Plan
(
const
common
::
vehicle_state
::
VehicleState
&
vehicle_state
,
const
bool
is_on_auto_mode
,
const
double
publish_time
,
std
::
vector
<
TrajectoryPoint
>*
planning_trajectory
)
{
...
...
@@ -60,7 +163,7 @@ bool Planning::Plan(const common::vehicle_state::VehicleState& vehicle_state,
// planned trajectory from the matched point, the matched point has
// relative time 0.
bool
planning_succeeded
=
p
tr_p
lanner_
->
Plan
(
matched_point
,
planning_trajectory
);
planner_
->
Plan
(
matched_point
,
planning_trajectory
);
if
(
!
planning_succeeded
)
{
last_trajectory_
.
clear
();
...
...
@@ -90,7 +193,7 @@ bool Planning::Plan(const common::vehicle_state::VehicleState& vehicle_state,
ComputeStartingPointFromVehicleState
(
vehicle_state
,
planning_cycle_time
);
bool
planning_succeeded
=
p
tr_p
lanner_
->
Plan
(
vehicle_state_point
,
planning_trajectory
);
planner_
->
Plan
(
vehicle_state_point
,
planning_trajectory
);
if
(
!
planning_succeeded
)
{
last_trajectory_
.
clear
();
return
false
;
...
...
@@ -165,5 +268,29 @@ std::vector<TrajectoryPoint> Planning::GetOverheadTrajectory(
return
overhead_trajectory
;
}
TrajectoryPb
Planning
::
ToTrajectoryPb
(
const
double
header_time
,
const
std
::
vector
<
TrajectoryPoint
>&
discretized_trajectory
)
{
TrajectoryPb
trajectory_pb
;
AdapterManager
::
FillPlanningTrajectoryHeader
(
"planning"
,
trajectory_pb
.
mutable_header
());
trajectory_pb
.
mutable_header
()
->
set_timestamp_sec
(
header_time
);
for
(
const
auto
&
trajectory_point
:
discretized_trajectory
)
{
auto
ptr_trajectory_point_pb
=
trajectory_pb
.
add_adc_trajectory_point
();
ptr_trajectory_point_pb
->
set_x
(
trajectory_point
.
x
());
ptr_trajectory_point_pb
->
set_y
(
trajectory_point
.
y
());
ptr_trajectory_point_pb
->
set_theta
(
trajectory_point
.
theta
());
ptr_trajectory_point_pb
->
set_curvature
(
trajectory_point
.
kappa
());
ptr_trajectory_point_pb
->
set_relative_time
(
trajectory_point
.
relative_time
());
ptr_trajectory_point_pb
->
set_speed
(
trajectory_point
.
v
());
ptr_trajectory_point_pb
->
set_acceleration_s
(
trajectory_point
.
a
());
ptr_trajectory_point_pb
->
set_accumulated_s
(
trajectory_point
.
s
());
}
return
std
::
move
(
trajectory_pb
);
}
}
// namespace planning
}
// namespace apollo
modules/planning/planning.h
浏览文件 @
1560cbfb
...
...
@@ -17,25 +17,55 @@
#ifndef MODULES_PLANNING_PLANNING_H_
#define MODULES_PLANNING_PLANNING_H_
#include <memory>
#include "modules/common/apollo_app.h"
#include "modules/common/proto/path_point.pb.h"
#include "modules/common/status/status.h"
#include "modules/common/util/factory.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/planning/planner/planner.h"
#include "modules/planning/proto/planning.pb.h"
#include "modules/planning/proto/planning_config.pb.h"
#include <memory>
/**
* @namespace apollo::planning
* @brief apollo::planning
*/
namespace
apollo
{
namespace
planning
{
class
Planning
{
/**
* @class Localization
*
* @brief Localization module main class. It processes GPS and IMU as input,
* to generate localization info.
*/
class
Planning
:
public
apollo
::
common
::
ApolloApp
{
public:
/**
* @brief Constructor
* @brief module name
* @return module name
*/
Planning
()
;
std
::
string
Name
()
const
override
;
/**
* @brief Destructor
* @brief module initialization function
* @return initialization status
*/
~
Planning
()
=
default
;
apollo
::
common
::
Status
Init
()
override
;
/**
* @brief module start function
* @return start status
*/
apollo
::
common
::
Status
Start
()
override
;
/**
* @brief module stop function
* @return stop status
*/
void
Stop
()
override
;
/**
* @brief Plan the trajectory given current vehicle state
...
...
@@ -55,6 +85,9 @@ class Planning {
void
Reset
();
private:
void
RegisterPlanners
();
void
RunOnce
();
std
::
pair
<
common
::
TrajectoryPoint
,
std
::
size_t
>
ComputeStartingPointFromLastTrajectory
(
const
double
curr_time
)
const
;
...
...
@@ -65,7 +98,17 @@ class Planning {
std
::
vector
<
common
::
TrajectoryPoint
>
GetOverheadTrajectory
(
const
std
::
size_t
matched_index
,
const
std
::
size_t
buffer_size
);
std
::
unique_ptr
<
Planner
>
ptr_planner_
;
ADCTrajectory
ToTrajectoryPb
(
const
double
header_time
,
const
std
::
vector
<
common
::
TrajectoryPoint
>&
discretized_trajectory
);
apollo
::
common
::
util
::
Factory
<
PlanningConfig
::
PlannerType
,
Planner
>
planner_factory_
;
PlanningConfig
config_
;
std
::
unique_ptr
<
Planner
>
planner_
;
std
::
vector
<
common
::
TrajectoryPoint
>
last_trajectory_
;
...
...
modules/planning/planning_node.cc
已删除
100644 → 0
浏览文件 @
2ebd4352
/******************************************************************************
* 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.
*****************************************************************************/
#include "modules/planning/planning_node.h"
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/log.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/proto/planning.pb.h"
namespace
apollo
{
namespace
planning
{
using
apollo
::
common
::
TrajectoryPoint
;
using
apollo
::
common
::
vehicle_state
::
VehicleState
;
using
apollo
::
common
::
adapter
::
AdapterManager
;
using
apollo
::
common
::
time
::
Clock
;
using
TrajectoryPb
=
ADCTrajectory
;
PlanningNode
::
PlanningNode
()
{
AdapterManager
::
Init
(
FLAGS_adapter_config_path
);
}
PlanningNode
::~
PlanningNode
()
{}
void
PlanningNode
::
Run
()
{
static
ros
::
Rate
loop_rate
(
FLAGS_planning_loop_rate
);
while
(
ros
::
ok
())
{
RunOnce
();
ros
::
spinOnce
();
loop_rate
.
sleep
();
}
}
void
PlanningNode
::
RunOnce
()
{
AdapterManager
::
Observe
();
if
(
AdapterManager
::
GetLocalization
()
==
nullptr
)
{
AERROR
<<
"Localization is not available; skip the planning cycle"
;
return
;
}
if
(
AdapterManager
::
GetLocalization
()
->
Empty
())
{
AERROR
<<
"localization messages are missing; skip the planning cycle"
;
return
;
}
else
{
AINFO
<<
"Get localization message;"
;
}
if
(
AdapterManager
::
GetChassis
()
==
nullptr
)
{
AERROR
<<
"Chassis is not available; skip the planning cycle"
;
return
;
}
if
(
AdapterManager
::
GetChassis
()
->
Empty
())
{
AERROR
<<
"Chassis messages are missing; skip the planning cycle"
;
return
;
}
else
{
AINFO
<<
"Get localization message;"
;
}
AINFO
<<
"Start planning ..."
;
const
auto
&
localization
=
AdapterManager
::
GetLocalization
()
->
GetLatestObserved
();
VehicleState
vehicle_state
(
localization
);
const
auto
&
chassis
=
AdapterManager
::
GetChassis
()
->
GetLatestObserved
();
bool
is_on_auto_mode
=
chassis
.
driving_mode
()
==
chassis
.
COMPLETE_AUTO_DRIVE
;
double
planning_cycle_time
=
1.0
/
FLAGS_planning_loop_rate
;
// the execution_start_time is the estimated time when the planned trajectory
// will be executed by the controller.
double
execution_start_time
=
apollo
::
common
::
time
::
ToSecond
(
apollo
::
common
::
time
::
Clock
::
Now
())
+
planning_cycle_time
;
std
::
vector
<
TrajectoryPoint
>
planning_trajectory
;
bool
res_planning
=
planning_
.
Plan
(
vehicle_state
,
is_on_auto_mode
,
execution_start_time
,
&
planning_trajectory
);
if
(
res_planning
)
{
TrajectoryPb
trajectory_pb
=
ToTrajectoryPb
(
execution_start_time
,
planning_trajectory
);
AdapterManager
::
PublishPlanningTrajectory
(
trajectory_pb
);
AINFO
<<
"Planning succeeded"
;
}
else
{
AINFO
<<
"Planning failed"
;
}
}
void
PlanningNode
::
Reset
()
{
planning_
.
Reset
();
}
TrajectoryPb
PlanningNode
::
ToTrajectoryPb
(
const
double
header_time
,
const
std
::
vector
<
TrajectoryPoint
>&
discretized_trajectory
)
{
TrajectoryPb
trajectory_pb
;
AdapterManager
::
FillPlanningTrajectoryHeader
(
"planning"
,
trajectory_pb
.
mutable_header
());
trajectory_pb
.
mutable_header
()
->
set_timestamp_sec
(
header_time
);
for
(
const
auto
&
trajectory_point
:
discretized_trajectory
)
{
auto
ptr_trajectory_point_pb
=
trajectory_pb
.
add_adc_trajectory_point
();
ptr_trajectory_point_pb
->
set_x
(
trajectory_point
.
x
());
ptr_trajectory_point_pb
->
set_y
(
trajectory_point
.
y
());
ptr_trajectory_point_pb
->
set_theta
(
trajectory_point
.
theta
());
ptr_trajectory_point_pb
->
set_curvature
(
trajectory_point
.
kappa
());
ptr_trajectory_point_pb
->
set_relative_time
(
trajectory_point
.
relative_time
());
ptr_trajectory_point_pb
->
set_speed
(
trajectory_point
.
v
());
ptr_trajectory_point_pb
->
set_acceleration_s
(
trajectory_point
.
a
());
ptr_trajectory_point_pb
->
set_accumulated_s
(
trajectory_point
.
s
());
}
return
std
::
move
(
trajectory_pb
);
}
}
// namespace planning
}
// namespace apollo
modules/planning/planning_node.h
已删除
100644 → 0
浏览文件 @
2ebd4352
/******************************************************************************
* 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.
*****************************************************************************/
#ifndef MODULES_PLANNING_PLANNING_NODE_H_
#define MODULES_PLANNING_PLANNING_NODE_H_
#include <vector>
#include "modules/common/proto/path_point.pb.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/planning/planning.h"
#include "modules/planning/proto/planning.pb.h"
/**
* @namespace apollo::planning
* @brief apollo::planning
*/
namespace
apollo
{
namespace
planning
{
/**
* @class PlanningNode
* @brief PlanningNode is a class that
* implements a ros node for planning module.
*/
class
PlanningNode
{
public:
/**
* @brief Constructor
*/
PlanningNode
();
/**
* @brief Destructor
*/
virtual
~
PlanningNode
();
/**
* @brief Start the planning node.
*/
void
Run
();
/**
* @brief Reset the planning node.
*/
void
Reset
();
private:
void
RunOnce
();
ADCTrajectory
ToTrajectoryPb
(
const
double
header_time
,
const
std
::
vector
<
common
::
TrajectoryPoint
>&
discretized_trajectory
);
Planning
planning_
;
};
}
// namespace planning
}
// namespace apollo
#endif
/* MODULES_PLANNING_PLANNING_NODE_H_ */
modules/planning/planning_test.cc
浏览文件 @
1560cbfb
...
...
@@ -14,8 +14,8 @@
* limitations under the License.
*****************************************************************************/
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/planner_factory.h"
#include "modules/planning/planning.h"
#include "modules/planning/proto/planning.pb.h"
...
...
@@ -28,7 +28,12 @@ namespace planning {
using
TrajectoryPb
=
ADCTrajectory
;
using
apollo
::
common
::
TrajectoryPoint
;
class
PlanningTest
:
public
::
testing
::
Test
{};
class
PlanningTest
:
public
::
testing
::
Test
{
virtual
void
SetUp
()
{
FLAGS_planning_config_file
=
"modules/planning/testdata/conf/planning_config.pb.txt"
;
FLAGS_adapter_config_path
=
"modules/planning/testdata/conf/adapter.conf"
;
}
};
TEST_F
(
PlanningTest
,
ComputeTrajectory
)
{
FLAGS_rtk_trajectory_filename
=
"modules/planning/testdata/garage.csv"
;
...
...
@@ -43,6 +48,7 @@ TEST_F(PlanningTest, ComputeTrajectory) {
std
::
vector
<
TrajectoryPoint
>
trajectory1
;
double
time1
=
0.1
;
planning
.
Init
();
planning
.
Plan
(
vehicle_state
,
false
,
time1
,
&
trajectory1
);
EXPECT_EQ
(
trajectory1
.
size
(),
(
std
::
size_t
)
FLAGS_rtk_trajectory_forward
);
...
...
@@ -77,6 +83,8 @@ TEST_F(PlanningTest, ComputeTrajectory) {
TEST_F
(
PlanningTest
,
ComputeTrajectoryNoRTKFile
)
{
FLAGS_rtk_trajectory_filename
=
""
;
Planning
planning
;
planning
.
Init
();
common
::
vehicle_state
::
VehicleState
vehicle_state
;
vehicle_state
.
set_x
(
586385.782841
);
vehicle_state
.
set_y
(
4140674.76065
);
...
...
@@ -94,10 +102,5 @@ TEST_F(PlanningTest, ComputeTrajectoryNoRTKFile) {
planning
.
Reset
();
}
TEST_F
(
PlanningTest
,
PlannerFactory
)
{
auto
ptr_planner
=
PlannerFactory
::
CreateInstance
(
PlannerType
::
OTHER
);
EXPECT_TRUE
(
ptr_planner
==
nullptr
);
}
}
// namespace control
}
// namespace apollo
modules/planning/proto/BUILD
浏览文件 @
1560cbfb
...
...
@@ -7,6 +7,7 @@ cc_proto_library(
name
=
"planning_proto"
,
protos
=
[
"planning.proto"
,
"planning_config.proto"
,
"planning_internal.proto"
,
],
deps
=
[
...
...
@@ -23,6 +24,7 @@ py_proto_compile(
name
=
"planning_proto_pylib"
,
protos
=
[
"planning.proto"
,
"planning_config.proto"
,
"planning_internal.proto"
,
],
deps
=
[
...
...
modules/planning/proto/planning_config.proto
0 → 100644
浏览文件 @
1560cbfb
syntax
=
"proto2"
;
package
apollo
.
planning
;
message
PlanningConfig
{
enum
PlannerType
{
RTK
=
0
;
EM
=
1
;
// expecting maximal
};
optional
PlannerType
planner_type
=
1
[
default
=
EM
];
}
modules/planning/testdata/conf/adapter.conf
0 → 100644
浏览文件 @
1560cbfb
config
{
type
:
LOCALIZATION
mode
:
RECEIVE_ONLY
message_history_limit
:
10
}
config
{
type
:
CHASSIS
mode
:
RECEIVE_ONLY
message_history_limit
:
10
}
config
{
type
:
PLANNING_TRAJECTORY
mode
:
PUBLISH_ONLY
}
is_ros
:
false
modules/planning/testdata/conf/planning.conf
0 → 100644
浏览文件 @
1560cbfb
--
adapter_config_path
=
modules
/
planning
/
conf
/
adapter
.
conf
\ No newline at end of file
modules/planning/testdata/conf/planning_config.pb.txt
0 → 100644
浏览文件 @
1560cbfb
planner_type: RTK
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录