change src1/common/modulecomm. change lenth to changeable, complete. change src/detection/detection_lidar_localizer_ekr, but not complete.

src/detection/detection_lidar_localizer_ekf/amathutils_lib/kalman_filter.cpp

@@ -0,0 +1,133 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. 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 "amathutils_lib/kalman_filter.hpp"
+
+KalmanFilter::KalmanFilter() {}
+KalmanFilter::KalmanFilter(const Eigen::MatrixXd &x, const Eigen::MatrixXd &A,
+                           const Eigen::MatrixXd &B, const Eigen::MatrixXd &C,
+                           const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R,
+                           const Eigen::MatrixXd &P)
+{
+  init(x, A, B, C, Q, R, P);
+}
+KalmanFilter::~KalmanFilter() {}
+bool KalmanFilter::init(const Eigen::MatrixXd &x, const Eigen::MatrixXd &A,
+                        const Eigen::MatrixXd &B, const Eigen::MatrixXd &C,
+                        const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R,
+                        const Eigen::MatrixXd &P)
+{
+  if (x.cols() == 0 || x.rows() == 0 || A.cols() == 0 || A.rows() == 0 ||
+      B.cols() == 0 || B.rows() == 0 || C.cols() == 0 || C.rows() == 0 ||
+      Q.cols() == 0 || Q.rows() == 0 || R.cols() == 0 || R.rows() == 0 ||
+      P.cols() == 0 || P.rows() == 0)
+  {
+    return false;
+  }
+  x_ = x;
+  A_ = A;
+  B_ = B;
+  C_ = C;
+  Q_ = Q;
+  R_ = R;
+  P_ = P;
+  return true;
+}
+bool KalmanFilter::init(const Eigen::MatrixXd &x, const Eigen::MatrixXd &P0)
+{
+  if (x.cols() == 0 || x.rows() == 0 || P0.cols() == 0 || P0.rows() == 0)
+  {
+    return false;
+  }
+  x_ = x;
+  P_ = P0;
+  return true;
+}
+
+void KalmanFilter::setA(const Eigen::MatrixXd &A) { A_ = A; }
+void KalmanFilter::setB(const Eigen::MatrixXd &B) { B_ = B; }
+void KalmanFilter::setC(const Eigen::MatrixXd &C) { C_ = C; }
+void KalmanFilter::setQ(const Eigen::MatrixXd &Q) { Q_ = Q; }
+void KalmanFilter::setR(const Eigen::MatrixXd &R) { R_ = R; }
+void KalmanFilter::getX(Eigen::MatrixXd &x) { x = x_; }
+void KalmanFilter::getP(Eigen::MatrixXd &P) { P = P_; }
+double KalmanFilter::getXelement(unsigned int i) { return x_(i); }
+
+bool KalmanFilter::predict(const Eigen::MatrixXd &x_next,
+                           const Eigen::MatrixXd &A,
+                           const Eigen::MatrixXd &Q)
+{
+  if (x_.rows() != x_next.rows() || A.cols() != P_.rows() ||
+      Q.cols() != Q.rows() || A.rows() != Q.cols())
+  {
+    return false;
+  }
+  x_ = x_next;
+  P_ = A * P_ * A.transpose() + Q;
+  return true;
+}
+bool KalmanFilter::predict(const Eigen::MatrixXd &x_next,
+                           const Eigen::MatrixXd &A)
+{
+  return predict(x_next, A, Q_);
+}
+
+bool KalmanFilter::predict(const Eigen::MatrixXd &u, const Eigen::MatrixXd &A,
+                           const Eigen::MatrixXd &B, const Eigen::MatrixXd &Q)
+{
+  if (A.cols() != x_.rows() || B.cols() != u.rows())
+  {
+    return false;
+  }
+  const Eigen::MatrixXd x_next = A * x_ + B * u;
+  return predict(x_next, A, Q);
+}
+bool KalmanFilter::predict(const Eigen::MatrixXd &u) { return predict(u, A_, B_, Q_); }
+
+bool KalmanFilter::update(const Eigen::MatrixXd &y,
+                          const Eigen::MatrixXd &y_pred,
+                          const Eigen::MatrixXd &C,
+                          const Eigen::MatrixXd &R)
+{
+  if (P_.cols() != C.cols() || R.rows() != R.cols() || R.rows() != C.rows() ||
+      y.rows() != y_pred.rows() || y.rows() != C.rows())
+  {
+    return false;
+  }
+  const Eigen::MatrixXd PCT = P_ * C.transpose();
+  const Eigen::MatrixXd K = PCT * ((R + C * PCT).inverse());
+
+  if (isnan(K.array()).any() || isinf(K.array()).any())
+  {
+    return false;
+  }
+
+  x_ = x_ + K * (y - y_pred);
+  P_ = P_ - K * (C * P_);
+  return true;
+}
+
+bool KalmanFilter::update(const Eigen::MatrixXd &y, const Eigen::MatrixXd &C,
+                          const Eigen::MatrixXd &R)
+{
+  if (C.cols() != x_.rows())
+  {
+    return false;
+  }
+  const Eigen::MatrixXd y_pred = C * x_;
+  return update(y, y_pred, C, R);
+}
+bool KalmanFilter::update(const Eigen::MatrixXd &y) { return update(y, C_, R_); }

