restored planning.proto for rosbag play. TODO: reference_line_decider

modules/planning/proto/planning.proto

@@ -8,6 +8,39 @@ import "modules/common/proto/path_point.proto";
 import "modules/canbus/proto/chassis.proto";
 import "modules/planning/proto/planning_internal.proto";
 
+// TODO(aaron): Migrate to apollo::common::TrajectoryPoint.
+message ADCTrajectoryPoint {
+  optional double x = 1;  // in meters.
+  optional double y = 2;  // in meters.
+  optional double z = 3;  // height in meters.
+
+  optional double speed = 6;  // speed, in meters / second
+  optional double acceleration_s = 7;  // acceleration in s direction
+  optional double curvature = 8;  // curvature (k = 1/r), unit: (1/meters)
+  // change of curvature in unit s (dk/ds)
+  optional double curvature_change_rate = 9;
+  // in seconds (relative_time = time_of_this_state - timestamp_in_header)
+  optional double relative_time = 10;
+  optional double theta = 11;  // relative to absolute coordinate system
+  // calculated from the first point in this trajectory
+  optional double accumulated_s = 12;
+
+  // in meters, reference to route SL-coordinate
+  optional double s = 4 [deprecated = true];
+  // in meters, reference to route SL-coordinate
+  optional double l = 5 [deprecated = true];
+}
+
+// TODO(aaron): Migrate to apollo::common::PathPoint.
+message ADCPathPoint {
+  optional double x = 1;  // in meters
+  optional double y = 2;  // in meters
+  optional double z = 3;  // in meters
+  optional double curvature = 4;  // curvature (k = 1/r), unit: (1/meters)
+  optional double heading = 5;  // relative to absolute coordinate system
+}
+
+
 message ADCSignals {
   enum SignalType {
     LEFT_TURN = 1;
@@ -29,9 +62,11 @@ message ADCTrajectory {
   optional apollo.common.Header header = 1;
   optional double total_path_length = 2; // in meters
   optional double total_path_time = 3; // in seconds
-  repeated apollo.common.TrajectoryPoint trajectory_point = 4;
+  repeated ADCTrajectoryPoint adc_trajectory_point = 4 [deprecated=true];
+  repeated apollo.common.TrajectoryPoint trajectory_point = 12;
   optional EStop estop = 6;
-  repeated apollo.common.PathPoint path_point = 7;
+  repeated ADCPathPoint adc_path_point = 7 [deprecated=true];
+  repeated apollo.common.PathPoint path_point = 13;
   optional apollo.planning_internal.Debug debug = 8;
   // is_replan == true mean replan triggered
   optional bool is_replan = 9 [default = false];

modules/planning/reference_line/BUILD

@@ -15,6 +15,7 @@ cc_library(
         "//modules/common/proto:path_point_proto",
         "//modules/map/pnc_map:pnc_path",
         "//modules/common/math:math",
+        "//modules/planning/common:data_center",
         "//modules/planning/math:double",
     ],
 )

modules/planning/reference_line/reference_line_decider.cc

+/******************************************************************************
+ * 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 reference_line_decider.cc
+ **/
+#include "modules/planning/reference_line/reference_line_decider.h"
+
+#include "modules/planning/math/double.h"
+
+namespace apollo {
+namespace planning {
+
+using ErrorCode = apollo::common::ErrorCode;
+
+ReferenceLineDecider::ReferenceLineDecider() { _it = _reference_lines.begin(); }
+
+ErrorCode ReferenceLineDecider::init(const DataCenter& data_center) {
+  _reference_lines.clear();
+  const auto& routing =
+      data_center.current_frame()->environment().routing_proxy().routing();
+  const auto& state = data_center.current_frame()
+                          ->environment()
+                          .vehicle_state_proxy()
+                          .vehicle_state();
+  auto* master = data_center.mutable_master();
+
+  // TODO: call build_reference_lines(data_center, router) when new routing are
+  // used!
+  if (routing.header().sequence_num() != _last_route_sequence_num ||
+      Double::compare(routing.header().timestamp_sec(),
+                      _last_route_timestamp) != 0) {
+    // now, get a new routing, then do initializing.
+    _route_reference_lines.clear();
+    std::vector<apollo::hdmap::RoutingResult> lanes;
+    const auto& map = data_center.map();
+    ErrorCode ret =
+        data_center.current_frame()->environment().routing_proxy().lanes(
+            map, &lanes);
+    if (ret != ErrorCode::OK) {
+      AERROR << "Fail to initialize lanes.";
+      return ret;
+    }
+    for (const auto& lane : lanes) {
+      _route_reference_lines.emplace_back();
+      ret = map.get_reference_line_from_routing(
+          lane, 0.0, lane.measurement().distance(),
+          &(_route_reference_lines.back()));
+      if (ret != ErrorCode::OK) {
+        AERROR << "Fail to initialize reference line.";
+        return ret;
+      }
+    }
+    _last_route_sequence_num = routing.header().sequence_num();
+    _last_route_timestamp = routing.header().timestamp_sec();
+    _current_route_index = 0;
+    _current_s = 0.0;
+    master->set_state(MasterStateMachine::MasterState::CRUISE);
+  }
+  // Logic as follow :
+  // 0. check the state of master:
+  if (_route_reference_lines.empty()) {
+    AERROR << "Have not got reference line yet.";
+    return ErrorCode::PLANNING_ERROR;
+  }
+
+  if (master->state() != MasterStateMachine::MasterState::CRUISE &&
+      master->state() != MasterStateMachine::MasterState::CHANGING_LANE) {
+    AERROR << "Only cruise or lane_chaning is accpeted.";
+    return ErrorCode::PLANNING_ERROR;
+  }
+
+  // 1. from current_route_index to size of route_reference_line, calculate the
+  // sl point.
+  std::vector<SLPoint> sl_points;
+  Eigen::Vector2d location(state.pose().position().x(),
+                           state.pose().position().y());
+  std::size_t next_route_index = _current_route_index;
+  for (std::size_t i = _current_route_index;
+       i < _route_reference_lines.size() && i - _current_route_index < 2; ++i) {
+    sl_points.emplace_back();
+    if (!_route_reference_lines[i].get_point_in_Frenet_frame(
+            location, &sl_points.back())) {
+      sl_points.pop_back();
+    } else {
+      if (fabs(sl_points.back().l()) <
+          _route_reference_lines[i].get_lane_width(sl_points.back().s())) {
+        next_route_index = i;
+      }
+    }
+  }
+
+  if (sl_points.empty()) {
+    AERROR << "Can not find location in the reference line.";
+    return ErrorCode::PLANNING_ERROR;
+  }
+  if (_current_route_index != next_route_index) {
+    master->set_state(MasterStateMachine::MasterState::CRUISE);
+    _current_s = sl_points.back().s();
+  } else {
+    _current_s = std::max(sl_points.begin()->s(), _current_s);
+  }
+
+  _current_route_index = next_route_index;
+
+  // 3. put current reference line in reference_lines.
+  auto reference_line = new ReferenceLine();
+  reference_line->move(_route_reference_lines[_current_route_index]);
+  _reference_lines.emplace_back(reference_line);
+
+  // 4. judge if the s in the range of lane change.
+  next_route_index = _current_route_index + 1;
+  if (next_route_index < _route_reference_lines.size()) {
+    SLPoint sl_point;
+    if (_route_reference_lines[next_route_index].get_point_in_Frenet_frame(
+            location, &sl_point)) {
+      // 5. if yes, put the next reference lane in the front of reference.
+      reference_line = new ReferenceLine();
+      reference_line->move(_route_reference_lines[next_route_index]);
+      _reference_lines.emplace_front(reference_line);
+    }
+  }
+
+  // 6. if finished, then change state to finsh.
+  if (next_route_index == _route_reference_lines.size()) {
+    if (_current_s + 4.5 >=
+        _route_reference_lines.back().reference_map_line().length()) {
+      master->set_state(MasterStateMachine::MasterState::FINISH);
+    }
+  }
+  _it = _reference_lines.begin();
+  return ErrorCode::OK;
+}
+
+ErrorCode ReferenceLineDecider::build_reference_lines(
+    const DataCenter& data_center,
+    const apollo::hdmap::RoutingResult& routing) {
+  auto* master = data_center.mutable_master();
+  if (routing.header().sequence_num() != _last_route_sequence_num ||
+      Double::compare(routing.header().timestamp_sec(),
+                      _last_route_timestamp) != 0) {
+    // now, get a new routing, then do initializing.
+    _route_reference_lines.clear();
+    std::vector<apollo::hdmap::RoutingResult> lanes;
+    const auto& map = data_center.map();
+    ErrorCode ret =
+        data_center.current_frame()->environment().routing_proxy().lanes(
+            map, &lanes);
+    if (ret != ErrorCode::OK) {
+      AERROR << "Could not initial lanes.";
+      return ret;
+    }
+    for (const auto& lane : lanes) {
+      _route_reference_lines.emplace_back();
+      ret = map.get_reference_line_from_routing(
+          lane, 0.0, lane.measurement().distance(),
+          &(_route_reference_lines.back()));
+      if (ret != ErrorCode::OK) {
+        AERROR << "Could not initial reference line.";
+        return ret;
+      }
+    }
+    _last_route_sequence_num = routing.header().sequence_num();
+    _last_route_timestamp = routing.header().timestamp_sec();
+    _current_route_index = 0;
+    _current_s = 0.0;
+    master->set_state(MasterStateMachine::MasterState::CRUISE);
+  }
+  return ErrorCode::OK;
+}
+
+std::unique_ptr<ReferenceLine> ReferenceLineDecider::next_reference_line() {
+  std::unique_ptr<ReferenceLine> ret = nullptr;
+  if (_it != _reference_lines.end()) {
+    ret = std::move(*_it);
+    ++_it;
+  }
+  return ret;
+}
+
+bool ReferenceLineDecider::has_next() const {
+  return _it != _reference_lines.end();
+}
+
+std::string ReferenceLineDecider::to_json() const { return ""; }
+
+std::size_t ReferenceLineDecider::num_of_reference_lines() const {
+  return _reference_lines.size();
+}
+
+}  // namespace planning
+}  // namespace apollo

modules/planning/reference_line/reference_line_decider.h

+/******************************************************************************
+ * 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 reference_line_decider.h
+ **/
+
+#ifndef MODULES_PLANNING_REFERENCE_LINE_REFERENCE_LINE_DECIDER_H_
+#define MODULES_PLANNING_REFERENCE_LINE_REFERENCE_LINE_DECIDER_H_
+
+#include <list>
+#include <vector>
+
+#include "modules/common/proto/error_code.pb.h"
+#include "modules/map/proto/routing.pb.h"
+
+#include "modules/planning/common/data_center.h"
+#include "modules/planning/common/environment.h"
+#include "modules/planning/reference_line/reference_line.h"
+
+namespace apollo {
+namespace planning {
+
+class ReferenceLineDecider {
+ public:
+  ReferenceLineDecider();
+  apollo::common::ErrorCode init(const DataCenter& data_center);
+  bool has_next() const;
+  std::unique_ptr<ReferenceLine> next_reference_line();
+  std::size_t num_of_reference_lines() const;
+
+ private:
+  apollo::common::ErrorCode build_reference_lines(
+      const DataCenter& data_center,
+      const apollo::hdmap::RoutingResult& routing);
+
+  double _last_route_timestamp = 0.0;
+  int64_t _last_route_sequence_num = -1;
+  std::size_t _current_route_index = 0;
+  double _current_s = 0.0;
+  std::vector<ReferenceLine> _route_reference_lines;
+  std::list<std::unique_ptr<ReferenceLine>> _reference_lines;
+  std::list<std::unique_ptr<ReferenceLine>>::iterator _it;
+};
+
+}  // namespace planning
+}  // namespace apollo
+
+#endif  // MODULES_PLANNING_REFERENCE_LINE_REFERENCE_LINE_DECIDER_H_