change tool/map_lanetoxodr,change move logic.

src/detection/detection_lidar_localizer_ekf/detection_lidar_localizer_ekf.pro

@@ -15,6 +15,8 @@ DEFINES += QT_DEPRECATED_WARNINGS
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
 SOURCES += \
+        ../../include/msgtype/ndtgpspos.pb.cc \
+        ../../include/msgtype/ndtpos.pb.cc \
         amathutils_lib/kalman_filter.cpp \
         amathutils_lib/time_delay_kalman_filter.cpp \
         ekf_localizer.cpp \
@@ -26,9 +28,30 @@ else: unix:!android: target.path = /opt/$${TARGET}/bin
 !isEmpty(target.path): INSTALLS += target
 
 HEADERS += \
+    ../../include/msgtype/ndtgpspos.pb.h \
+    ../../include/msgtype/ndtpos.pb.h \
     amathutils_lib/kalman_filter.hpp \
     amathutils_lib/time_delay_kalman_filter.hpp \
     ekf_localizer.h
 
 INCLUDEPATH += $$PWD/../../../thirdpartylib/amathutils_lib/include
 INCLUDEPATH += /opt/ros/melodic/include
+
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+!include(../../../include/ivboost.pri ) {
+    error( "Couldn't find the ivboost.pri file!" )
+}
+
+
+!include(../../../include/iveigen.pri ) {
+    error( "Couldn't find the iveigen.pri file!" )
+}
+

src/detection/detection_lidar_localizer_ekf/ekf_localizer.cpp

@@ -23,8 +23,9 @@
 #define DEBUG_PRINT_MAT(X) { if (show_debug_info_) { std::cout << #X << ": " << X << std::endl; } }
 
 // clang-format on
-EKFLocalizer::EKFLocalizer() : nh_(""), pnh_("~"), dim_x_(6 /* x, y, yaw, yaw_bias, vx, wz */)
+EKFLocalizer::EKFLocalizer()
 {
+    dim_x_ = 6;
     show_debug_info_ = false;
     ekf_rate_ = 50.0;
     ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1);
