Perception: add tiemstamp trigger procedure in motion manager service

modules/perception/obstacle/camera/motion/BUILD

@@ -14,6 +14,8 @@ cc_library(
         "-Wno-deprecated",
     ],
     deps = [
+        "//modules/common",
+        "//modules/common:log",
         "//modules/perception/obstacle/base",
     ],
 )

modules/perception/obstacle/camera/motion/plane_motion.cc

@@ -14,6 +14,7 @@
  * limitations under the License.
  *****************************************************************************/
 
+#include "modules/common/log.h"
 #include "modules/perception/obstacle/camera/motion/plane_motion.h"
 
 namespace apollo {
@@ -52,29 +53,44 @@ void PlaneMotion::generate_motion_matrix(VehicleStatus *vehicledata) {
   vehicledata->motion = motion_2d;
 }
 
-void PlaneMotion::add_new_motion(VehicleStatus *vehicledata,
-                                 float motion_time_dif, bool image_read) {
+void PlaneMotion::accumulate_motion(VehicleStatus *vehicledata,
+                                        float motion_time_dif) {
   generate_motion_matrix(vehicledata);  // compute vehicledata.motion
+  // accumulate CAN+IMU / Localization motion
+  mat_motion_2d_image_ *= vehicledata->motion;
+  time_difference_ += motion_time_dif;
+}
 
-  // both CAN+IMU and image time stamp
-  mat_motion_2d_image_ =
-      mat_motion_2d_image_ * vehicledata->motion;  // accumulate CAN+IMU motion
-
-  time_difference_ +=
-      motion_time_dif;  // accumulate time diff before inserting into buffer
-
-  if (image_read) {
-    // image capture time stamp to insert the buffer for the accumulated motion
-    for (int k = 0; k < static_cast<int>(mot_buffer_->size()); k++) {
-      (*mot_buffer_)[k].motion *= mat_motion_2d_image_;
-    }
+void PlaneMotion::update_motion_buffer(VehicleStatus *vehicledata) {
+  for (int k = 0; k < static_cast<int>(mot_buffer_->size()); k++) {
+    (*mot_buffer_)[k].motion *= mat_motion_2d_image_;
+  }
 
-    vehicledata->time_d = time_difference_;
-    vehicledata->motion = mat_motion_2d_image_;
-    mot_buffer_->push_back(*vehicledata);  // a new image frame is added
-    mat_motion_2d_image_ =
-        Eigen::Matrix3f::Identity();  // reset image accumulated motion
-    time_difference_ = 0;             // reset the accumulated time difference
+  vehicledata->time_d = time_difference_;
+  vehicledata->motion = mat_motion_2d_image_;
+  mot_buffer_->push_back(*vehicledata);  // a new image frame is added
+  mat_motion_2d_image_ =
+      Eigen::Matrix3f::Identity();  // reset image accumulated motion
+  time_difference_ = 0;             // reset the accumulated time difference
+}
+void PlaneMotion::add_new_motion(VehicleStatus *vehicledata,
+                                 float motion_time_dif,
+                                 int motion_operation_flag) {
+  switch (motion_operation_flag) {
+    case ACCUM_MOTION:
+        accumulate_motion(vehicledata, motion_time_dif);
+        break;
+    case ACCUM_PUSH_MOTION:
+        accumulate_motion(vehicledata, motion_time_dif);
+        update_motion_buffer(vehicledata);
+        break;
+    case PUSH_ACCUM_MOTION:
+        update_motion_buffer(vehicledata);
+        accumulate_motion(vehicledata, motion_time_dif);
+        break;
+    default:
+        AERROR << "motion operation flag:wrong type";
+        return;
   }
 }
 

modules/perception/obstacle/camera/motion/plane_motion.h

@@ -37,6 +37,7 @@ class PlaneMotion {
   explicit PlaneMotion(int s, bool sync_time_stamp, float time_unit);
 
   ~PlaneMotion(void);
+  enum { ACCUM_MOTION = 0, ACCUM_PUSH_MOTION, PUSH_ACCUM_MOTION };
 
  private:
   MotionBufferPtr mot_buffer_;
@@ -48,6 +49,9 @@ class PlaneMotion {
   // motion matrix of accumulation through high sampling CAN+IMU input sequence
   void generate_motion_matrix(
       VehicleStatus *vehicledata);  // generate inverse motion
+  void accumulate_motion(VehicleStatus *vehicledata,
+                         float motion_time_dif);
+  void update_motion_buffer(VehicleStatus *vehicledata);
 
  public:
   void cleanbuffer() {
@@ -73,7 +77,7 @@ class PlaneMotion {
   // void init(int s) { set_buffer_size(s); }
 
   void add_new_motion(VehicleStatus *vehicledata, float motion_time_dif,
-                      bool image_read);
+                      int motion_operation_flag);
 
   MotionBufferPtr get_buffer() { return mot_buffer_; }
 };

modules/perception/obstacle/onboard/BUILD

@@ -159,6 +159,7 @@ cc_library(
         "motion_service.h",
     ],
     deps = [
+        ":perception_obstacle_shared_data",
         "//modules/common/adapters:adapter_manager",
         "//modules/perception/lib/config_manager",
         "//modules/perception/obstacle/camera/motion",

modules/perception/obstacle/onboard/motion_service.cc

@@ -13,7 +13,7 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  *****************************************************************************/
-
+#include<limits>
 #include "modules/perception/obstacle/onboard/motion_service.h"
 #include "modules/perception/lib/base/mutex.h"
 #include "modules/perception/onboard/event_manager.h"
@@ -31,6 +31,13 @@ bool MotionService::InitInternal() {
   vehicle_planemotion_ = new PlaneMotion(motion_buffer_size_, false,
                                          1.0f / motion_sensor_frequency_);
 
+  CHECK(shared_data_manager_ != NULL);
+  camera_shared_data_ = dynamic_cast<CameraSharedData*>(
+    shared_data_manager_->GetSharedData("CameraSharedData"));
+    if (camera_shared_data_ == nullptr) {
+      AERROR << "Failed to get CameraSharedData.";
+      return false;
+    }
   AINFO << "init MotionService success.";
   return true;
 }
@@ -68,7 +75,20 @@ void MotionService::OnLocalization(
   pre_timestamp = localization.measurement_time();
 
   // add motion to buffer
-  vehicle_planemotion_->add_new_motion(&vehicle_status, timestamp_diff, false);
+  double camera_timestamp =
+    camera_shared_data_->GetLatestTimestamp();
+  if (std::abs(pre_timestamp-camera_timestamp) <
+        std::numeric_limits<double>::epsilon()) {
+      // exactly same timestamp
+      vehicle_planemotion_->add_new_motion(&vehicle_status,
+       timestamp_diff, PlaneMotion::ACCUM_PUSH_MOTION);
+  } else if (pre_timestamp < camera_timestamp) {
+      vehicle_planemotion_->add_new_motion(&vehicle_status,
+       timestamp_diff, PlaneMotion::ACCUM_MOTION);
+  } else {
+      vehicle_planemotion_->add_new_motion(&vehicle_status,
+       timestamp_diff, PlaneMotion::PUSH_ACCUM_MOTION);
+  }
 }
 
 void MotionService::GetVehicleInformation(

modules/perception/obstacle/onboard/motion_service.h

@@ -25,7 +25,7 @@
 #include "modules/perception/obstacle/camera/motion/plane_motion.h"
 #include "modules/perception/onboard/subnode.h"
 #include "modules/perception/onboard/subnode_helper.h"
-
+#include "modules/perception/obstacle/onboard/camera_shared_data.h"
 namespace apollo {
 namespace perception {
 
@@ -55,11 +55,13 @@ class MotionService : public Subnode {
   PlaneMotion *vehicle_planemotion_ = nullptr;
   double pre_azimuth = 0;  // a invalid value
   double pre_timestamp = 0;
+  // double pre_camera_timestamp = 0;
   bool start_flag_ = false;
   const int motion_buffer_size_ = 6000;
   const int motion_sensor_frequency_ = 100;
   Mutex mutex_;
   std::list<VehicleInformation> vehicle_information_buffer_;
+  CameraSharedData* camera_shared_data_ = nullptr;
   // MotionBufferPtr motion_buffer_;
   DISALLOW_COPY_AND_ASSIGN(MotionService);
 };

modules/perception/onboard/common_shared_data.h

@@ -23,6 +23,8 @@
 #include <string>
 #include <utility>
 #include <vector>
+#include <algorithm>
+#include <limits>
 
 #include "boost/format.hpp"
 #include "gflags/gflags.h"
@@ -77,7 +79,10 @@ class CommonSharedData : public SharedData {
   CommonSharedData() {}
   virtual ~CommonSharedData() {}
 
-  bool Init() override { return true; }
+  bool Init() override {
+    latest_timestamp_ = std::numeric_limits<double>::min();
+    return true;
+  }
   // @brief: you must impl your own name func
   // @return: name of your own class
   virtual std::string name() const = 0;
@@ -103,6 +108,7 @@ class CommonSharedData : public SharedData {
 
   bool Get(const CommonSharedDataKey &key, SharedDataPtr<M> *data);
 
+  double GetLatestTimestamp() const;
   // @brief: remove shared data with the given key
   // @param [in]: key
   // @return : true or false
@@ -135,6 +141,7 @@ class CommonSharedData : public SharedData {
   Mutex mutex_;
   CommonSharedDataStat stat_;
   DataAddedTimeMap data_added_time_map_;
+  double latest_timestamp_ = std::numeric_limits<double>::min();
 
   DISALLOW_COPY_AND_ASSIGN(CommonSharedData);
 };
@@ -145,6 +152,7 @@ void CommonSharedData<M>::Reset() {
   AINFO << "Reset " << name() << ", map size: " << data_map_.size();
   data_map_.clear();
   data_added_time_map_.clear();
+  latest_timestamp_ = std::numeric_limits<double>::min();
 }
 
 template <class M>
@@ -186,7 +194,6 @@ bool CommonSharedData<M>::Add(const std::string &key,
 
   const uint64_t timestamp = ::time(NULL);
   data_added_time_map_.emplace(DataKeyTimestampPair(key, timestamp));
-
   ++stat_.add_cnt;
   return true;
 }
@@ -194,6 +201,8 @@ bool CommonSharedData<M>::Add(const std::string &key,
 template <class M>
 bool CommonSharedData<M>::Add(const CommonSharedDataKey &key,
                               const SharedDataPtr<M> &data) {
+  // update latest_timestamp for SharedData
+  latest_timestamp_ = std::max(latest_timestamp_, key.timestamp);
   return Add(key.ToString(), data);
 }
 
@@ -216,6 +225,11 @@ bool CommonSharedData<M>::Get(const CommonSharedDataKey &key,
   return Get(key.ToString(), data);
 }
 
+template <class M>
+double CommonSharedData<M>::GetLatestTimestamp() const {
+  return latest_timestamp_;
+}
+
 template <class M>
 bool CommonSharedData<M>::Remove(const std::string &key) {
   MutexLock lock(&mutex_);