src/detection/detection_lidar_localizer_ekf/amathutils_lib/kalman_filter.hpp

@@ -0,0 +1,203 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. 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.
+ */
+
+#ifndef AMATHUTILS_LIB_KALMAN_FILTER_HPP
+#define AMATHUTILS_LIB_KALMAN_FILTER_HPP
+
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/LU>
+
+/**
+ * @file kalman_filter.h
+ * @brief kalman filter class
+ * @author Takamasa Horibe
+ * @date 2019.05.01
+ */
+
+class KalmanFilter
+{
+public:
+  /**
+   * @brief No initialization constructor.
+   */
+  KalmanFilter();
+
+  /**
+   * @brief constructor with initialization
+   * @param x initial state
+   * @param A coefficient matrix of x for process model
+   * @param B coefficient matrix of u for process model
+   * @param C coefficient matrix of x for measurement model
+   * @param Q covariace matrix for process model
+   * @param R covariance matrix for measurement model
+   * @param P initial covariance of estimated state
+   */
+  KalmanFilter(const Eigen::MatrixXd &x, const Eigen::MatrixXd &A,
+               const Eigen::MatrixXd &B, const Eigen::MatrixXd &C,
+               const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R,
+               const Eigen::MatrixXd &P);
+
+  /**
+   * @brief destructor
+   */
+  ~KalmanFilter();
+
+  /**
+   * @brief initialization of kalman filter
+   * @param x initial state
+   * @param A coefficient matrix of x for process model
+   * @param B coefficient matrix of u for process model
+   * @param C coefficient matrix of x for measurement model
+   * @param Q covariace matrix for process model
+   * @param R covariance matrix for measurement model
+   * @param P initial covariance of estimated state
+   */
+  bool init(const Eigen::MatrixXd &x, const Eigen::MatrixXd &A,
+            const Eigen::MatrixXd &B, const Eigen::MatrixXd &C,
+            const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R,
+            const Eigen::MatrixXd &P);
+
+  /**
+   * @brief initialization of kalman filter
+   * @param x initial state
+   * @param P initial covariance of estimated state
+   */
+  bool init(const Eigen::MatrixXd &x, const Eigen::MatrixXd &P0);
+
+  /**
+   * @brief set A of process model
+   * @param A coefficient matrix of x for process model
+   */
+  void setA(const Eigen::MatrixXd &A);
+
+  /**
+   * @brief set B of process model
+   * @param B coefficient matrix of u for process model
+   */
+  void setB(const Eigen::MatrixXd &B);
+
+  /**
+   * @brief set C of measurement model
+   * @param C coefficient matrix of x for measurement model
+   */
+  void setC(const Eigen::MatrixXd &C);
+
+  /**
+   * @brief set covariace matrix Q for process model
+   * @param Q covariace matrix for process model
+   */
+  void setQ(const Eigen::MatrixXd &Q);
+
+  /**
+   * @brief set covariance matrix R for measurement model
+   * @param R covariance matrix for measurement model
+   */
+  void setR(const Eigen::MatrixXd &R);
+
+  /**
+   * @brief get current kalman filter state
+   * @param x kalman filter state
+   */
+  void getX(Eigen::MatrixXd &x);
+
+  /**
+   * @brief get current kalman filter covariance
+   * @param P kalman filter covariance
+   */
+  void getP(Eigen::MatrixXd &P);
+
+  /**
+   * @brief get component of current kalman filter state
+   * @param i index of kalman filter state
+   * @return value of i's component of the kalman filter state x[i]
+   */
+  double getXelement(unsigned int i);
+
+  /**
+   * @brief calculate kalman filter state and covariance by prediction model with A, B, Q matrix. This is mainly for EKF with variable matrix.
+   * @param u input for model
+   * @param A coefficient matrix of x for process model
+   * @param B coefficient matrix of u for process model
+   * @param Q covariace matrix for process model
+   * @return bool to check matrix operations are being performed properly
+   */
+  bool predict(const Eigen::MatrixXd &u, const Eigen::MatrixXd &A,
+               const Eigen::MatrixXd &B, const Eigen::MatrixXd &Q);
+
+  /**
+   * @brief calculate kalman filter covariance with prediction model with x, A, Q matrix. This is mainly for EKF with variable matrix.
+   * @param x_next predicted state
+   * @param A coefficient matrix of x for process model
+   * @param Q covariace matrix for process model
+   * @return bool to check matrix operations are being performed properly
+   */
+  bool predict(const Eigen::MatrixXd &x_next, const Eigen::MatrixXd &A,
+               const Eigen::MatrixXd &Q);
+
+  /**
+   * @brief calculate kalman filter covariance with prediction model with x, A, Q matrix. This is mainly for EKF with variable matrix.
+   * @param x_next predicted state
+   * @param A coefficient matrix of x for process model
+   * @return bool to check matrix operations are being performed properly
+   */
+  bool predict(const Eigen::MatrixXd &x_next, const Eigen::MatrixXd &A);
+
+  /**
+   * @brief calculate kalman filter state by prediction model with A, B and Q being class menber variables.
+   * @param u input for the model
+   * @return bool to check matrix operations are being performed properly
+   */
+  bool predict(const Eigen::MatrixXd &u);
+
+  /**
+   * @brief calculate kalman filter state by measurement model with y_pred, C and R matrix. This is mainly for EKF with variable matrix.
+   * @param y measured values
+   * @param y output values expected from measurement model
+   * @param C coefficient matrix of x for measurement model
+   * @param R covariance matrix for measurement model
+   * @return bool to check matrix operations are being performed properly
+   */
+  bool update(const Eigen::MatrixXd &y, const Eigen::MatrixXd &y_pred,
+              const Eigen::MatrixXd &C, const Eigen::MatrixXd &R);
+
+  /**
+   * @brief calculate kalman filter state by measurement model with C and R matrix. This is mainly for EKF with variable matrix.
+   * @param y measured values
+   * @param C coefficient matrix of x for measurement model
+   * @param R covariance matrix for measurement model
+   * @return bool to check matrix operations are being performed properly
+   */
+  bool update(const Eigen::MatrixXd &y, const Eigen::MatrixXd &C,
+              const Eigen::MatrixXd &R);
+
+  /**
+   * @brief calculate kalman filter state by measurement model with C and R being class menber variables.
+   * @param y measured values
+   * @return bool to check matrix operations are being performed properly
+   */
+  bool update(const Eigen::MatrixXd &y);
+
+protected:
+  Eigen::MatrixXd x_;  //!< @brief current estimated state
+  Eigen::MatrixXd A_;  //!< @brief coefficient matrix of x for process model x[k+1] = A*x[k] + B*u[k]
+  Eigen::MatrixXd B_;  //!< @brief coefficient matrix of u for process model x[k+1] = A*x[k] + B*u[k]
+  Eigen::MatrixXd C_;  //!< @brief coefficient matrix of x for measurement model y[k] = C * x[k]
+  Eigen::MatrixXd Q_;  //!< @brief covariace matrix for process model x[k+1] = A*x[k] + B*u[k]
+  Eigen::MatrixXd R_;  //!< @brief covariance matrix for measurement model y[k] = C * x[k]
+  Eigen::MatrixXd P_;  //!< @brief covariance of estimated state
+};
+
+#endif  // AMATHUTILS_LIB_KALMAN_FILTER_HPP