@@ -123,148 +124,148 @@ EKFLocalizer::EKFLocalizer() : nh_(""), pnh_("~"), dim_x_(6 /* x, y, yaw, yaw_bi
 
 EKFLocalizer::~EKFLocalizer(){};
 
-/*
- * timerCallback
- */
-void EKFLocalizer::timerCallback(const ros::TimerEvent& e)
-{
-  DEBUG_INFO("========================= timer called =========================");
-
-  /* predict model in EKF */
-  auto start = std::chrono::system_clock::now();
-  DEBUG_INFO("------------------------- start prediction -------------------------");
-  predictKinematicsModel();
-  double elapsed =
-      std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count();
-  DEBUG_INFO("[EKF] predictKinematicsModel calculation time = %f [ms]", elapsed * 1.0e-6);
-  DEBUG_INFO("------------------------- end prediction -------------------------\n");
-
-//  /* pose measurement update */
-//  if (current_pose_ptr_ != nullptr)
+///*
+// * timerCallback
+// */
+//void EKFLocalizer::timerCallback(const ros::TimerEvent& e)
+//{
+//  DEBUG_INFO("========================= timer called =========================");
+
+//  /* predict model in EKF */
+//  auto start = std::chrono::system_clock::now();
+//  DEBUG_INFO("------------------------- start prediction -------------------------");
+//  predictKinematicsModel();
+//  double elapsed =
+//      std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count();
+//  DEBUG_INFO("[EKF] predictKinematicsModel calculation time = %f [ms]", elapsed * 1.0e-6);
+//  DEBUG_INFO("------------------------- end prediction -------------------------\n");
+
+////  /* pose measurement update */
+////  if (current_pose_ptr_ != nullptr)
+////  {
+////    DEBUG_INFO("------------------------- start Pose -------------------------");
+////    start = std::chrono::system_clock::now();
+////    measurementUpdatePose(*current_pose_ptr_);
+////    elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count();
+////    DEBUG_INFO("[EKF] measurementUpdatePose calculation time = %f [ms]", elapsed * 1.0e-6);
+////    DEBUG_INFO("------------------------- end Pose -------------------------\n");
+////  }
+
+////  /* twist measurement update */
+////  if (current_twist_ptr_ != nullptr)
+////  {
+////    DEBUG_INFO("------------------------- start twist -------------------------");
+////    start = std::chrono::system_clock::now();
+////    measurementUpdateTwist(*current_twist_ptr_);
+////    elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count();
+////    DEBUG_INFO("[EKF] measurementUpdateTwist calculation time = %f [ms]", elapsed * 1.0e-6);
+////    DEBUG_INFO("------------------------- end twist -------------------------\n");
+////  }
+
+//  /* set current pose, twist */
+//  setCurrentResult();
+
+//  /* publish ekf result */
+//  publishEstimateResult();
+//}
+
+//void EKFLocalizer::showCurrentX()
+//{
+//  if (show_debug_info_)
 //  {
-//    DEBUG_INFO("------------------------- start Pose -------------------------");
-//    start = std::chrono::system_clock::now();
-//    measurementUpdatePose(*current_pose_ptr_);
-//    elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count();
-//    DEBUG_INFO("[EKF] measurementUpdatePose calculation time = %f [ms]", elapsed * 1.0e-6);
-//    DEBUG_INFO("------------------------- end Pose -------------------------\n");
+//    Eigen::MatrixXd X(dim_x_, 1);
+//    ekf_.getLatestX(X);
+//    DEBUG_PRINT_MAT(X.transpose());
 //  }
+//}
+
+///*
+// * setCurrentResult
+// */
+//void EKFLocalizer::setCurrentResult()
+//{
+//  current_ekf_pose_.header.frame_id = pose_frame_id_;
+//  current_ekf_pose_.header.stamp = ros::Time::now();
+//  current_ekf_pose_.pose.position.x = ekf_.getXelement(IDX::X);
+//  current_ekf_pose_.pose.position.y = ekf_.getXelement(IDX::Y);
 
-//  /* twist measurement update */
-//  if (current_twist_ptr_ != nullptr)
+//  tf2::Quaternion q_tf;
+//  double roll, pitch, yaw;
+//  if (current_pose_ptr_ != nullptr)
+//  {
+//    current_ekf_pose_.pose.position.z = current_pose_ptr_->pose.position.z;
+//    tf2::fromMsg(current_pose_ptr_->pose.orientation, q_tf); /* use Pose pitch and roll */
+//    tf2::Matrix3x3(q_tf).getRPY(roll, pitch, yaw);
+//  }
+//  else
 //  {
-//    DEBUG_INFO("------------------------- start twist -------------------------");
-//    start = std::chrono::system_clock::now();
-//    measurementUpdateTwist(*current_twist_ptr_);
-//    elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count();
-//    DEBUG_INFO("[EKF] measurementUpdateTwist calculation time = %f [ms]", elapsed * 1.0e-6);
-//    DEBUG_INFO("------------------------- end twist -------------------------\n");
+//    current_ekf_pose_.pose.position.z = 0.0;
+//    roll = 0;
+//    pitch = 0;
 //  }
+//  yaw = ekf_.getXelement(IDX::YAW) + ekf_.getXelement(IDX::YAWB);
+//  q_tf.setRPY(roll, pitch, yaw);
+//  tf2::convert(q_tf, current_ekf_pose_.pose.orientation);
 
-  /* set current pose, twist */
-  setCurrentResult();
+//  current_ekf_twist_.header.frame_id = "base_link";
+//  current_ekf_twist_.header.stamp = ros::Time::now();
+//  current_ekf_twist_.twist.linear.x = ekf_.getXelement(IDX::VX);
+//  current_ekf_twist_.twist.angular.z = ekf_.getXelement(IDX::WZ);
+//}
 
-  /* publish ekf result */
-  publishEstimateResult();
-}
+///*
+// * timerTFCallback
+// */
+//void EKFLocalizer::timerTFCallback(const ros::TimerEvent& e)
+//{
+//  if (current_ekf_pose_.header.frame_id == "")
+//    return;
 
-void EKFLocalizer::showCurrentX()
-{
-  if (show_debug_info_)
-  {
-    Eigen::MatrixXd X(dim_x_, 1);
-    ekf_.getLatestX(X);
-    DEBUG_PRINT_MAT(X.transpose());
-  }
-}
+//  geometry_msgs::TransformStamped transformStamped;
+//  transformStamped.header.stamp = ros::Time::now();
+//  transformStamped.header.frame_id = current_ekf_pose_.header.frame_id;
+//  transformStamped.child_frame_id = "ekf_pose";
+//  transformStamped.transform.translation.x = current_ekf_pose_.pose.position.x;
+//  transformStamped.transform.translation.y = current_ekf_pose_.pose.position.y;
+//  transformStamped.transform.translation.z = current_ekf_pose_.pose.position.z;
 
-/*
- * setCurrentResult
- */
-void EKFLocalizer::setCurrentResult()
-{
-  current_ekf_pose_.header.frame_id = pose_frame_id_;
-  current_ekf_pose_.header.stamp = ros::Time::now();
-  current_ekf_pose_.pose.position.x = ekf_.getXelement(IDX::X);
-  current_ekf_pose_.pose.position.y = ekf_.getXelement(IDX::Y);
-
-  tf2::Quaternion q_tf;
-  double roll, pitch, yaw;
-  if (current_pose_ptr_ != nullptr)
-  {
-    current_ekf_pose_.pose.position.z = current_pose_ptr_->pose.position.z;
-    tf2::fromMsg(current_pose_ptr_->pose.orientation, q_tf); /* use Pose pitch and roll */
-    tf2::Matrix3x3(q_tf).getRPY(roll, pitch, yaw);
-  }
-  else
-  {
-    current_ekf_pose_.pose.position.z = 0.0;
-    roll = 0;
-    pitch = 0;
-  }
-  yaw = ekf_.getXelement(IDX::YAW) + ekf_.getXelement(IDX::YAWB);
-  q_tf.setRPY(roll, pitch, yaw);
-  tf2::convert(q_tf, current_ekf_pose_.pose.orientation);
-
-  current_ekf_twist_.header.frame_id = "base_link";
-  current_ekf_twist_.header.stamp = ros::Time::now();
-  current_ekf_twist_.twist.linear.x = ekf_.getXelement(IDX::VX);
-  current_ekf_twist_.twist.angular.z = ekf_.getXelement(IDX::WZ);
-}
+//  transformStamped.transform.rotation.x = current_ekf_pose_.pose.orientation.x;
+//  transformStamped.transform.rotation.y = current_ekf_pose_.pose.orientation.y;
+//  transformStamped.transform.rotation.z = current_ekf_pose_.pose.orientation.z;
+//  transformStamped.transform.rotation.w = current_ekf_pose_.pose.orientation.w;
 
-/*
- * timerTFCallback
- */
-void EKFLocalizer::timerTFCallback(const ros::TimerEvent& e)
-{
-  if (current_ekf_pose_.header.frame_id == "")
-    return;
-
-  geometry_msgs::TransformStamped transformStamped;
-  transformStamped.header.stamp = ros::Time::now();
-  transformStamped.header.frame_id = current_ekf_pose_.header.frame_id;
-  transformStamped.child_frame_id = "ekf_pose";
-  transformStamped.transform.translation.x = current_ekf_pose_.pose.position.x;
-  transformStamped.transform.translation.y = current_ekf_pose_.pose.position.y;
-  transformStamped.transform.translation.z = current_ekf_pose_.pose.position.z;
-
-  transformStamped.transform.rotation.x = current_ekf_pose_.pose.orientation.x;
-  transformStamped.transform.rotation.y = current_ekf_pose_.pose.orientation.y;
-  transformStamped.transform.rotation.z = current_ekf_pose_.pose.orientation.z;
-  transformStamped.transform.rotation.w = current_ekf_pose_.pose.orientation.w;
-
-  tf_br_.sendTransform(transformStamped);
-}
+//  tf_br_.sendTransform(transformStamped);
+//}
 
-/*
- * getTransformFromTF
- */
-bool EKFLocalizer::getTransformFromTF(std::string parent_frame, std::string child_frame,
-                                      geometry_msgs::TransformStamped& transform)
-{
-  tf2_ros::Buffer tf_buffer;
-  tf2_ros::TransformListener tf_listener(tf_buffer);
-  ros::Duration(0.1).sleep();
-  if (parent_frame.front() == '/')
-    parent_frame.erase(0, 1);
-  if (child_frame.front() == '/')
-    child_frame.erase(0, 1);
-
-  for (int i = 0; i < 50; ++i)
-  {
-    try
-    {
-      transform = tf_buffer.lookupTransform(parent_frame, child_frame, ros::Time(0));
-      return true;
-    }
-    catch (tf2::TransformException& ex)
-    {
-      ROS_WARN("%s", ex.what());
-      ros::Duration(0.1).sleep();
-    }
-  }
-  return false;
-}
+///*
+// * getTransformFromTF
+// */
+//bool EKFLocalizer::getTransformFromTF(std::string parent_frame, std::string child_frame,
+//                                      geometry_msgs::TransformStamped& transform)
+//{
+//  tf2_ros::Buffer tf_buffer;
+//  tf2_ros::TransformListener tf_listener(tf_buffer);
+//  ros::Duration(0.1).sleep();
+//  if (parent_frame.front() == '/')
+//    parent_frame.erase(0, 1);
+//  if (child_frame.front() == '/')
+//    child_frame.erase(0, 1);
+
+//  for (int i = 0; i < 50; ++i)
+//  {
+//    try
+//    {
+//      transform = tf_buffer.lookupTransform(parent_frame, child_frame, ros::Time(0));
+//      return true;
+//    }
+//    catch (tf2::TransformException& ex)
+//    {
+//      ROS_WARN("%s", ex.what());
+//      ros::Duration(0.1).sleep();
+//    }
+//  }
+//  return false;
+//}
 
 ///*
 // * callbackInitialPose
@@ -472,216 +473,216 @@ void EKFLocalizer::predictKinematicsModel()
 /*
  * measurementUpdatePose
  */
-void EKFLocalizer::measurementUpdatePose(const geometry_msgs::PoseStamped& pose)
-{
-  if (pose.header.frame_id != pose_frame_id_)
-  {
-    ROS_WARN_DELAYED_THROTTLE(2, "pose frame_id is %s, but pose_frame is set as %s. They must be same.",
-                              pose.header.frame_id.c_str(), pose_frame_id_.c_str());
-  }
-  Eigen::MatrixXd X_curr(dim_x_, 1);  // curent state
-  ekf_.getLatestX(X_curr);
-  DEBUG_PRINT_MAT(X_curr.transpose());
+//void EKFLocalizer::measurementUpdatePose(const geometry_msgs::PoseStamped& pose)
+//{
+//  if (pose.header.frame_id != pose_frame_id_)
+//  {
+//    ROS_WARN_DELAYED_THROTTLE(2, "pose frame_id is %s, but pose_frame is set as %s. They must be same.",
+//                              pose.header.frame_id.c_str(), pose_frame_id_.c_str());
+//  }
+//  Eigen::MatrixXd X_curr(dim_x_, 1);  // curent state
+//  ekf_.getLatestX(X_curr);
+//  DEBUG_PRINT_MAT(X_curr.transpose());
 
-  constexpr int dim_y = 3;  // pos_x, pos_y, yaw, depending on Pose output
-  const ros::Time t_curr = ros::Time::now();
+//  constexpr int dim_y = 3;  // pos_x, pos_y, yaw, depending on Pose output
+//  const ros::Time t_curr = ros::Time::now();
 
-  /* Calculate delay step */
-  double delay_time = (t_curr - pose.header.stamp).toSec() + pose_additional_delay_;
-  if (delay_time < 0.0)
-  {
-    delay_time = 0.0;
-    ROS_WARN_DELAYED_THROTTLE(1.0, "Pose time stamp is inappropriate, set delay to 0[s]. delay = %f", delay_time);
-  }
-  int delay_step = std::roundf(delay_time / ekf_dt_);
-  if (delay_step > extend_state_step_ - 1)
-  {
-    ROS_WARN_DELAYED_THROTTLE(1.0,
-                              "Pose delay exceeds the compensation limit, ignored. delay: %f[s], limit = "
-                              "extend_state_step * ekf_dt : %f [s]",
-                              delay_time, extend_state_step_ * ekf_dt_);
-    return;
-  }
-  DEBUG_INFO("delay_time: %f [s]", delay_time);
+//  /* Calculate delay step */
+//  double delay_time = (t_curr - pose.header.stamp).toSec() + pose_additional_delay_;
+//  if (delay_time < 0.0)
+//  {
+//    delay_time = 0.0;
+//    ROS_WARN_DELAYED_THROTTLE(1.0, "Pose time stamp is inappropriate, set delay to 0[s]. delay = %f", delay_time);
+//  }
+//  int delay_step = std::roundf(delay_time / ekf_dt_);
+//  if (delay_step > extend_state_step_ - 1)
+//  {
+//    ROS_WARN_DELAYED_THROTTLE(1.0,
+//                              "Pose delay exceeds the compensation limit, ignored. delay: %f[s], limit = "
+//                              "extend_state_step * ekf_dt : %f [s]",
+//                              delay_time, extend_state_step_ * ekf_dt_);
+//    return;
+//  }
+//  DEBUG_INFO("delay_time: %f [s]", delay_time);
 
-  /* Set yaw */
-  const double yaw_curr = ekf_.getXelement((unsigned int)(delay_step * dim_x_ + IDX::YAW));
-  double yaw = tf2::getYaw(pose.pose.orientation);
-  const double ekf_yaw = ekf_.getXelement(delay_step * dim_x_ + IDX::YAW);
-  const double yaw_error = normalizeYaw(yaw - ekf_yaw);  // normalize the error not to exceed 2 pi
-  yaw = yaw_error + ekf_yaw;
+//  /* Set yaw */
+//  const double yaw_curr = ekf_.getXelement((unsigned int)(delay_step * dim_x_ + IDX::YAW));
+//  double yaw = tf2::getYaw(pose.pose.orientation);
+//  const double ekf_yaw = ekf_.getXelement(delay_step * dim_x_ + IDX::YAW);
+//  const double yaw_error = normalizeYaw(yaw - ekf_yaw);  // normalize the error not to exceed 2 pi
+//  yaw = yaw_error + ekf_yaw;
 
-  /* Set measurement matrix */
-  Eigen::MatrixXd y(dim_y, 1);
-  y << pose.pose.position.x, pose.pose.position.y, yaw;
+//  /* Set measurement matrix */
+//  Eigen::MatrixXd y(dim_y, 1);
+//  y << pose.pose.position.x, pose.pose.position.y, yaw;
 
-  if (isnan(y.array()).any() || isinf(y.array()).any())
-  {
-    ROS_WARN("[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message.");
-    return;
-  }
+//  if (isnan(y.array()).any() || isinf(y.array()).any())
+//  {
+//    ROS_WARN("[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message.");
+//    return;
+//  }
 
-  /* Gate */
-  Eigen::MatrixXd y_ekf(dim_y, 1);
-  y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::X), ekf_.getXelement(delay_step * dim_x_ + IDX::Y), ekf_yaw;
-  Eigen::MatrixXd P_curr, P_y;
-  ekf_.getLatestP(P_curr);
-  P_y = P_curr.block(0, 0, dim_y, dim_y);
-  if (!mahalanobisGate(pose_gate_dist_, y_ekf, y, P_y))
-  {
-    ROS_WARN_DELAYED_THROTTLE(2.0, "[EKF] Pose measurement update, mahalanobis distance is over limit. ignore "
-                                   "measurement data.");
-    return;
-  }
+//  /* Gate */
+//  Eigen::MatrixXd y_ekf(dim_y, 1);
+//  y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::X), ekf_.getXelement(delay_step * dim_x_ + IDX::Y), ekf_yaw;
+//  Eigen::MatrixXd P_curr, P_y;
+//  ekf_.getLatestP(P_curr);
+//  P_y = P_curr.block(0, 0, dim_y, dim_y);
+//  if (!mahalanobisGate(pose_gate_dist_, y_ekf, y, P_y))
+//  {
+//    ROS_WARN_DELAYED_THROTTLE(2.0, "[EKF] Pose measurement update, mahalanobis distance is over limit. ignore "
+//                                   "measurement data.");
+//    return;
+//  }
 
-  DEBUG_PRINT_MAT(y.transpose());
-  DEBUG_PRINT_MAT(y_ekf.transpose());
-  DEBUG_PRINT_MAT((y - y_ekf).transpose());
+//  DEBUG_PRINT_MAT(y.transpose());
+//  DEBUG_PRINT_MAT(y_ekf.transpose());
+//  DEBUG_PRINT_MAT((y - y_ekf).transpose());
 
-  /* Set measurement matrix */
-  Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_);
-  C(0, IDX::X) = 1.0;    // for pos x
-  C(1, IDX::Y) = 1.0;    // for pos y
-  C(2, IDX::YAW) = 1.0;  // for yaw
+//  /* Set measurement matrix */
+//  Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_);
+//  C(0, IDX::X) = 1.0;    // for pos x
+//  C(1, IDX::Y) = 1.0;    // for pos y
+//  C(2, IDX::YAW) = 1.0;  // for yaw
 
