提交 1560cbfb 编写于 作者: J Jiangtao Hu 提交者: Dong Li

add planning config and refactor to plugin houston code. (#29)

上级 2ebd4352
......@@ -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",
......
......@@ -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",
......
......@@ -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);
......
......@@ -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)
......@@ -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",
......
/******************************************************************************
* 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
/******************************************************************************
* 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_ */
......@@ -16,23 +16,126 @@
#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) {
const bool is_on_auto_mode, const double publish_time,
std::vector<TrajectoryPoint>* planning_trajectory) {
double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;
double execution_start_time = publish_time;
......@@ -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 =
ptr_planner_->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 =
ptr_planner_->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
......@@ -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
*/
std::string Name() const override;
/**
* @brief module initialization function
* @return initialization status
*/
Planning();
apollo::common::Status Init() override;
/**
* @brief Destructor
* @brief module start function
* @return start status
*/
~Planning() = default;
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_;
......
/******************************************************************************
* 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
/******************************************************************************
* 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_ */
......@@ -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
......@@ -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 = [
......
syntax = "proto2";
package apollo.planning;
message PlanningConfig {
enum PlannerType {
RTK = 0;
EM = 1; // expecting maximal
};
optional PlannerType planner_type = 1 [default = EM];
}
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
--adapter_config_path=modules/planning/conf/adapter.conf
\ No newline at end of file
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册