src/detection/detection_lidar_localizer_ekf/amathutils_lib/time_delay_kalman_filter.cpp

@@ -0,0 +1,97 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. 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 "amathutils_lib/time_delay_kalman_filter.hpp"
+
+TimeDelayKalmanFilter::TimeDelayKalmanFilter() {}
+
+void TimeDelayKalmanFilter::init(const Eigen::MatrixXd &x, const Eigen::MatrixXd &P0,
+                                 const int max_delay_step)
+{
+  max_delay_step_ = max_delay_step;
+  dim_x_ = x.rows();
+  dim_x_ex_ = dim_x_ * max_delay_step;
+
+  x_ = Eigen::MatrixXd::Zero(dim_x_ex_, 1);
+  P_ = Eigen::MatrixXd::Zero(dim_x_ex_, dim_x_ex_);
+
+  for (int i = 0; i < max_delay_step_; ++i)
+  {
+    x_.block(i * dim_x_, 0, dim_x_, 1) = x;
+    P_.block(i * dim_x_, i * dim_x_, dim_x_, dim_x_) = P0;
+  }
+}
+
+void TimeDelayKalmanFilter::getLatestX(Eigen::MatrixXd &x) { x = x_.block(0, 0, dim_x_, 1); }
+void TimeDelayKalmanFilter::getLatestP(Eigen::MatrixXd &P) { P = P_.block(0, 0, dim_x_, dim_x_); }
+
+bool TimeDelayKalmanFilter::predictWithDelay(const Eigen::MatrixXd &x_next, const Eigen::MatrixXd &A,
+                                             const Eigen::MatrixXd &Q)
+{
+/*
+ * time delay model:
+ * 
+ *     [A   0   0]      [P11   P12   P13]      [Q   0   0]
+ * A = [I   0   0], P = [P21   P22   P23], Q = [0   0   0]
+ *     [0   I   0]      [P31   P32   P33]      [0   0   0]
+ * 
+ * covariance calculation in prediction : P = A * P * A' + Q
+ *
+ *     [A*P11*A'*+Q  A*P11  A*P12]
+ * P = [     P11*A'    P11    P12]
+ *     [     P21*A'    P21    P22]
+ */
+
+  const int d_dim_x = dim_x_ex_ - dim_x_;
+
+  /* slide states in the time direction */
+  Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex_, 1);
+  x_tmp.block(0, 0, dim_x_, 1) = x_next;
+  x_tmp.block(dim_x_, 0, d_dim_x, 1) = x_.block(0, 0, d_dim_x, 1);
+  x_ = x_tmp;
+
+  /* update P with delayed measurement A matrix structure */
+  Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex_, dim_x_ex_);
+  P_tmp.block(0, 0, dim_x_, dim_x_) = A * P_.block(0, 0, dim_x_, dim_x_) * A.transpose() + Q;
+  P_tmp.block(0, dim_x_, dim_x_, d_dim_x) = A * P_.block(0, 0, dim_x_, d_dim_x);
+  P_tmp.block(dim_x_, 0, d_dim_x, dim_x_) = P_.block(0, 0, d_dim_x, dim_x_) * A.transpose();
+  P_tmp.block(dim_x_, dim_x_, d_dim_x, d_dim_x) = P_.block(0, 0, d_dim_x, d_dim_x);
+  P_ = P_tmp;
+
+  return true;
+}
+
+bool TimeDelayKalmanFilter::updateWithDelay(const Eigen::MatrixXd &y, const Eigen::MatrixXd &C,
+                                            const Eigen::MatrixXd &R, const int delay_step)
+{
+  if (delay_step >= max_delay_step_)
+  {
+    std::cerr << "delay step is larger than max_delay_step. ignore update." << std::endl;
+    return false;
+  }
+
+  const int dim_y = y.rows();
+
+  /* set measurement matrix */
+  Eigen::MatrixXd C_ex = Eigen::MatrixXd::Zero(dim_y, dim_x_ex_);
+  C_ex.block(0, dim_x_ * delay_step, dim_y, dim_x_) = C;
+
+  /* update */
+  if (!update(y, C_ex, R))
+    return false;
+
+  return true;
+}