-  /* Set measurement noise covariancs */
-  Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
-  if (use_pose_with_covariance_)
-  {
-    R(0, 0) = current_pose_covariance_.at(0);   // x - x
-    R(0, 1) = current_pose_covariance_.at(1);   // x - y
-    R(0, 2) = current_pose_covariance_.at(5);   // x - yaw
-    R(1, 0) = current_pose_covariance_.at(6);   // y - x
-    R(1, 1) = current_pose_covariance_.at(7);   // y - y
-    R(1, 2) = current_pose_covariance_.at(11);  // y - yaw
-    R(2, 0) = current_pose_covariance_.at(30);  // yaw - x
-    R(2, 1) = current_pose_covariance_.at(31);  // yaw - y
-    R(2, 2) = current_pose_covariance_.at(35);  // yaw - yaw
-  }
-  else
-  {
-    const double ekf_yaw = ekf_.getXelement(IDX::YAW);
-    const double vx = ekf_.getXelement(IDX::VX);
-    const double wz = ekf_.getXelement(IDX::WZ);
-    const double cov_pos_x = std::pow(pose_measure_uncertainty_time_ * vx * cos(ekf_yaw), 2.0);
-    const double cov_pos_y = std::pow(pose_measure_uncertainty_time_ * vx * sin(ekf_yaw), 2.0);
-    const double cov_yaw = std::pow(pose_measure_uncertainty_time_ * wz, 2.0);
-    R(0, 0) = std::pow(pose_stddev_x_, 2) + cov_pos_x;  // pos_x
-    R(1, 1) = std::pow(pose_stddev_y_, 2) + cov_pos_y;  // pos_y
-    R(2, 2) = std::pow(pose_stddev_yaw_, 2) + cov_yaw;  // yaw
-  }
+//  /* Set measurement noise covariancs */
+//  Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
+//  if (use_pose_with_covariance_)
+//  {
+//    R(0, 0) = current_pose_covariance_.at(0);   // x - x
+//    R(0, 1) = current_pose_covariance_.at(1);   // x - y
+//    R(0, 2) = current_pose_covariance_.at(5);   // x - yaw
+//    R(1, 0) = current_pose_covariance_.at(6);   // y - x
+//    R(1, 1) = current_pose_covariance_.at(7);   // y - y
+//    R(1, 2) = current_pose_covariance_.at(11);  // y - yaw
+//    R(2, 0) = current_pose_covariance_.at(30);  // yaw - x
+//    R(2, 1) = current_pose_covariance_.at(31);  // yaw - y
+//    R(2, 2) = current_pose_covariance_.at(35);  // yaw - yaw
+//  }
+//  else
+//  {
+//    const double ekf_yaw = ekf_.getXelement(IDX::YAW);
+//    const double vx = ekf_.getXelement(IDX::VX);
+//    const double wz = ekf_.getXelement(IDX::WZ);
+//    const double cov_pos_x = std::pow(pose_measure_uncertainty_time_ * vx * cos(ekf_yaw), 2.0);
+//    const double cov_pos_y = std::pow(pose_measure_uncertainty_time_ * vx * sin(ekf_yaw), 2.0);
+//    const double cov_yaw = std::pow(pose_measure_uncertainty_time_ * wz, 2.0);
+//    R(0, 0) = std::pow(pose_stddev_x_, 2) + cov_pos_x;  // pos_x
+//    R(1, 1) = std::pow(pose_stddev_y_, 2) + cov_pos_y;  // pos_y
+//    R(2, 2) = std::pow(pose_stddev_yaw_, 2) + cov_yaw;  // yaw
+//  }
 
