Added pose container unit test

modules/prediction/container/pose/BUILD

@@ -14,4 +14,14 @@ cc_library(
     ],
 )
 
+cc_test(
+    name = "pose_container_test",
+    size = "small",
+    srcs = ["pose_container_test.cc"],
+    deps = [
+        "//modules/prediction/container/pose:pose_container",
+        "@gtest//:main",
+    ]
+)
+
 cpplint()

modules/prediction/container/pose/pose_container.cc

@@ -35,6 +35,20 @@ void PoseContainer::Insert(const ::google::protobuf::Message& message) {
 
 void PoseContainer::Update(
     const localization::LocalizationEstimate &localization) {
+  if (!localization.has_measurement_time()) {
+    AERROR << "Localization message has no timestamp ["
+           << localization.ShortDebugString() << "].";
+    return;
+  } else if (!localization.has_pose()) {
+    AERROR << "Localization message has no pose ["
+           << localization.ShortDebugString() << "].";
+  } else if (!localization.pose().has_position() ||
+             !localization.pose().has_linear_velocity()) {
+    AERROR << "Localization message has no position or linear velocity ["
+           << localization.ShortDebugString() << "].";
+    return;
+  }
+
   std::lock_guard<std::mutex> lock(g_mutex_);
   if (obstacle_ptr_.get() == nullptr) {
     obstacle_ptr_.reset(new PerceptionObstacle());
@@ -47,10 +61,10 @@ void PoseContainer::Update(
   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);
+  velocity.set_x(localization.pose().linear_velocity().x());
+  velocity.set_y(localization.pose().linear_velocity().y());
+  velocity.set_z(localization.pose().linear_velocity().z());
+  obstacle_ptr_->mutable_velocity()->CopyFrom(velocity);
 
   obstacle_ptr_->set_type(type_);
   obstacle_ptr_->set_timestamp(localization.measurement_time());

modules/prediction/container/pose/pose_container_test.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.
+ *****************************************************************************/
+
+#include "modules/prediction/container/pose/pose_container.h"
+
+#include <array>
+#include "gtest/gtest.h"
+
+#include "modules/localization/proto/localization.pb.h"
+
+namespace apollo {
+namespace prediction {
+
+using ::apollo::localization::LocalizationEstimate;
+using ::apollo::localization::Pose;
+using ::apollo::perception::PerceptionObstacle;
+
+class PoseContainerTest : public ::testing::Test {
+ public:
+  virtual void SetUp() {}
+
+ protected:
+  void InitPose(LocalizationEstimate *localization);
+
+ protected:
+  PoseContainer pose_;
+
+ private:
+  std::array<double, 2> position_{{1.0, 1.5}};
+  std::array<double, 2> velocity_{{2.0, 2.5}};
+  double timestamp_ = 3.0;
+};
+
+void PoseContainerTest::InitPose(LocalizationEstimate *localization) {
+  localization->mutable_pose()->mutable_position()->set_x(position_[0]);
+  localization->mutable_pose()->mutable_position()->set_y(position_[1]);
+  localization->mutable_pose()->mutable_linear_velocity()->set_x(
+      velocity_[0]);
+  localization->mutable_pose()->mutable_linear_velocity()->set_y(
+      velocity_[1]);
+  localization->set_measurement_time(timestamp_);
+}
+
+TEST_F(PoseContainerTest, Insertion) {
+  LocalizationEstimate localization;
+  InitPose(&localization);
+
+  pose_.Insert(localization);
+  EXPECT_DOUBLE_EQ(pose_.GetTimestamp(), 3.0);
+
+  PerceptionObstacle *obstacle = pose_.ToPerceptionObstacle();
+  EXPECT_TRUE(obstacle != nullptr);
+  EXPECT_TRUE(obstacle->has_position());
+  EXPECT_TRUE(obstacle->has_velocity());
+  EXPECT_DOUBLE_EQ(obstacle->position().x(), 1.0);
+  EXPECT_DOUBLE_EQ(obstacle->position().y(), 1.5);
+  EXPECT_DOUBLE_EQ(obstacle->velocity().x(), 2.0);
+  EXPECT_DOUBLE_EQ(obstacle->velocity().y(), 2.5);
+}
+
+}  // namespace prediction
+}  // namespace apollo