src/detection/detection_lidar_localizer_ekf/amathutils_lib/time_delay_kalman_filter.hpp

@@ -0,0 +1,85 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. 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.
+ */
+
+#ifndef AMATHUTILS_LIB_TIME_DELAY_KALMAN_FILTER_HPP
+#define AMATHUTILS_LIB_TIME_DELAY_KALMAN_FILTER_HPP
+
+#include <iostream>
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/LU>
+#include "amathutils_lib/kalman_filter.hpp"
+
+/**
+ * @file time_delay_kalman_filter.h
+ * @brief kalman filter with delayed measurement class
+ * @author Takamasa Horibe
+ * @date 2019.05.01
+ */
+
+class TimeDelayKalmanFilter : public KalmanFilter
+{
+public:
+  /**
+   * @brief No initialization constructor.
+   */
+  TimeDelayKalmanFilter();
+
+  /**
+   * @brief initialization of kalman filter
+   * @param x initial state
+   * @param P0 initial covariance of estimated state
+   * @param max_delay_step Maximum number of delay steps, which determines the dimension of the extended kalman filter
+   */
+  void init(const Eigen::MatrixXd &x, const Eigen::MatrixXd &P, const int max_delay_step);
+
+  /**
+   * @brief get latest time estimated state
+   * @param x latest time estimated state
+   */
+  void getLatestX(Eigen::MatrixXd &x);
+
+  /**
+   * @brief get latest time estimation covariance
+   * @param P latest time estimation covariance
+   */
+  void getLatestP(Eigen::MatrixXd &P);
+
+  /**
+   * @brief calculate kalman filter covariance by predicion model with time delay. This is mainly for EKF of nonlinear process model.
+   * @param x_next predicted state by prediction model
+   * @param A coefficient matrix of x for process model
+   * @param Q covariace matrix for process model
+   */
+  bool predictWithDelay(const Eigen::MatrixXd &x_next, const Eigen::MatrixXd &A,
+                        const Eigen::MatrixXd &Q);
+
+  /**
+   * @brief calculate kalman filter covariance by measurement model with time delay. This is mainly for EKF of nonlinear process model.
+   * @param y measured values
+   * @param C coefficient matrix of x for measurement model
+   * @param R covariance matrix for measurement model
+   * @param delay_step measurement delay
+   */
+  bool updateWithDelay(const Eigen::MatrixXd &y, const Eigen::MatrixXd &C,
+                       const Eigen::MatrixXd &R, const int delay_step);
+
+private:
+  int max_delay_step_;  //!< @brief maximum number of delay steps
+  int dim_x_;           //!< @brief dimension of latest state
+  int dim_x_ex_;        //!< @brief dimension of extended state with dime delay
+};
+
+#endif  // AMATHUTILS_LIB_TIME_DELAY_KALMAN_FILTER_HPP

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 += \
+        amathutils_lib/kalman_filter.cpp \
+        amathutils_lib/time_delay_kalman_filter.cpp \
         ekf_localizer.cpp \
         main.cpp
 