-  /* In order to avoid a large change at the time of updating, measuremeent update is performed by dividing at every
-   * step. */
-  R *= (ekf_rate_ / pose_rate_);
+//  /* In order to avoid a large change at the time of updating, measuremeent update is performed by dividing at every
+//   * step. */
+//  R *= (ekf_rate_ / pose_rate_);
 
-  ekf_.updateWithDelay(y, C, R, delay_step);
+//  ekf_.updateWithDelay(y, C, R, delay_step);
 
-  // debug
-  Eigen::MatrixXd X_result(dim_x_, 1);
-  ekf_.getLatestX(X_result);
-  DEBUG_PRINT_MAT(X_result.transpose());
-  DEBUG_PRINT_MAT((X_result - X_curr).transpose());
-}
+//  // debug
+//  Eigen::MatrixXd X_result(dim_x_, 1);
+//  ekf_.getLatestX(X_result);
+//  DEBUG_PRINT_MAT(X_result.transpose());
+//  DEBUG_PRINT_MAT((X_result - X_curr).transpose());
+//}
 
-/*
- * measurementUpdateTwist
- */
-void EKFLocalizer::measurementUpdateTwist(const geometry_msgs::TwistStamped& twist)
-{
-  if (twist.header.frame_id != "base_link")
-  {
-    ROS_WARN_DELAYED_THROTTLE(2.0, "twist frame_id must be base_link");
-  }
+///*
+// * measurementUpdateTwist
+// */
+//void EKFLocalizer::measurementUpdateTwist(const geometry_msgs::TwistStamped& twist)
+//{
+//  if (twist.header.frame_id != "base_link")
+//  {
+//    ROS_WARN_DELAYED_THROTTLE(2.0, "twist frame_id must be base_link");
+//  }
 
-  Eigen::MatrixXd X_curr(dim_x_, 1);  // curent state
-  ekf_.getLatestX(X_curr);
-  DEBUG_PRINT_MAT(X_curr.transpose());
+//  Eigen::MatrixXd X_curr(dim_x_, 1);  // curent state
+//  ekf_.getLatestX(X_curr);
+//  DEBUG_PRINT_MAT(X_curr.transpose());
 
-  constexpr int dim_y = 2;  // vx, wz
-  const ros::Time t_curr = ros::Time::now();
+//  constexpr int dim_y = 2;  // vx, wz
+//  const ros::Time t_curr = ros::Time::now();
 
-  /* Calculate delay step */
-  double delay_time = (t_curr - twist.header.stamp).toSec() + twist_additional_delay_;
-  if (delay_time < 0.0)
-  {
-    ROS_WARN_DELAYED_THROTTLE(1.0, "Twist time stamp is inappropriate (delay = %f [s]), set delay to 0[s].",
-                              delay_time);
-    delay_time = 0.0;
-  }
-  int delay_step = std::roundf(delay_time / ekf_dt_);
-  if (delay_step > extend_state_step_ - 1)
-  {
-    ROS_WARN_DELAYED_THROTTLE(1.0,
-                              "Twist delay exceeds the compensation limit, ignored. delay: %f[s], limit = "
-                              "extend_state_step * ekf_dt : %f [s]",
-                              delay_time, extend_state_step_ * ekf_dt_);
-    return;
-  }
-  DEBUG_INFO("delay_time: %f [s]", delay_time);
+//  /* Calculate delay step */
+//  double delay_time = (t_curr - twist.header.stamp).toSec() + twist_additional_delay_;
+//  if (delay_time < 0.0)
+//  {
+//    ROS_WARN_DELAYED_THROTTLE(1.0, "Twist time stamp is inappropriate (delay = %f [s]), set delay to 0[s].",
+//                              delay_time);
+//    delay_time = 0.0;
+//  }
+//  int delay_step = std::roundf(delay_time / ekf_dt_);
+//  if (delay_step > extend_state_step_ - 1)
+//  {
+//    ROS_WARN_DELAYED_THROTTLE(1.0,
+//                              "Twist delay exceeds the compensation limit, ignored. delay: %f[s], limit = "
+//                              "extend_state_step * ekf_dt : %f [s]",
+//                              delay_time, extend_state_step_ * ekf_dt_);
+//    return;
+//  }
+//  DEBUG_INFO("delay_time: %f [s]", delay_time);
 
-  /* Set measurement matrix */
-  Eigen::MatrixXd y(dim_y, 1);
-  y << twist.twist.linear.x, twist.twist.angular.z;
+//  /* Set measurement matrix */
+//  Eigen::MatrixXd y(dim_y, 1);
+//  y << twist.twist.linear.x, twist.twist.angular.z;
 
-  if (isnan(y.array()).any() || isinf(y.array()).any())
-  {
-    ROS_WARN("[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message.");
-    return;
-  }
+//  if (isnan(y.array()).any() || isinf(y.array()).any())
+//  {
+//    ROS_WARN("[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message.");
+//    return;
+//  }
 
-  /* Gate */
-  Eigen::MatrixXd y_ekf(dim_y, 1);
-  y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::VX), ekf_.getXelement(delay_step * dim_x_ + IDX::WZ);
-  Eigen::MatrixXd P_curr, P_y;
-  ekf_.getLatestP(P_curr);
-  P_y = P_curr.block(4, 4, dim_y, dim_y);
-  if (!mahalanobisGate(twist_gate_dist_, y_ekf, y, P_y))
-  {
-    ROS_WARN_DELAYED_THROTTLE(2.0, "[EKF] Twist measurement update, mahalanobis distance is over limit. ignore "
-                                   "measurement data.");
-    return;
-  }
+//  /* Gate */
+//  Eigen::MatrixXd y_ekf(dim_y, 1);
+//  y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::VX), ekf_.getXelement(delay_step * dim_x_ + IDX::WZ);
+//  Eigen::MatrixXd P_curr, P_y;
+//  ekf_.getLatestP(P_curr);
+//  P_y = P_curr.block(4, 4, dim_y, dim_y);
+//  if (!mahalanobisGate(twist_gate_dist_, y_ekf, y, P_y))
+//  {
+//    ROS_WARN_DELAYED_THROTTLE(2.0, "[EKF] Twist measurement update, mahalanobis distance is over limit. ignore "
+//                                   "measurement data.");
+//    return;
+//  }
 
