Implement prediction/container/pose_container.cc

modules/prediction/container/obstacles/obstacle.cc

@@ -21,6 +21,7 @@
 #include <iomanip>
 #include <unordered_set>
 #include <utility>
+#include <algorithm>
 
 #include "modules/common/log.h"
 #include "modules/common/math/math_utils.h"

modules/prediction/container/pose/BUILD

@@ -8,6 +8,8 @@ cc_library(
     hdrs = ["pose_container.h"],
     deps = [
         "//modules/prediction/container:container",
+        "//modules/perception/proto:perception_proto",
+        "//modules/localization/proto:localization_proto",
     ]
 )
 

modules/prediction/container/pose/pose_container.cc

@@ -19,7 +19,41 @@
 namespace apollo {
 namespace prediction {
 
+using apollo::perception::PerceptionObstacle;
+using apollo::perception::Point;
+
+std::mutex PoseContainer::g_mutex_;
+int PoseContainer::id_ = -1;
+PerceptionObstacle::Type PoseContainer::type_ = PerceptionObstacle::VEHICLE;
+
 void PoseContainer::Insert(const ::google::protobuf::Message& message) {}
 
+void PoseContainer::Update(
+    const localization::LocalizationEstimate &localization) {
+  if (obstacle_ptr_.get() == nullptr) {
+    obstacle_ptr_.reset(new PerceptionObstacle());
+  }
+  obstacle_ptr_->set_id(id_);
+  Point position;
+  position.set_x(localization.pose().position().x());
+  position.set_y(localization.pose().position().y());
+  position.set_z(localization.pose().position().z());
+  obstacle_ptr_->mutable_position()->CopyFrom(position);
+
+  Point velocity;
+  position.set_x(localization.pose().linear_velocity().x());
+  position.set_y(localization.pose().linear_velocity().y());
+  position.set_z(localization.pose().linear_velocity().z());
+  obstacle_ptr_->mutable_position()->CopyFrom(position);
+
+  obstacle_ptr_->set_type(type_);
+  obstacle_ptr_->set_timestamp(localization.measurement_time());
 }
+
+PerceptionObstacle* PoseContainer::ToPerceptionObstacle() {
+  std::lock_guard<std::mutex> lock(g_mutex_);
+  return obstacle_ptr_.get();
 }
+
+}  // namespace prediction
+}  // namespace apollo

modules/prediction/container/pose/pose_container.h

@@ -22,7 +22,12 @@
 #ifndef MODULES_PREDICTION_CONTAINER_POSE_OBSTACLES_H_
 #define MODULES_PREDICTION_CONTAINER_POSE_OBSTACLES_H_
 
+#include <memory>
+#include <mutex>
+
 #include "modules/prediction/container/container.h"
+#include "modules/perception/proto/perception_obstacle.pb.h"
+#include "modules/localization/proto/localization.pb.h"
 
 namespace apollo {
 namespace prediction {
@@ -44,6 +49,26 @@ class PoseContainer : public Container {
    * @param Data message to be inserted in protobuf
    */
   void Insert(const ::google::protobuf::Message& message) override;
+
+  /**
+   * @brief Update the content in pose container by localization.
+   * @param localization The information to update the content in
+   *        the pose container.
+   */
+  void Update(const localization::LocalizationEstimate &localization);
+
+  /**
+   * @brief Transform pose to a perception obstacle.
+   * @return A pointer to a perception obstacle.
+   */
+  apollo::perception::PerceptionObstacle* ToPerceptionObstacle();
+
+ private:
+  std::unique_ptr<apollo::perception::PerceptionObstacle> obstacle_ptr_;
+
+  static std::mutex g_mutex_;
+  static int id_;
+  static apollo::perception::PerceptionObstacle::Type type_;
 };
 
 }  // namespace prediction