@@ -24,6 +26,8 @@ else: unix:!android: target.path = /opt/$${TARGET}/bin
 !isEmpty(target.path): INSTALLS += target
 
 HEADERS += \
+    amathutils_lib/kalman_filter.hpp \
+    amathutils_lib/time_delay_kalman_filter.hpp \
     ekf_localizer.h
 
 INCLUDEPATH += $$PWD/../../../thirdpartylib/amathutils_lib/include

src/detection/detection_lidar_localizer_ekf/ekf_localizer.h

@@ -132,27 +132,27 @@ private:
   /**
    * @brief set pose measurement
    */
-  void callbackPose(const geometry_msgs::PoseStamped::ConstPtr& msg);
+//  void callbackPose(const geometry_msgs::PoseStamped::ConstPtr& msg);
 
   /**
    * @brief set twist measurement
    */
-  void callbackTwist(const geometry_msgs::TwistStamped::ConstPtr& msg);
+//  void callbackTwist(const geometry_msgs::TwistStamped::ConstPtr& msg);
 
   /**
    * @brief set poseWithCovariance measurement
    */
-  void callbackPoseWithCovariance(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg);
+//  void callbackPoseWithCovariance(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg);
 
   /**
    * @brief set twistWithCovariance measurement
    */
-  void callbackTwistWithCovariance(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr& msg);
+//  void callbackTwistWithCovariance(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr& msg);
 
   /**
    * @brief set initial_pose to current EKF pose
    */