-  DEBUG_PRINT_MAT(y.transpose());
-  DEBUG_PRINT_MAT(y_ekf.transpose());
-  DEBUG_PRINT_MAT((y - y_ekf).transpose());
+//  DEBUG_PRINT_MAT(y.transpose());
+//  DEBUG_PRINT_MAT(y_ekf.transpose());
+//  DEBUG_PRINT_MAT((y - y_ekf).transpose());
 
-  /* Set measurement matrix */
-  Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_);
-  C(0, IDX::VX) = 1.0;  // for vx
-  C(1, IDX::WZ) = 1.0;  // for wz
+//  /* Set measurement matrix */
+//  Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_);
+//  C(0, IDX::VX) = 1.0;  // for vx
+//  C(1, IDX::WZ) = 1.0;  // for wz
 
-  /* Set measurement noise covariancs */
-  Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
-  if (use_twist_with_covariance_)
-  {
-    R(0, 0) = current_twist_covariance_.at(0);   // vx - vx
-    R(0, 1) = current_twist_covariance_.at(5);   // vx - wz
-    R(1, 0) = current_twist_covariance_.at(30);  // wz - vx
-    R(1, 1) = current_twist_covariance_.at(35);  // wz - wz
-  }
-  else
-  {
-    R(0, 0) = twist_stddev_vx_ * twist_stddev_vx_ * ekf_dt_;  // for vx
-    R(1, 1) = twist_stddev_wz_ * twist_stddev_wz_ * ekf_dt_;  // for wz
-  }
+//  /* Set measurement noise covariancs */
+//  Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
+//  if (use_twist_with_covariance_)
+//  {
+//    R(0, 0) = current_twist_covariance_.at(0);   // vx - vx
+//    R(0, 1) = current_twist_covariance_.at(5);   // vx - wz
+//    R(1, 0) = current_twist_covariance_.at(30);  // wz - vx
+//    R(1, 1) = current_twist_covariance_.at(35);  // wz - wz
+//  }
+//  else
+//  {
+//    R(0, 0) = twist_stddev_vx_ * twist_stddev_vx_ * ekf_dt_;  // for vx
+//    R(1, 1) = twist_stddev_wz_ * twist_stddev_wz_ * ekf_dt_;  // for wz
+//  }
 
-  /* In order to avoid a large change by update, measurement update is performed by dividing at every step. */
-  R *= (ekf_rate_ / twist_rate_);
+//  /* In order to avoid a large change by update, measurement update is performed by dividing at every step. */
+//  R *= (ekf_rate_ / twist_rate_);
 
-  ekf_.updateWithDelay(y, C, R, delay_step);
+//  ekf_.updateWithDelay(y, C, R, delay_step);
 
-  // debug
-  Eigen::MatrixXd X_result(dim_x_, 1);
-  ekf_.getLatestX(X_result);
-  DEBUG_PRINT_MAT(X_result.transpose());
-  DEBUG_PRINT_MAT((X_result - X_curr).transpose());
-};
+//  // debug
+//  Eigen::MatrixXd X_result(dim_x_, 1);
+//  ekf_.getLatestX(X_result);
+//  DEBUG_PRINT_MAT(X_result.transpose());
+//  DEBUG_PRINT_MAT((X_result - X_curr).transpose());
+//};
 
