(1)migrate object_table; (2)get rid of temp migration TODOs

modules/planning/common/BUILD

@@ -16,6 +16,13 @@ cc_library(
     ],
 )
 
+cc_library(
+    name = "lru_cache",
+    hdrs = [
+        "lru_cache.h",
+    ],
+)
+
 cc_library(
     name = "map_object",
     srcs = [
@@ -29,6 +36,23 @@ cc_library(
     ],
 )
 
+
+cc_library(
+    name = "object_table",
+    srcs = [
+        "object_table.cc",
+    ],
+    hdrs = [
+        "object_table.h",
+    ],
+    deps = [
+        ":lru_cache",
+        ":map_object",
+        ":obstacle",
+        ":planning_gflags",
+    ],
+)
+
 cc_library(
     name = "obstacle",
     srcs = [
@@ -175,6 +199,7 @@ cc_library(
     deps = [
         ":environment",
         ":frame",
+        ":object_table",
         ":planning_gflags",
         "//modules/common",
         "//modules/common/status",

modules/planning/common/data_center.cc

@@ -33,6 +33,7 @@ namespace planning {
 using apollo::common::Status;
 
 DataCenter::DataCenter() {
+  _object_table.reset(new ObjectTable());
   _master.reset(new MasterStateMachine());
 
   AINFO << "Data Center is ready!";
@@ -88,5 +89,13 @@ MasterStateMachine *DataCenter::mutable_master() const {
   return _master.get();
 }
 
+ObjectTable* DataCenter::mutable_object_table() const {
+    return _object_table.get();
+}
+
+const ObjectTable& DataCenter::object_table() const {
+    return *(_object_table.get());
+}
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/common/data_center.h

@@ -28,6 +28,7 @@
 #include "modules/common/macro.h"
 #include "modules/common/status/status.h"
 #include "modules/map/hdmap/hdmap.h"
+#include "modules/planning/common/object_table.h"
 #include "modules/planning/common/frame.h"
 #include "modules/planning/common/environment.h"
 #include "modules/planning/state_machine/master_state_machine.h"
@@ -53,11 +54,15 @@ class DataCenter {
 
   const Frame *last_frame() const;
 
+  const ObjectTable& object_table() const;
+  ObjectTable* mutable_object_table() const;
+
  private:
   std::unordered_map<uint32_t, std::unique_ptr<Frame>> _frames;
   std::list<uint32_t> _sequence_queue;
   Environment _environment;
   std::unique_ptr<Frame> _frame = nullptr;
+  std::unique_ptr<ObjectTable> _object_table = nullptr;
   std::unique_ptr<MasterStateMachine> _master = nullptr;
   ::apollo::hdmap::HDMap map_;
 

modules/planning/common/lru_cache.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 lru_cache.h
+ **/
+
+#ifndef MODULES_PLANNING_COMMON_LRU_CACHE_H
+#define MODULES_PLANNING_COMMON_LRU_CACHE_H
+
+#include <iostream>
+#include <vector>
+#include <unordered_map>
+#include <utility>
+#include <mutex>
+
+namespace apollo {
+namespace planning {
+
+template<class K, class V>
+struct Node {
+  K key;
+  V val;
+  Node* prev;
+  Node* next;
+  Node(): prev(nullptr), next(nullptr) {
+    key = {};
+    val = {};
+  }
+
+  template <typename VV>
+  Node(const K& key, VV&& val) : key(key), val(std::forward<VV>(val)),
+                                 prev(nullptr), next(nullptr) {}
+};
+
+template<class K, class V>
+class LRUCache {
+ private:
+  size_t _capacity;
+  size_t _size;
+  std::unordered_map< K, Node<K, V> >  _map;
+  Node<K, V> _head;
+  Node<K, V> _tail;
+
+ public:
+  LRUCache() : _capacity(10), _map(0), _head(), _tail() {
+    init();
+  }
+
+  explicit LRUCache(size_t capacity) :
+      _capacity(capacity), _map(0), _head(), _tail() {
+    init();
+  }
+
+  ~LRUCache() {
+    clear();
+  }
+
+  void get_cache(std::unordered_map<K, V>& cache) {
+    for (auto it = _map.begin(); it != _map.end(); ++it) {
+      cache[it->first] = it->second.val;
+    }
+  }
+
+  V& operator[] (const K& key) {
+    if (!contains(key)) {
+      K obsolete;
+      get_obsolete(&obsolete);
+    }
+    return _map[key].val;
+  }
+
+  /*
+   * Silently get all as vector
+   */
+  void get_all_silently(std::vector<V*>& ret) {
+    for (auto it = _map.begin(); it != _map.end(); ++it) {
+      ret.push_back(&it->second.val);
+    }
+  }
+
+  /*
+   * for both add & update purposes
+   */
+  template <typename VV>
+  bool put(const K& key, VV&& val) {
+    K tmp;
+    return update(key, std::forward<VV>(val), &tmp, false, false);
+  }
+
+  /*
+   * update existing elements only
+   */
+  template <typename VV>
+  bool update(const K& key, VV&& val) {
+    if (!contains(key)) {
+      return false;
+    }
+    K tmp;
+    return update(key, std::forward<VV>(val), &tmp, true, false);
+  }
+
+  /*
+   * silently update existing elements only
+   */
+  template <typename VV>
+  bool update_silently(const K& key, VV& val) {
+    if (!contains(key)) {
+      return false;
+    }
+    K tmp;
+    return update(key, std::forward<VV>(val), &tmp, true, true);
+  }
+
+  /*
+   * add new elements only
+   */
+  template <typename VV>
+  bool add(const K& key, VV& val) {
+    K tmp;
+    return update(key, std::forward<VV>(val), &tmp, true, false);
+  }
+
+  template <typename VV>
+  bool put_and_get_obsolete(const K& key, VV& val, K* obs) {
+    return update(key, std::forward<VV>(val), obs, false, false);
+  }
+
+  template <typename VV>
+  bool add_and_get_obsolete(const K& key, VV& val, K* obs) {
+    return update(key, std::forward<VV>(val), obs, true, false);
+  }
+
+  V* get_silently(const K& key) {
+    return get(key, true);
+  }
+
+  V* get(const K& key) {
+    return get(key, false);
+  }
+
+  bool get_copy_silently(const K& key, const V* val) {
+    return get_copy(key, val, true);
+  }
+
+  bool get_copy(const K& key, const V* val) {
+    return get_copy(key, val, false);
+  }
+
+  size_t size() {
+    return _size;
+  }
+
+  bool full() {
+    return size() > 0 && size() >= _capacity;
+  }
+
+  bool empty() {
+    return size() == 0;
+  }
+
+  size_t capacity() {
+    return _capacity;
+  }
+
+  void set_capacity(size_t capacity) {
+    _capacity = capacity;
+  }
+
+  Node<K, V>* first() {
+    if (size()) {
+      return _head.next;
+    }
+    return nullptr;
+  }
+
+  bool contains(const K& key) {
+    return _map.find(key) != _map.end();
+  }
+
+  bool prioritize(const K& key) {
+    if (contains(key)) {
+      auto *node = &_map[key];
+      detach(node);
+      attach(node);
+      return true;
+    }
+    return false;
+  }
+
+  void clear() {
+    _map.clear();
+    _size = 0;
+  }
+
+ private:
+  void init() {
+    _head.prev = nullptr;
+    _head.next = &_tail;
+    _tail.prev = &_head;
+    _tail.next = nullptr;
+    _size = 0;
+  }
+
+  void detach(Node<K, V> *node) {
+    if (node->prev != nullptr) {
+      node->prev->next = node->next;
+    }
+    if (node->next != nullptr) {
+      node->next->prev = node->prev;
+    }
+    node->prev = nullptr;
+    node->next = nullptr;
+    --_size;
+  }
+
+  void attach(Node<K, V> *node) {
+    node->prev = &_head;
+    node->next = _head.next;
+    _head.next = node;
+    if (node->next != nullptr) {
+      node->next->prev = node;
+    }
+    ++_size;
+  }
+
+  template <typename VV>
+  bool update(const K& key, VV&& val, K* obs, bool add_only,
+              bool silent_update) {
+    if (obs == nullptr) {
+      return false;
+    }
+    if (contains(key)) {
+      if (!add_only) {
+        _map[key].val = std::forward<VV>(val);
+        if (!silent_update) {
+          auto *node = &_map[key];
+          detach(node);
+          attach(node);
+        } else {
+          return false;
+        }
+      }
+    } else {
+      if (full() && !get_obsolete(obs)) {
+        return false;
+      }
+
+      _map.emplace(key, Node<K, V>(key, std::forward<VV>(val)));
+      attach(&_map[key]);
+    }
+    return true;
+  }
+
+  V* get(const K& key, bool silent) {
+    if (contains(key)) {
+      auto *node = &_map[key];
+      if (!silent) {
+        detach(node);
+        attach(node);
+      }
+      return &node->val;
+    }
+    return nullptr;
+  }
+
+  bool get_copy(const K& key, const V* val, bool silent) {
+    if (contains(key)) {
+      auto *node = &_map[key];
+      if (!silent) {
+        detach(node);
+        attach(node);
+      }
+      *val = node->val;
+      return true;
+    }
+    return false;
+  }
+
+  bool get_obsolete(K* key) {
+    if (full()) {
+      auto *node = _tail.prev;
+      detach(node);
+      _map.erase(node->key);
+      *key = node->key;
+      return true;
+    }
+    return false;
+  }
+};
+
+}  // namespace planning
+}  // namespace apollo
+
+#endif  // MODULES_PLANNING_COMMON_LRU_CACHE_H

modules/planning/common/object_table.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 object_table.cc
+ **/
+
+#include "modules/planning/common/object_table.h"
+
+namespace apollo {
+namespace planning {
+
+bool ObjectTable::get_obstacle(const uint32_t id, Obstacle** const obstacle) {
+  auto *obstacle_from_cache = _obstacle_cache.get(id);
+  if (!obstacle_from_cache) {
+    return false;
+  }
+
+  *obstacle = obstacle_from_cache->get();
+  return true;
+}
+
+void ObjectTable::put_obstacle(std::unique_ptr<Obstacle> obstacle) {
+  _obstacle_cache.put(obstacle->Id(), std::move(obstacle));
+}
+
+bool ObjectTable::get_map_object(const std::string& id,
+                                 MapObject** const map_object) {
+  auto *map_object_from_cache = _map_object_cache.get(id);
+  if (!map_object_from_cache) {
+    return false;
+  }
+
+  *map_object = map_object_from_cache->get();
+
+  return true;
+}
+
+void ObjectTable::put_map_object(std::unique_ptr<MapObject>& map_object) {
+  _map_object_cache.put(map_object->Id(), std::move(map_object));
+}
+
+}  // namespace planning
+}  // namespace apollo

modules/planning/common/object_table.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 object_table.h
+ **/
+
+#ifndef MODULES_PLANNING_COMMON_OBJECT_TABLE_H
+#define MODULES_PLANNING_COMMON_OBJECT_TABLE_H
+
+#include <memory>
+#include <string>
+
+#include "modules/planning/common/lru_cache.h"
+#include "modules/planning/common/map_object.h"
+#include "modules/planning/common/obstacle.h"
+#include "modules/planning/common/planning_gflags.h"
+
+namespace apollo {
+namespace planning {
+
+class ObjectTable {
+ public:
+  ObjectTable() :
+      _obstacle_cache(FLAGS_object_table_obstacle_capacity),
+      _map_object_cache(FLAGS_object_table_obstacle_capacity) {}
+
+  bool get_obstacle(const uint32_t id, Obstacle** const obstacle);
+  void put_obstacle(std::unique_ptr<Obstacle> obstacle);
+  bool get_map_object(const std::string& id, MapObject** const map_object);
+  void put_map_object(std::unique_ptr<MapObject>& map_object);
+
+ private:
+  LRUCache<uint32_t, std::unique_ptr<Obstacle>> _obstacle_cache;
+  LRUCache<std::string, std::unique_ptr<MapObject>> _map_object_cache;
+};
+
+}  // namespace planning
+}  // namespace apollo
+
+#endif  // MODULES_PLANNING_COMMON_OBJECT_TABLE_H

modules/planning/common/planning_data.cc

@@ -68,12 +68,6 @@ void PlanningData::set_computed_trajectory(
   _computed_trajectory = std::move(publishable_trajectory);
 }
 
-// TODO: to be fixed
-int PlanningData::get_obstacle_by_id(const uint32_t id,
-                                     PlanningObject** const obstacle) {
-  return 0;
-}
-
 const PathData& PlanningData::path_data() const { return _path_data; }
 
 const SpeedData& PlanningData::speed_data() const { return _speed_data; }

modules/planning/common/planning_data.h

@@ -57,10 +57,6 @@ class PlanningData {
 
   virtual std::string type() const = 0;
 
-  // TODO: to be fixed
-  int get_obstacle_by_id(const uint32_t id,
-                         PlanningObject** const planning_object);
-
   const PathData& path_data() const;
   const SpeedData& speed_data() const;
 

modules/planning/optimizer/dp_st_speed/dp_st_graph.cc

@@ -283,16 +283,10 @@ ErrorCode DPSTGraph::get_object_decision(const STGraphData& st_graph_data,
            obs_boundaries.begin();
        obs_it != obs_boundaries.end(); ++obs_it) {
     CHECK_GE(obs_it->points().size(), 4);
-    PlanningObject* object_ptr = nullptr;
-
-    // TODO: to be fixed
-    // DataCenter::instance()->mutable_object_table()->get_obstacle(obs_it->Id(),
-    //                                                             &object_ptr);
-    DataCenter::instance()
-        ->current_frame()
-        ->mutable_planning_data()
-        ->get_obstacle_by_id(obs_it->id(), &object_ptr);
 
+    Obstacle* object_ptr = nullptr;
+    DataCenter::instance()->mutable_object_table()->get_obstacle(obs_it->id(),
+                                                                 &object_ptr);
     if (obs_it->points().front().x() <= 0) {
       ObjectDecisionType dec;
       dec.mutable_yield();

modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc

@@ -31,18 +31,13 @@ namespace planning {
 
 using apollo::common::config::VehicleConfigHelper;
 
-// TODO: to be fixed
-// DpStSpeedOptimizer::DpStSpeedOptimizer(
-//    const std::string& name, const ::boost::property_tree::ptree& ptree)
-//    : SpeedOptimizer(name) {}
 DpStSpeedOptimizer::DpStSpeedOptimizer(const std::string& name)
     : SpeedOptimizer(name) {}
 
-ErrorCode DpStSpeedOptimizer::process(const DataCenter& data_center,
-                                      const PathData& path_data,
-                                      const TrajectoryPoint& init_point,
-                                      DecisionData* const decision_data,
-                                      SpeedData* const speed_data) const {
+ErrorCode DpStSpeedOptimizer::process(const PathData& path_data,
+                                       const TrajectoryPoint& init_point,
+                                       DecisionData* const decision_data,
+                                       SpeedData* const speed_data) const {
   ::apollo::common::config::VehicleParam veh_param =
       VehicleConfigHelper::GetConfig().vehicle_param();
 
@@ -54,12 +49,14 @@ ErrorCode DpStSpeedOptimizer::process(const DataCenter& data_center,
   DPSTBoundaryMapper st_mapper(st_boundary_config, veh_param);
   std::vector<STGraphBoundary> boundaries;
 
-  // TODO: to be fixed
-  // if (st_mapper.get_graph_boundary(data_center, *decision_data, path_data,
-  //                                 dp_st_configuration.total_path_length(),
-  //                                 dp_st_configuration.total_time(),
-  //                                 &boundaries) != ErrorCode::PLANNING_OK) {
-  if (0) {
+  common::TrajectoryPoint initial_planning_point = DataCenter::instance()
+      ->current_frame()->mutable_planning_data()->init_planning_point();
+  if (st_mapper.get_graph_boundary(initial_planning_point,
+                                   *decision_data,
+                                   path_data,
+                                   dp_st_configuration.total_path_length(),
+                                   dp_st_configuration.total_time(),
+                                   &boundaries) != ErrorCode::PLANNING_OK) {
     AERROR << "Mapping obstacle for dp st speed optimizer failed.";
     return ErrorCode::PLANNING_ERROR_FAILED;
   }

modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.h

@@ -30,17 +30,12 @@ namespace planning {
 
 class DpStSpeedOptimizer : public SpeedOptimizer {
  public:
-  // TODO: to be fixed
-  // explicit DpStSpeedOptimizer(const std::string& name,
-  //                            const boost::property_tree::ptree& ptree);
   explicit DpStSpeedOptimizer(const std::string& name);
 
- private:
-  virtual ErrorCode process(const DataCenter& data_center,
-                            const PathData& path_data,
-                            const TrajectoryPoint& init_point,
-                            DecisionData* const decision_data,
-                            SpeedData* const speed_data) const;
+  virtual ErrorCode process(const PathData& path_data,
+                             const TrajectoryPoint& init_point,
+                             DecisionData* const decision_data,
+                             SpeedData* const speed_data) const;
 };
 
 }  // namespace planning