-  void callbackInitialPose(const geometry_msgs::PoseWithCovarianceStamped& msg);
+//  void callbackInitialPose(const geometry_msgs::PoseWithCovarianceStamped& msg);
 
   /**
    * @brief initialization of EKF

src1/common/modulecomm/inter/intercomm.cpp

@@ -288,14 +288,23 @@ int intercomm::writemsg(const char *str, const unsigned int nSize)
     interunit * pinter = (interunit *)mpa;
 
     pinter->mMutexUnit.lock();
-    char * pdata = (char *)pinter->strdatabuf;
-    mpinfo = (procinter_info *)pdata;
-    mphead = (procinter_head *)(pdata+sizeof(procinter_info));
+
     if(nSize > pinter->nbufsize)
     {
-        qDebug("procsm::writemsg message size is very big");
-        return -1;
+ //       qDebug("procsm::writemsg message size is very big");
+
+        int nnewsize = std::max(nSize*11/10,nSize+100);
+        char * strdatabuf = new char[sizeof(procinter_info)+ pinter->nPacCount*sizeof(procinter_head) + nnewsize];
+
+        memcpy(strdatabuf,pinter->strdatabuf,sizeof(procinter_info)+ pinter->nPacCount*sizeof(procinter_head) + pinter->nbufsize);
+        pinter->nbufsize = nnewsize;
+        delete pinter->strdatabuf;
+        pinter->strdatabuf = strdatabuf;
+        qDebug("inter resize buf,new buffer size is %d",nnewsize);
     }
+    char * pdata = (char *)pinter->strdatabuf;
+    mpinfo = (procinter_info *)pdata;
+    mphead = (procinter_head *)(pdata+sizeof(procinter_info));
 
 WRITEMSG:
 
@@ -414,6 +423,7 @@ int intercomm::readmsg(unsigned int index, char *str, unsigned int nMaxSize, uns
     interunit * pinter = (interunit *)mpa;
 
     if(pinter->nbufsize == 0)return 0;
+    pinter->mMutexUnit.lock();
     char * pdata = (char *)pinter->strdatabuf;
     mpinfo = (procinter_info *)pdata;
     mphead = (procinter_head *)(pdata+sizeof(procinter_info));
@@ -463,6 +473,7 @@ int intercomm::readmsg(unsigned int index, char *str, unsigned int nMaxSize, uns
             }
         }
     }
+    pinter->mMutexUnit.unlock();
     return nRtn;
 }
 

src1/common/modulecomm/shm/procsm.cpp

@@ -2,7 +2,7 @@
 #include <thread>
 #include <QTime>
 #include <QThread>
-
+#include <algorithm>
 #include "procsm.h"
 
 
@@ -70,7 +70,7 @@ procsm::procsm(const char * strsmname,const unsigned int nBufSize,const unsigned
 
         mmodulemsg_type.mnBufSize = nBufSize;
         mmodulemsg_type.mnMsgBufCount = nMaxPacCount;
-        strncpy(mmodulemsg_type.mstrmsgname,strsmname,255);
+        strncpy(mmodulemsg_type.mstrmsgname,strasmname,255);
 #ifdef USEDBUS
         mmsg = QDBusMessage::createSignal("/catarc/adc",  "adc.adciv.modulecomm", strsmname);
         mmsg<<1;
@@ -151,7 +151,12 @@ procsm::procsm(const char * strsmname,const unsigned int nBufSize,const unsigned
 void procsm::recreateasm(int nbufsize)
 {
 
-    mnBufSize = nbufsize+100;
+    mpASMPtr->lock();
+    qDebug("recreate asms");
+
+
+    mnBufSize = std::max(nbufsize*11/10,nbufsize+1000);
+ //   mnBufSize = nbufsize+100;
     char strasmname[256];
     ASM_PTR * pasm = (ASM_PTR *)mpASMPtr->data();
     snprintf(strasmname,256,"%s_%lld",mstrsmname,QDateTime::currentMSecsSinceEpoch());
@@ -164,8 +169,14 @@ void procsm::recreateasm(int nbufsize)
     mmodulemsg_type.mnMsgBufCount = mnMaxPacCount;
     strncpy(mmodulemsg_type.mstrmsgname,mASM_State.mstrshmname,255);
 
+    mpASM->lock();
+    int noldmemsize = mpASM->size();
+    char * px = new char[mpASM->size()];
+    memcpy(px,mpASM->data(),noldmemsize);
+    mpASM->unlock();
+    mpASM->detach();
 
-
+    qDebug("new asm name is %s,buffer size is %d ",mASM_State.mstrshmname,mnBufSize);
     mpASM = new QSharedMemory(mASM_State.mstrshmname);
 
     bool bAttach = false;
@@ -194,14 +205,15 @@ void procsm::recreateasm(int nbufsize)
     {
 
         mpASM->create(sizeof(procsm_info)+mnMaxPacCount*sizeof(procsm_head) + mnBufSize);
+        memcpy(mpASM->data(),px,noldmemsize);
         char * p = (char *)mpASM->data();
         mpinfo = (procsm_info *)p;
         mphead = (procsm_head *)(p+sizeof(procsm_info));
         mpinfo->mCap = mnMaxPacCount;
         mpinfo->mnBufSize = mnBufSize;
-        mpinfo->mFirst = 0;
-        mpinfo->mNext = 0;
-        mpinfo->mLock = 0;
+//        mpinfo->mFirst = nfirst;
+//        mpinfo->mNext = nnext;
+//        mpinfo->mLock = 0;
     }
 
 
@@ -217,7 +229,7 @@ void procsm::recreateasm(int nbufsize)
         mnMaxPacCount = mpinfo->mCap;
         mnBufSize = mpinfo->mnBufSize;
         //        qDebug("attach successful");
-        mstrtem = new char[mnBufSize];
+ //       mstrtem = new char[mnBufSize];
     }
     else
     {
@@ -225,6 +237,10 @@ void procsm::recreateasm(int nbufsize)
         qDebug("Share Memory Error.");
     }
 
+    mpASMPtr->unlock();
+
+    delete px;
+
 
 
 
@@ -247,12 +263,16 @@ void procsm::recreateasm(int nbufsize)
 bool procsm::AttachMem()
 {
 
-    mpASMPtr->attach();
+    if(!mpASMPtr->isAttached())mpASMPtr->attach();
     if(mpASMPtr->isAttached())
     {
         ASM_PTR * pasmptr = (ASM_PTR *)(mpASMPtr->data());
         mASM_State = * pasmptr;
 
+        if(mpASM != 0)
+        {
+            if(mpASM->isAttached())mpASM->detach();
+        }
         mpASM = new QSharedMemory(mASM_State.mstrshmname);
 
         mpASM->attach();
@@ -351,14 +371,18 @@ int procsm::MoveMem(const unsigned int nSize)
 void procsm::checkasm()
 {
 
+    mpASMPtr->lock();
     ASM_PTR * pASM_PTR = (ASM_PTR * )mpASMPtr->data();
     if(pASM_PTR->mnUpdateTime == mASM_State.mnUpdateTime)
     {
 
+        mpASMPtr->unlock();
         return;
     }
+    qDebug("reattch mem.");
     mbAttach = false;
     AttachMem();
+    mpASMPtr->unlock();
 }
 
 int procsm::writemsg(const char *str, const unsigned int nSize)
@@ -472,7 +496,7 @@ WRITEMSG:
 unsigned int procsm::getcurrentnext()
 {
 
-
+    checkasm();
     unsigned int nNext;
     mpASM->lock();
     nNext = mpinfo->mNext;
@@ -487,12 +511,12 @@ unsigned int procsm::getcurrentnext()
 //if return > 0 readdata
 int procsm::readmsg(unsigned int index, char *str, unsigned int nMaxSize,unsigned int * nRead,QDateTime * pdt)
 {
+    checkasm();
     if(mbAttach == false)
     {
         std::cout<<"ShareMemory Attach fail."<<std::endl;
         return -1;
-    }
-    checkasm();
+    }    
     int nRtn = 0;
     mpASM->lock();
 

src1/common/modulecomm/shm/procsm.h

@@ -87,7 +87,7 @@ public:
 
 private:
     int MoveMem(const unsigned int nSize);
-    QSharedMemory * mpASM;
+    QSharedMemory * mpASM = 0;
     QSharedMemory * mpASMPtr;
     unsigned int mnBufSize;
     unsigned int mnMaxPacCount;

src1/decision/interface/ivdecision.cpp

@@ -67,6 +67,14 @@ void ivdecision::modulerun()
         //ShareMsg
     }
 
+    iv::modulecomm::Unregister(mpaVechicleState);
+    iv::modulecomm::Unregister(mpaDecition);
+    iv::modulecomm::Unregister(mpachassismsg);
+    iv::modulecomm::Unregister(mpavboxmsg);
+    iv::modulecomm::Unregister(mpaultramsg);
+    iv::modulecomm::Unregister(mpav2xmsg);
+    iv::modulecomm::Unregister(mpap900msg);
+
     iv::modulecomm::Unregister(mpapadmsg);
     iv::modulecomm::Unregister(mpahmimsg);
     iv::modulecomm::Unregister(mpamobileyemsg);