-/*
- * mahalanobisGate
- */
+///*
+// * mahalanobisGate
+// */
 bool EKFLocalizer::mahalanobisGate(const double& dist_max, const Eigen::MatrixXd& x, const Eigen::MatrixXd& obj_x,
                                    const Eigen::MatrixXd& cov)
 {
@@ -695,79 +696,270 @@ bool EKFLocalizer::mahalanobisGate(const double& dist_max, const Eigen::MatrixXd
   return true;
 }
 
-/*
- * publishEstimateResult
- */
-void EKFLocalizer::publishEstimateResult()
-{
-  ros::Time current_time = ros::Time::now();
-  Eigen::MatrixXd X(dim_x_, 1);
-  Eigen::MatrixXd P(dim_x_, dim_x_);
-  ekf_.getLatestX(X);
-  ekf_.getLatestP(P);
-
-  /* publish latest pose */
-  pub_pose_.publish(current_ekf_pose_);
-
-  /* publish latest pose with covariance */
-  geometry_msgs::PoseWithCovarianceStamped pose_cov;
-  pose_cov.header.stamp = current_time;
-  pose_cov.header.frame_id = current_ekf_pose_.header.frame_id;
-  pose_cov.pose.pose = current_ekf_pose_.pose;
-  pose_cov.pose.covariance[0] = P(IDX::X, IDX::X);
-  pose_cov.pose.covariance[1] = P(IDX::X, IDX::Y);
-  pose_cov.pose.covariance[5] = P(IDX::X, IDX::YAW);
-  pose_cov.pose.covariance[6] = P(IDX::Y, IDX::X);
-  pose_cov.pose.covariance[7] = P(IDX::Y, IDX::Y);
-  pose_cov.pose.covariance[11] = P(IDX::Y, IDX::YAW);
-  pose_cov.pose.covariance[30] = P(IDX::YAW, IDX::X);
-  pose_cov.pose.covariance[31] = P(IDX::YAW, IDX::Y);
-  pose_cov.pose.covariance[35] = P(IDX::YAW, IDX::YAW);
-  pub_pose_cov_.publish(pose_cov);
-
-  /* publish latest twist */
-  pub_twist_.publish(current_ekf_twist_);
-
-  /* publish latest twist with covariance */
-  geometry_msgs::TwistWithCovarianceStamped twist_cov;
-  twist_cov.header.stamp = current_time;
-  twist_cov.header.frame_id = current_ekf_twist_.header.frame_id;
-  twist_cov.twist.twist = current_ekf_twist_.twist;
-  twist_cov.twist.covariance[0] = P(IDX::VX, IDX::VX);
-  twist_cov.twist.covariance[5] = P(IDX::VX, IDX::WZ);
-  twist_cov.twist.covariance[30] = P(IDX::WZ, IDX::VX);
-  twist_cov.twist.covariance[35] = P(IDX::WZ, IDX::WZ);
-  pub_twist_cov_.publish(twist_cov);
-
-
-  /* publish yaw bias */
-  std_msgs::Float64 yawb;
-  yawb.data = X(IDX::YAWB);
-  pub_yaw_bias_.publish(yawb);
-
-  /* debug measured pose */
-  if (current_pose_ptr_ != nullptr)
-  {
-    geometry_msgs::PoseStamped p;
-    p = *current_pose_ptr_;
-    p.header.stamp = current_time;
-    pub_measured_pose_.publish(p);
-  }
+///*
+// * publishEstimateResult
+// */
+//void EKFLocalizer::publishEstimateResult()
+//{
+//  ros::Time current_time = ros::Time::now();
+//  Eigen::MatrixXd X(dim_x_, 1);
+//  Eigen::MatrixXd P(dim_x_, dim_x_);
+//  ekf_.getLatestX(X);
+//  ekf_.getLatestP(P);
+
+//  /* publish latest pose */
+//  pub_pose_.publish(current_ekf_pose_);
+
+//  /* publish latest pose with covariance */
+//  geometry_msgs::PoseWithCovarianceStamped pose_cov;
+//  pose_cov.header.stamp = current_time;
+//  pose_cov.header.frame_id = current_ekf_pose_.header.frame_id;
+//  pose_cov.pose.pose = current_ekf_pose_.pose;
+//  pose_cov.pose.covariance[0] = P(IDX::X, IDX::X);
+//  pose_cov.pose.covariance[1] = P(IDX::X, IDX::Y);
+//  pose_cov.pose.covariance[5] = P(IDX::X, IDX::YAW);
+//  pose_cov.pose.covariance[6] = P(IDX::Y, IDX::X);
+//  pose_cov.pose.covariance[7] = P(IDX::Y, IDX::Y);
+//  pose_cov.pose.covariance[11] = P(IDX::Y, IDX::YAW);
+//  pose_cov.pose.covariance[30] = P(IDX::YAW, IDX::X);
+//  pose_cov.pose.covariance[31] = P(IDX::YAW, IDX::Y);
+//  pose_cov.pose.covariance[35] = P(IDX::YAW, IDX::YAW);
+//  pub_pose_cov_.publish(pose_cov);
+
+//  /* publish latest twist */
+//  pub_twist_.publish(current_ekf_twist_);
+
+//  /* publish latest twist with covariance */
+//  geometry_msgs::TwistWithCovarianceStamped twist_cov;
+//  twist_cov.header.stamp = current_time;
+//  twist_cov.header.frame_id = current_ekf_twist_.header.frame_id;
+//  twist_cov.twist.twist = current_ekf_twist_.twist;
+//  twist_cov.twist.covariance[0] = P(IDX::VX, IDX::VX);
+//  twist_cov.twist.covariance[5] = P(IDX::VX, IDX::WZ);
+//  twist_cov.twist.covariance[30] = P(IDX::WZ, IDX::VX);
+//  twist_cov.twist.covariance[35] = P(IDX::WZ, IDX::WZ);
+//  pub_twist_cov_.publish(twist_cov);
+
+
+//  /* publish yaw bias */
+//  std_msgs::Float64 yawb;
+//  yawb.data = X(IDX::YAWB);
+//  pub_yaw_bias_.publish(yawb);
+
+//  /* debug measured pose */
+//  if (current_pose_ptr_ != nullptr)
+//  {
+//    geometry_msgs::PoseStamped p;
+//    p = *current_pose_ptr_;
+//    p.header.stamp = current_time;
+//    pub_measured_pose_.publish(p);
+//  }
 
-  /* debug publish */
-  double RAD2DEG = 180.0 / 3.141592;
-  double pose_yaw = 0.0;
-  if (current_pose_ptr_ != nullptr)
-    pose_yaw = tf2::getYaw(current_pose_ptr_->pose.orientation) * RAD2DEG;
-
-  std_msgs::Float64MultiArray msg;
-  msg.data.push_back(X(IDX::YAW) * RAD2DEG);   // [0] ekf yaw angle
-  msg.data.push_back(pose_yaw);                // [1] measurement yaw angle
-  msg.data.push_back(X(IDX::YAWB) * RAD2DEG);  // [2] yaw bias
-  pub_debug_.publish(msg);
-}
+//  /* debug publish */
+//  double RAD2DEG = 180.0 / 3.141592;
+//  double pose_yaw = 0.0;
+//  if (current_pose_ptr_ != nullptr)
+//    pose_yaw = tf2::getYaw(current_pose_ptr_->pose.orientation) * RAD2DEG;
+
+//  std_msgs::Float64MultiArray msg;
+//  msg.data.push_back(X(IDX::YAW) * RAD2DEG);   // [0] ekf yaw angle
+//  msg.data.push_back(pose_yaw);                // [1] measurement yaw angle
+//  msg.data.push_back(X(IDX::YAWB) * RAD2DEG);  // [2] yaw bias
+//  pub_debug_.publish(msg);
+//}
 
 double EKFLocalizer::normalizeYaw(const double& yaw)
 {
   return std::atan2(std::sin(yaw), std::cos(yaw));
 }
+
+void EKFLocalizer::UpdateGPS(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void )&index;
+    (void )&dt;
+    (void )strmemname;
+
+    if(nSize<1)return;
+
+    iv::lidar::ndtpos xndtpos;
+    if(!xndtpos.ParseFromArray(strdata,nSize))
+    {
+       std::cout<<"EKFLocalizer::UpdateGPS error."<<std::endl;
+       return;
+    }
+
+    mMutexCurPose.lock();
+    mCurPose.x = xndtpos.pose_x();
+    mCurPose.y = xndtpos.pose_y();
+    mCurPose.z = xndtpos.pose_z();
+    mCurPose.pitch = xndtpos.pose_pitch();
+    mCurPose.roll = xndtpos.pose_roll();
+    mCurPose.yaw = xndtpos.pose_yaw();
+    mbCurPoseUpdate = true;
+    mMutexCurPose.unlock();
+
+
+}
+
+void EKFLocalizer::measurementUpdatePose()
+{
+    if(mbCurPoseUpdate == false)
+    {
+        return;
+    }
+
+    iv::pose xpose;
+    mMutexCurPose.lock();
+    xpose = mCurPose;
+    mbCurPoseUpdate = false;
+    mMutexCurPose.unlock();
+      Eigen::MatrixXd X_curr(dim_x_, 1);  // curent state
+      ekf_.getLatestX(X_curr);
+      DEBUG_PRINT_MAT(X_curr.transpose());
+
+      constexpr int dim_y = 3;  // pos_x, pos_y, yaw, depending on Pose output
+ //     const ros::Time t_curr = ros::Time::now();
+
+      /* Calculate delay step */
+      double delay_time =pose_additional_delay_;// (t_curr - pose.header.stamp).toSec() + pose_additional_delay_;
+      if (delay_time < 0.0)
+      {
+        delay_time = 0.0;
+//        ROS_WARN_DELAYED_THROTTLE(1.0, "Pose time stamp is inappropriate, set delay to 0[s]. delay = %f", delay_time);
+      }
+      int delay_step = std::roundf(delay_time / ekf_dt_);
+      if (delay_step > extend_state_step_ - 1)
+      {
+//        ROS_WARN_DELAYED_THROTTLE(1.0,
+//                                  "Pose delay exceeds the compensation limit, ignored. delay: %f[s], limit = "
+//                                  "extend_state_step * ekf_dt : %f [s]",
+//                                  delay_time, extend_state_step_ * ekf_dt_);
+          qDebug("delay error.");
+        return;
+      }
+      DEBUG_INFO("delay_time: %f [s]", delay_time);
+
+      /* Set yaw */
+      const double yaw_curr = ekf_.getXelement((unsigned int)(delay_step * dim_x_ + IDX::YAW));
+//      double yaw = tf2::getYaw(pose.pose.orientation);
+      double yaw = xpose.yaw;
+      const double ekf_yaw = ekf_.getXelement(delay_step * dim_x_ + IDX::YAW);
+      const double yaw_error = normalizeYaw(yaw - ekf_yaw);  // normalize the error not to exceed 2 pi
+      yaw = yaw_error + ekf_yaw;
+
+      /* Set measurement matrix */
+      Eigen::MatrixXd y(dim_y, 1);
+//      y << pose.pose.position.x, pose.pose.position.y, yaw;
+      y << xpose.x, xpose.y, yaw;
+
+      if (isnan(y.array()).any() || isinf(y.array()).any())
+      {
+//        ROS_WARN("[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message.");
+        qDebug("[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message.");
+        return;
+      }
+
+      /* Gate */
+      Eigen::MatrixXd y_ekf(dim_y, 1);
+      y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::X), ekf_.getXelement(delay_step * dim_x_ + IDX::Y), ekf_yaw;
+      Eigen::MatrixXd P_curr, P_y;
+      ekf_.getLatestP(P_curr);
+      P_y = P_curr.block(0, 0, dim_y, dim_y);
+      if (!mahalanobisGate(pose_gate_dist_, y_ekf, y, P_y))
+      {
+//        ROS_WARN_DELAYED_THROTTLE(2.0, "[EKF] Pose measurement update, mahalanobis distance is over limit. ignore "
+//                                       "measurement data.");
+        qDebug("[EKF] Pose measurement update, mahalanobis distance is over limit. ignore "
+                                       "measurement data.");
+        return;
+      }
+
+      DEBUG_PRINT_MAT(y.transpose());
+      DEBUG_PRINT_MAT(y_ekf.transpose());
+      DEBUG_PRINT_MAT((y - y_ekf).transpose());
+
+      /* Set measurement matrix */
+      Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_);
+      C(0, IDX::X) = 1.0;    // for pos x
+      C(1, IDX::Y) = 1.0;    // for pos y
+      C(2, IDX::YAW) = 1.0;  // for yaw
+
+      /* Set measurement noise covariancs */
+      Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
+      if (use_pose_with_covariance_)
+      {
+        R(0, 0) = current_pose_covariance_.at(0);   // x - x
+        R(0, 1) = current_pose_covariance_.at(1);   // x - y
+        R(0, 2) = current_pose_covariance_.at(5);   // x - yaw
+        R(1, 0) = current_pose_covariance_.at(6);   // y - x
+        R(1, 1) = current_pose_covariance_.at(7);   // y - y
+        R(1, 2) = current_pose_covariance_.at(11);  // y - yaw
+        R(2, 0) = current_pose_covariance_.at(30);  // yaw - x
+        R(2, 1) = current_pose_covariance_.at(31);  // yaw - y
+        R(2, 2) = current_pose_covariance_.at(35);  // yaw - yaw
+      }
+      else
+      {
+        const double ekf_yaw = ekf_.getXelement(IDX::YAW);
+        const double vx = ekf_.getXelement(IDX::VX);
+        const double wz = ekf_.getXelement(IDX::WZ);
+        const double cov_pos_x = std::pow(pose_measure_uncertainty_time_ * vx * cos(ekf_yaw), 2.0);
+        const double cov_pos_y = std::pow(pose_measure_uncertainty_time_ * vx * sin(ekf_yaw), 2.0);
+        const double cov_yaw = std::pow(pose_measure_uncertainty_time_ * wz, 2.0);
+        R(0, 0) = std::pow(pose_stddev_x_, 2) + cov_pos_x;  // pos_x
+        R(1, 1) = std::pow(pose_stddev_y_, 2) + cov_pos_y;  // pos_y
+        R(2, 2) = std::pow(pose_stddev_yaw_, 2) + cov_yaw;  // yaw
+      }
+
+      /* In order to avoid a large change at the time of updating, measuremeent update is performed by dividing at every
+       * step. */
+      R *= (ekf_rate_ / pose_rate_);
+
+      ekf_.updateWithDelay(y, C, R, delay_step);
+
+      // debug
+      Eigen::MatrixXd X_result(dim_x_, 1);
+      ekf_.getLatestX(X_result);
+      DEBUG_PRINT_MAT(X_result.transpose());
+      DEBUG_PRINT_MAT((X_result - X_curr).transpose());
+}
+
+void EKFLocalizer::timerproc()
+{
+      /* predict model in EKF */
+      auto start = std::chrono::system_clock::now();
+      DEBUG_INFO("------------------------- start prediction -------------------------");
+      predictKinematicsModel();
+      double elapsed =
+          std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count();
+      DEBUG_INFO("[EKF] predictKinematicsModel calculation time = %f [ms]", elapsed * 1.0e-6);
+      DEBUG_INFO("------------------------- end prediction -------------------------\n");
+
+      /* pose measurement update */
+//      if (current_pose_ptr_ != nullptr)
+//      {
+        DEBUG_INFO("------------------------- start Pose -------------------------");
+        start = std::chrono::system_clock::now();
+        measurementUpdatePose();
+        elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count();
+        DEBUG_INFO("[EKF] measurementUpdatePose calculation time = %f [ms]", elapsed * 1.0e-6);
+        DEBUG_INFO("------------------------- end Pose -------------------------\n");
+//      }
+
+    ////  /* twist measurement update */
+    ////  if (current_twist_ptr_ != nullptr)
+    ////  {
+    ////    DEBUG_INFO("------------------------- start twist -------------------------");
+    ////    start = std::chrono::system_clock::now();
+    ////    measurementUpdateTwist(*current_twist_ptr_);
+    ////    elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count();
+    ////    DEBUG_INFO("[EKF] measurementUpdateTwist calculation time = %f [ms]", elapsed * 1.0e-6);
+    ////    DEBUG_INFO("------------------------- end twist -------------------------\n");
+    ////  }
+
+    //  /* set current pose, twist */
+    //  setCurrentResult();
+
+    //  /* publish ekf result */
+    //  publishEstimateResult();
+}

src/detection/detection_lidar_localizer_ekf/ekf_localizer.h

@@ -17,6 +17,7 @@
 #include <iostream>
 #include <vector>
 #include <chrono>
+#include <QMutex>
 
 //#include <ros/ros.h>
 //#include <geometry_msgs/PoseArray.h>
@@ -27,14 +28,38 @@
 //#include <geometry_msgs/TransformStamped.h>
 //#include <std_msgs/Float64.h>
 //#include <std_msgs/Float64MultiArray.h>
-#include <tf2/utils.h>
-#include <tf2/LinearMath/Quaternion.h>
-#include <tf2_ros/transform_listener.h>
-#include <tf2_ros/transform_broadcaster.h>
+//#include <tf2/utils.h>
+//#include <tf2/LinearMath/Quaternion.h>
+//#include <tf2_ros/transform_listener.h>
+//#include <tf2_ros/transform_broadcaster.h>
+
+#include "boost/array.hpp"
+
+#include "ndtgpspos.pb.h"
+#include "ndtpos.pb.h"
 
 #include "amathutils_lib/kalman_filter.hpp"
 #include "amathutils_lib/time_delay_kalman_filter.hpp"
 
+#include "modulecomm.h"
+
+namespace iv {
+
+
+struct pose
+{
+  double x;
+  double y;
+  double z;
+  double roll;
+  double pitch;
+  double yaw;
+  double vx;
+  double vy;
+};
+
+}
+
 class EKFLocalizer
 {
 public:
@@ -42,23 +67,23 @@ public:
   ~EKFLocalizer();
 
 private:
-  ros::NodeHandle nh_;                   //!< @brief ros public node handler
-  ros::NodeHandle pnh_;                  //!< @brief  ros private node handler
-  ros::Publisher pub_pose_;              //!< @brief ekf estimated pose publisher
-  ros::Publisher pub_pose_cov_;          //!< @brief estimated ekf pose with covariance publisher
-  ros::Publisher pub_twist_;             //!< @brief ekf estimated twist publisher
-  ros::Publisher pub_twist_cov_;         //!< @brief ekf estimated twist with covariance publisher
-  ros::Publisher pub_debug_;             //!< @brief debug info publisher
-  ros::Publisher pub_measured_pose_;     //!< @brief debug measurement pose publisher
-  ros::Publisher pub_yaw_bias_;          //!< @brief ekf estimated yaw bias publisher
-  ros::Subscriber sub_initialpose_;      //!< @brief initial pose subscriber
-  ros::Subscriber sub_pose_;             //!< @brief measurement pose subscriber
-  ros::Subscriber sub_twist_;            //!< @brief measurement twist subscriber
-  ros::Subscriber sub_pose_with_cov_;    //!< @brief measurement pose with covariance subscriber
-  ros::Subscriber sub_twist_with_cov_;   //!< @brief measurement twist with covariance subscriber
-  ros::Timer timer_control_;             //!< @brief time for ekf calculation callback
-  ros::Timer timer_tf_;                  //!< @brief timer to send transform
-  tf2_ros::TransformBroadcaster tf_br_;  //!< @brief tf broadcaster
+//  ros::NodeHandle nh_;                   //!< @brief ros public node handler
+//  ros::NodeHandle pnh_;                  //!< @brief  ros private node handler
+//  ros::Publisher pub_pose_;              //!< @brief ekf estimated pose publisher
+//  ros::Publisher pub_pose_cov_;          //!< @brief estimated ekf pose with covariance publisher
+//  ros::Publisher pub_twist_;             //!< @brief ekf estimated twist publisher
+//  ros::Publisher pub_twist_cov_;         //!< @brief ekf estimated twist with covariance publisher
+//  ros::Publisher pub_debug_;             //!< @brief debug info publisher
+//  ros::Publisher pub_measured_pose_;     //!< @brief debug measurement pose publisher
+//  ros::Publisher pub_yaw_bias_;          //!< @brief ekf estimated yaw bias publisher
+//  ros::Subscriber sub_initialpose_;      //!< @brief initial pose subscriber
+//  ros::Subscriber sub_pose_;             //!< @brief measurement pose subscriber
+//  ros::Subscriber sub_twist_;            //!< @brief measurement twist subscriber
+//  ros::Subscriber sub_pose_with_cov_;    //!< @brief measurement pose with covariance subscriber
+//  ros::Subscriber sub_twist_with_cov_;   //!< @brief measurement twist with covariance subscriber
+//  ros::Timer timer_control_;             //!< @brief time for ekf calculation callback
+//  ros::Timer timer_tf_;                  //!< @brief timer to send transform
+//  tf2_ros::TransformBroadcaster tf_br_;  //!< @brief tf broadcaster
 
   TimeDelayKalmanFilter ekf_;  //!< @brief  extended kalman filter instance.
 
@@ -122,12 +147,12 @@ private:
   /**
    * @brief computes update & prediction of EKF for each ekf_dt_[s] time
    */
-  void timerCallback(const ros::TimerEvent& e);
+//  void timerCallback(const ros::TimerEvent& e);
 
   /**
    * @brief publish tf for tf_rate [Hz]
    */
-  void timerTFCallback(const ros::TimerEvent& e);
+//  void timerTFCallback(const ros::TimerEvent& e);
 
   /**
    * @brief set pose measurement
@@ -168,13 +193,13 @@ private:
    * @brief compute EKF update with pose measurement
    * @param pose measurement value
    */
-  void measurementUpdatePose(const geometry_msgs::PoseStamped& pose);
+//  void measurementUpdatePose(const geometry_msgs::PoseStamped& pose);
 
   /**
    * @brief compute EKF update with pose measurement
    * @param twist measurement value
    */
-  void measurementUpdateTwist(const geometry_msgs::TwistStamped& twist);
+//  void measurementUpdateTwist(const geometry_msgs::TwistStamped& twist);
 
   /**
    * @brief check whether a measurement value falls within the mahalanobis distance threshold
@@ -190,8 +215,8 @@ private:
   /**
    * @brief get transform from frame_id
    */
-  bool getTransformFromTF(std::string parent_frame, std::string child_frame,
-                          geometry_msgs::TransformStamped& transform);
+//  bool getTransformFromTF(std::string parent_frame, std::string child_frame,
+//                          geometry_msgs::TransformStamped& transform);
 
   /**
    * @brief normalize yaw angle
@@ -203,17 +228,29 @@ private:
   /**
    * @brief set current EKF estimation result to current_ekf_pose_ & current_ekf_twist_
    */
-  void setCurrentResult();
+//  void setCurrentResult();
 
   /**
    * @brief publish current EKF estimation result
    */
-  void publishEstimateResult();
+//  void publishEstimateResult();
 
   /**
    * @brief for debug
    */
-  void showCurrentX();
+//  void showCurrentX();
 
   friend class EKFLocalizerTestSuite;  // for test code
+
+
+private:
+  void UpdateGPS(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+
+  iv::pose mCurPose;
+  bool mbCurPoseUpdate = false;
+  QMutex mMutexCurPose;
+
+  void measurementUpdatePose();
+
+  void timerproc();
 };

src/detection/detection_lidar_localizer_ekf/main.cpp

@@ -1,7 +1,11 @@
 #include <QCoreApplication>
 
+#include "modulecomm.h"
+#include "ivbacktrace.h"
+
 int main(int argc, char *argv[])
 {
+    RegisterIVBackTrace();
     QCoreApplication a(argc, argv);
 
     return a.exec();

src/tool/map_lanetoxodr/dialogroadmove.cpp

@@ -20,6 +20,10 @@ DialogRoadMove::DialogRoadMove(OpenDrive * pxodr,Road * pRoad, QWidget *parent)
     {
         setWindowTitle(QString(pRoad->GetRoadId().data()));
     }
+
+    ui->comboBox_dirmode->addItem("Entire Road");
+    ui->comboBox_dirmode->addItem("Road Start");
+    ui->comboBox_dirmode->setCurrentIndex(0);
 }
 
 DialogRoadMove::~DialogRoadMove()
@@ -55,10 +59,21 @@ void DialogRoadMove::on_pushButton_Move_clicked()
     Road newroad = *mpRoad;
 
     double hdg = 0;
+    if(ui->comboBox_dirmode->currentIndex() == 1)
+    {
     if(mpRoad->GetGeometryBlockCount()>0)
     {
         hdg = mpRoad->GetGeometryBlock(0)->GetGeometryAt(0)->GetHdg();
     }
+    }
+    else
+    {
+        double x1,y1,hdg1;
+        double x2,y2,hdg2;
+        xodrfunc::GetRoadXYByS(mpRoad,0,x1,y1,hdg1);
+        xodrfunc::GetRoadXYByS(mpRoad,std::max(0.0,mpRoad->GetRoadLength()-0.1),x2,y2,hdg2);
+        hdg = xodrfunc::CalcHdg(QPointF(x1,y1),QPointF(x2,y2));
+    }
 
     double yoff = fMoveX*sin(hdg+M_PI/2.0) + fMoveY *sin(hdg);
     double xoff = fMoveX*cos(hdg+M_PI/2.0) + fMoveY *cos(hdg);

src/tool/map_lanetoxodr/dialogroadmove.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>552</width>
-    <height>283</height>
+    <width>591</width>
+    <height>341</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -16,8 +16,8 @@
   <widget class="QPushButton" name="pushButton_Move">
    <property name="geometry">
     <rect>
-     <x>200</x>
-     <y>170</y>
+     <x>180</x>
+     <y>270</y>
      <width>89</width>
      <height>25</height>
     </rect>
@@ -72,6 +72,29 @@
     </rect>
    </property>
   </widget>
+  <widget class="QComboBox" name="comboBox_dirmode">
+   <property name="geometry">
+    <rect>
+     <x>300</x>
+     <y>160</y>
+     <width>161</width>
+     <height>31</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_3">
+   <property name="geometry">
+    <rect>
+     <x>59</x>
+     <y>163</y>
+     <width>161</width>
+     <height>31</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Direction Mode</string>
+   </property>
+  </widget>
  </widget>
  <resources/>
  <connections/>

src/tool/map_lanetoxodr/main.cpp

@@ -5,6 +5,12 @@
 #include "mainwindow.h"
 #endif
 
+#ifndef ANDROID
+
+#include "ivbacktrace.h"
+
+#endif
+
 #include <QApplication>
 
 
@@ -49,6 +55,11 @@ bool requestPermission() {
 int main(int argc, char *argv[])
 {
 
+#ifndef ANDROID
+
+#include "ivbacktrace.h"
+    RegisterIVBackTrace();
+#endif
     QApplication a(argc, argv);
 
 #ifdef ANDROID