|
|
@@ -0,0 +1,1818 @@
|
|
|
+/*
|
|
|
+ * Copyright 2015-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.
|
|
|
+ */
|
|
|
+
|
|
|
+/*
|
|
|
+ Localization program using Normal Distributions Transform
|
|
|
+
|
|
|
+ Yuki KITSUKAWA
|
|
|
+ */
|
|
|
+
|
|
|
+#include <pthread.h>
|
|
|
+#include <chrono>
|
|
|
+#include <fstream>
|
|
|
+#include <iostream>
|
|
|
+#include <memory>
|
|
|
+#include <sstream>
|
|
|
+#include <string>
|
|
|
+
|
|
|
+#include <thread>
|
|
|
+
|
|
|
+#include <boost/filesystem.hpp>
|
|
|
+
|
|
|
+
|
|
|
+#include <tf/tf.h>
|
|
|
+#include <tf/transform_broadcaster.h>
|
|
|
+#include <tf/transform_datatypes.h>
|
|
|
+#include <tf/transform_listener.h>
|
|
|
+
|
|
|
+#include <pcl/io/io.h>
|
|
|
+#include <pcl/io/pcd_io.h>
|
|
|
+#include <pcl/point_types.h>
|
|
|
+#include <pcl_conversions/pcl_conversions.h>
|
|
|
+
|
|
|
+//#include <ndt_cpu/NormalDistributionsTransform.h>
|
|
|
+#include <pcl/registration/ndt.h>
|
|
|
+
|
|
|
+
|
|
|
+#include <pcl/io/pcd_io.h>
|
|
|
+#include <pcl/point_types.h>
|
|
|
+#include <pcl/point_cloud.h>
|
|
|
+#include <pcl/registration/ndt.h>
|
|
|
+#include <pcl/registration/gicp.h>
|
|
|
+#include <pcl/filters/voxel_grid.h>
|
|
|
+//#include <pcl/visualization/pcl_visualizer.h>
|
|
|
+
|
|
|
+#include <QFile>
|
|
|
+
|
|
|
+//#include <ndt_gpu/NormalDistributionsTransform.h>
|
|
|
+#include "ndtpos.pb.h"
|
|
|
+#include "ndtgpspos.pb.h"
|
|
|
+
|
|
|
+#include <pcl_ros/point_cloud.h>
|
|
|
+#include <pcl_ros/transforms.h>
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+#include "ndt_matching.h"
|
|
|
+
|
|
|
+#include "modulecomm.h"
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+#include "gnss_coordinate_convert.h"
|
|
|
+
|
|
|
+//the following are UBUNTU/LINUX ONLY terminal color codes.
|
|
|
+#define RESET "\033[0m"
|
|
|
+#define BLACK "\033[30m" /* Black */
|
|
|
+#define RED "\033[31m" /* Red */
|
|
|
+#define GREEN "\033[32m" /* Green */
|
|
|
+#define YELLOW "\033[33m" /* Yellow */
|
|
|
+#define BLUE "\033[34m" /* Blue */
|
|
|
+#define MAGENTA "\033[35m" /* Magenta */
|
|
|
+#define CYAN "\033[36m" /* Cyan */
|
|
|
+#define WHITE "\033[37m" /* White */
|
|
|
+#define BOLDBLACK "\033[1m\033[30m" /* Bold Black */
|
|
|
+#define BOLDRED "\033[1m\033[31m" /* Bold Red */
|
|
|
+#define BOLDGREEN "\033[1m\033[32m" /* Bold Green */
|
|
|
+#define BOLDYELLOW "\033[1m\033[33m" /* Bold Yellow */
|
|
|
+#define BOLDBLUE "\033[1m\033[34m" /* Bold Blue */
|
|
|
+#define BOLDMAGENTA "\033[1m\033[35m" /* Bold Magenta */
|
|
|
+#define BOLDCYAN "\033[1m\033[36m" /* Bold Cyan */
|
|
|
+#define BOLDWHITE "\033[1m\033[37m" /* Bold White */
|
|
|
+
|
|
|
+
|
|
|
+#define PREDICT_POSE_THRESHOLD 0.5
|
|
|
+
|
|
|
+#define Wa 0.4
|
|
|
+#define Wb 0.3
|
|
|
+#define Wc 0.3
|
|
|
+
|
|
|
+#include "ivfault.h"
|
|
|
+#include "ivlog.h"
|
|
|
+
|
|
|
+extern iv::Ivfault *gfault ;
|
|
|
+extern iv::Ivlog *givlog;
|
|
|
+
|
|
|
+static int gindex = 0;
|
|
|
+iv::lidar_pose glidar_pose;
|
|
|
+
|
|
|
+void * gpset;
|
|
|
+void * gppose;
|
|
|
+
|
|
|
+static bool g_hasmap = false;
|
|
|
+
|
|
|
+enum class MethodType
|
|
|
+{
|
|
|
+ PCL_GENERIC = 0,
|
|
|
+ PCL_ANH = 1,
|
|
|
+ PCL_ANH_GPU = 2,
|
|
|
+ PCL_OPENMP = 3,
|
|
|
+};
|
|
|
+static MethodType _method_type = MethodType::PCL_GENERIC;
|
|
|
+
|
|
|
+static pose initial_pose, predict_pose, predict_pose_imu, predict_pose_odom, predict_pose_imu_odom, previous_pose,
|
|
|
+ ndt_pose, current_pose, current_pose_imu, current_pose_odom, current_pose_imu_odom, localizer_pose;
|
|
|
+
|
|
|
+static double offset_x, offset_y, offset_z, offset_yaw; // current_pos - previous_pose
|
|
|
+static double offset_imu_x, offset_imu_y, offset_imu_z, offset_imu_roll, offset_imu_pitch, offset_imu_yaw;
|
|
|
+static double offset_odom_x, offset_odom_y, offset_odom_z, offset_odom_roll, offset_odom_pitch, offset_odom_yaw;
|
|
|
+static double offset_imu_odom_x, offset_imu_odom_y, offset_imu_odom_z, offset_imu_odom_roll, offset_imu_odom_pitch,
|
|
|
+ offset_imu_odom_yaw;
|
|
|
+
|
|
|
+// Can't load if typed "pcl::PointCloud<pcl::PointXYZRGB> map, add;"
|
|
|
+static pcl::PointCloud<pcl::PointXYZ> map, add;
|
|
|
+
|
|
|
+// If the map is loaded, map_loaded will be 1.
|
|
|
+static int map_loaded = 0;
|
|
|
+static int _use_gnss = 1;
|
|
|
+static int init_pos_set = 0;
|
|
|
+
|
|
|
+static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndtraw(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
|
|
|
+static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr glocalndtraw(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
|
|
|
+
|
|
|
+// Default values
|
|
|
+static int max_iter = 30; // Maximum iterations
|
|
|
+static float ndt_res = 1.0; // Resolution
|
|
|
+static double step_size = 0.1; // Step size
|
|
|
+static double trans_eps = 0.01; // Transformation epsilon
|
|
|
+
|
|
|
+
|
|
|
+static double exe_time = 0.0;
|
|
|
+static bool has_converged;
|
|
|
+static int iteration = 0;
|
|
|
+static double fitness_score = 0.0;
|
|
|
+static double trans_probability = 0.0;
|
|
|
+
|
|
|
+static double diff = 0.0;
|
|
|
+static double diff_x = 0.0, diff_y = 0.0, diff_z = 0.0, diff_yaw;
|
|
|
+
|
|
|
+static double current_velocity = 0.0, previous_velocity = 0.0, previous_previous_velocity = 0.0; // [m/s]
|
|
|
+static double current_velocity_x = 0.0, previous_velocity_x = 0.0;
|
|
|
+static double current_velocity_y = 0.0, previous_velocity_y = 0.0;
|
|
|
+static double current_velocity_z = 0.0, previous_velocity_z = 0.0;
|
|
|
+// static double current_velocity_yaw = 0.0, previous_velocity_yaw = 0.0;
|
|
|
+static double current_velocity_smooth = 0.0;
|
|
|
+
|
|
|
+static double current_velocity_imu_x = 0.0;
|
|
|
+static double current_velocity_imu_y = 0.0;
|
|
|
+static double current_velocity_imu_z = 0.0;
|
|
|
+
|
|
|
+static double current_accel = 0.0, previous_accel = 0.0; // [m/s^2]
|
|
|
+static double current_accel_x = 0.0;
|
|
|
+static double current_accel_y = 0.0;
|
|
|
+static double current_accel_z = 0.0;
|
|
|
+// static double current_accel_yaw = 0.0;
|
|
|
+
|
|
|
+static double angular_velocity = 0.0;
|
|
|
+
|
|
|
+static int use_predict_pose = 0;
|
|
|
+
|
|
|
+
|
|
|
+static std::chrono::time_point<std::chrono::system_clock> matching_start, matching_end;
|
|
|
+
|
|
|
+
|
|
|
+static int _queue_size = 1000;
|
|
|
+
|
|
|
+
|
|
|
+static double predict_pose_error = 0.0;
|
|
|
+
|
|
|
+static double _tf_x, _tf_y, _tf_z, _tf_roll, _tf_pitch, _tf_yaw;
|
|
|
+static Eigen::Matrix4f tf_btol;
|
|
|
+
|
|
|
+static std::string _localizer = "velodyne";
|
|
|
+static std::string _offset = "linear"; // linear, zero, quadratic
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+static bool _get_height = false;
|
|
|
+static bool _use_local_transform = false;
|
|
|
+static bool _use_imu = false;
|
|
|
+static bool _use_odom = false;
|
|
|
+static bool _imu_upside_down = false;
|
|
|
+static bool _output_log_data = false;
|
|
|
+
|
|
|
+static std::string _imu_topic = "/imu_raw";
|
|
|
+
|
|
|
+static std::ofstream ofs;
|
|
|
+static std::string filename;
|
|
|
+
|
|
|
+//static sensor_msgs::Imu imu;
|
|
|
+//static nav_msgs::Odometry odom;
|
|
|
+
|
|
|
+// static tf::TransformListener local_transform_listener;
|
|
|
+static tf::StampedTransform local_transform;
|
|
|
+
|
|
|
+static unsigned int points_map_num = 0;
|
|
|
+
|
|
|
+pthread_mutex_t mutex;
|
|
|
+
|
|
|
+pthread_mutex_t mutex_read;
|
|
|
+
|
|
|
+pthread_mutex_t mutex_pose;
|
|
|
+
|
|
|
+bool gbNeedGPSUpdateNDT = false;
|
|
|
+
|
|
|
+pcl::PointCloud<pcl::PointXYZ>::Ptr local_map_ptr;
|
|
|
+pcl::PointCloud<pcl::PointXYZ>::Ptr global_map_ptr;
|
|
|
+
|
|
|
+pose glastmappose;
|
|
|
+
|
|
|
+static double imu_angular_velocity_x=0;
|
|
|
+static double imu_angular_velocity_y=0;
|
|
|
+static double imu_angular_velocity_z=0;
|
|
|
+static double imu_linear_acceleration_x=0;
|
|
|
+static double imu_linear_acceleration_y=0;
|
|
|
+static double imu_linear_acceleration_z =0;
|
|
|
+
|
|
|
+extern QFile * gpFileLastPos;//Save Last Positin
|
|
|
+static bool gbFileNDTUpdate;
|
|
|
+
|
|
|
+extern double garm_x ;
|
|
|
+extern double garm_y ;
|
|
|
+
|
|
|
+#include <QDateTime>
|
|
|
+
|
|
|
+//cv::Mat gmatimage;
|
|
|
+void ListenPoseSet(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
|
|
|
+{
|
|
|
+ iv::lidar::ndtpos pos;
|
|
|
+
|
|
|
+ if(false == pos.ParseFromArray(strdata,nSize))
|
|
|
+ {
|
|
|
+ std::cout<<" ndtpos parse error."<<std::endl;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ SetCurPose(pos.pose_x(),pos.pose_y(),pos.pose_yaw(),pos.pose_z(),pos.pose_pitch(),pos.pose_roll());
|
|
|
+
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+static void SetLocalMap()
|
|
|
+{
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr local_ptr(new pcl::PointCloud<pcl::PointXYZ>());
|
|
|
+
|
|
|
+ int nSize = global_map_ptr->size();
|
|
|
+
|
|
|
+ int i;
|
|
|
+ for(i=0;i<nSize;i++)
|
|
|
+ {
|
|
|
+ pcl::PointXYZ xp = global_map_ptr->at(i);
|
|
|
+ if(sqrt(pow(xp.x - current_pose.x,2)+pow(xp.y-current_pose.y,2)+pow(xp.z-current_pose.z,2))<30)
|
|
|
+ {
|
|
|
+ local_ptr->push_back(xp);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ glastmappose = current_pose;
|
|
|
+
|
|
|
+ local_map_ptr = local_ptr;
|
|
|
+ std::cout<<"current map size is "<<local_map_ptr->size()<<std::endl;
|
|
|
+}
|
|
|
+
|
|
|
+static bool gbNDTRun = true;
|
|
|
+
|
|
|
+static bool gbGFRun = true;
|
|
|
+static bool gbLMRun= true;
|
|
|
+static std::thread * gpmapthread, * gpgpsfixthread,*gpsavelastthread;
|
|
|
+static bool gbNeedUpdate = false;
|
|
|
+
|
|
|
+extern iv::gpspos glastndtgpspos;
|
|
|
+extern iv::gpspos gcurndtgpspos;
|
|
|
+extern iv::gpspos gcurgpsgpspos;
|
|
|
+extern bool gbGPSFix;
|
|
|
+extern int64_t gnLastGPSTime;
|
|
|
+
|
|
|
+static bool gbgpsupdatendt = false;
|
|
|
+
|
|
|
+static int gusemapindex = -1;
|
|
|
+static int gcurmapindex = -1;
|
|
|
+
|
|
|
+extern std::vector<iv::ndtmaptrace> gvector_trace;
|
|
|
+
|
|
|
+extern void * gpa,* gpb ,* gpc, * gpd;
|
|
|
+
|
|
|
+iv::gpspos PoseToGPS(iv::gpspos xori,pose xpose)
|
|
|
+{
|
|
|
+ double x_o,y_o;
|
|
|
+ GaussProjCal(xori.lon,xori.lat,&x_o,&y_o);
|
|
|
+ double lon,lat;
|
|
|
+ double hdg_o = (90 - xori.heading)*M_PI/180.0;
|
|
|
+ double rel_x = xpose.x * cos(hdg_o) - xpose.y * sin(hdg_o);
|
|
|
+ double rel_y = xpose.x * sin(hdg_o) + xpose.y * cos(hdg_o);
|
|
|
+ GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
|
|
|
+ double hdg_c = hdg_o + xpose.yaw;
|
|
|
+
|
|
|
+ hdg_c = M_PI/2.0 - hdg_c;
|
|
|
+ double heading = hdg_c * 180.0/M_PI;
|
|
|
+ while(heading < 0)heading = heading + 360;
|
|
|
+ while(heading >360)heading = heading - 360;
|
|
|
+ iv::gpspos xgpspos;
|
|
|
+ xgpspos.lon = lon;
|
|
|
+ xgpspos.lat = lat;
|
|
|
+ xgpspos.height = xpose.z + xori.height;
|
|
|
+ xgpspos.heading = heading;
|
|
|
+ xgpspos.pitch = xpose.pitch + xori.pitch;
|
|
|
+ xgpspos.roll = xpose.roll + xori.roll;
|
|
|
+ xgpspos.ve = xpose.vx * cos(hdg_o) - xpose.vy * sin(hdg_o);
|
|
|
+ xgpspos.vn = xpose.vx * sin(hdg_o) + xpose.vy * cos(hdg_o);
|
|
|
+
|
|
|
+ if((garm_x != 0)||(garm_y != 0)) //arm use to Convert pos to GPS Posision
|
|
|
+ {
|
|
|
+ GaussProjCal(xgpspos.lon,xgpspos.lat,&x_o,&y_o);
|
|
|
+ hdg_o = (90 - xgpspos.heading)*M_PI/180.0;
|
|
|
+ rel_x = garm_x *(-1) * cos(hdg_o) - garm_y *(-1) * sin(hdg_o);
|
|
|
+ rel_y = garm_x *(-1) * sin(hdg_o) + garm_y *(-1) * cos(hdg_o);
|
|
|
+ GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
|
|
|
+ xgpspos.lon = lon;
|
|
|
+ xgpspos.lat = lat;
|
|
|
+ }
|
|
|
+ return xgpspos;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+pose CalcPose(iv::gpspos xori, iv::gpspos xcur)
|
|
|
+{
|
|
|
+ double lon,lat;
|
|
|
+ if((garm_x != 0)||(garm_y != 0)) //arm use to Convert pos to GPS Posision
|
|
|
+ {
|
|
|
+ double x_o,y_o;
|
|
|
+ GaussProjCal(xcur.lon,xcur.lat,&x_o,&y_o);
|
|
|
+ double hdg_o = (90 - xcur.heading)*M_PI/180.0;
|
|
|
+ double rel_x = garm_x *(1) * cos(hdg_o) - garm_y *(1) * sin(hdg_o);
|
|
|
+ double rel_y = garm_x *(1) * sin(hdg_o) + garm_y *(1) * cos(hdg_o);
|
|
|
+ GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
|
|
|
+ xcur.lon = lon;
|
|
|
+ xcur.lat = lat;
|
|
|
+ }
|
|
|
+ double x_o,y_o,hdg_o;
|
|
|
+ double x_c,y_c,hdg_c;
|
|
|
+ GaussProjCal(xori.lon,xori.lat,&x_o,&y_o);
|
|
|
+ GaussProjCal(xcur.lon,xcur.lat,&x_c,&y_c);
|
|
|
+ double dis = sqrt(pow(x_c-x_o,2)+ pow(y_c-y_o,2));
|
|
|
+ double rel_x0,rel_y0;
|
|
|
+ rel_x0 = x_c -x_o;
|
|
|
+ rel_y0 = y_c -y_o;
|
|
|
+ double rel_x,rel_y;
|
|
|
+
|
|
|
+ hdg_o = (90 - xori.heading)*M_PI/180.0;
|
|
|
+ hdg_c = (90 - xcur.heading)*M_PI/180.0;
|
|
|
+ rel_x = rel_x0 * cos(-hdg_o) - rel_y0 * sin(-hdg_o);
|
|
|
+ rel_y = rel_x0 * sin(-hdg_o) + rel_y0 * cos(-hdg_o);
|
|
|
+
|
|
|
+
|
|
|
+ double rel_hdg;
|
|
|
+ rel_hdg = hdg_c - hdg_o;
|
|
|
+ pose posex;
|
|
|
+ posex.x = rel_x;
|
|
|
+ posex.y = rel_y;
|
|
|
+ posex.z = xcur.height - xori.height;
|
|
|
+ posex.pitch = xcur.pitch - xori.pitch;
|
|
|
+ posex.roll = xcur.roll - xori.roll;
|
|
|
+ posex.yaw = rel_hdg;
|
|
|
+
|
|
|
+ posex.vx = xcur.ve * cos(-hdg_o) - xcur.vn * sin(-hdg_o);
|
|
|
+ posex.vy = xcur.ve * sin(-hdg_o) + xcur.vn * cos(-hdg_o);
|
|
|
+
|
|
|
+ return posex;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+static double gettracedis(int index,pose posex)
|
|
|
+{
|
|
|
+ double fdismin = 1000000.;
|
|
|
+ double zdiff = 0;
|
|
|
+ int neari;
|
|
|
+ if(index < 0)return 1000000.0;
|
|
|
+ if(index>= gvector_trace.size())
|
|
|
+ {
|
|
|
+ return 1000000.0;
|
|
|
+ }
|
|
|
+ int i;
|
|
|
+ std::vector<iv::ndttracepoint> vt = gvector_trace.at(index).mvector_trace;
|
|
|
+ int nsize = vt.size();
|
|
|
+// std::cout<<"size is "<<nsize<<std::endl;
|
|
|
+ for(i=0;i<nsize;i++)
|
|
|
+ {
|
|
|
+ double fdis = sqrt(pow(posex.x - vt.at(i).x,2) + pow(posex.y - vt.at(i).y,2));
|
|
|
+// std::cout<<"fdis is "<<fdis<<std::endl;
|
|
|
+ if((fdis < fdismin) &&(fabs(posex.z - vt.at(i).z)<5))
|
|
|
+ {
|
|
|
+ fdismin = fdis;
|
|
|
+ neari = i;
|
|
|
+ zdiff = fabs(posex.z - vt.at(i).z);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return fdismin;
|
|
|
+}
|
|
|
+
|
|
|
+static void getmindistrace(int & index, double & ftracedis)
|
|
|
+{
|
|
|
+ double fdismin = 1000000.0;
|
|
|
+ int xindexmin = -1;
|
|
|
+ int i;
|
|
|
+ int nsize = gvector_trace.size();
|
|
|
+
|
|
|
+
|
|
|
+ for(i=0;i<nsize;i++)
|
|
|
+ {
|
|
|
+ pose posex = CalcPose(gvector_trace[i].mndtorigin,gcurndtgpspos);
|
|
|
+ double fdis = gettracedis(i,posex);
|
|
|
+ if(fdis<fdismin)
|
|
|
+ {
|
|
|
+ fdismin = fdis;
|
|
|
+ xindexmin = i;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(xindexmin != -1)
|
|
|
+ {
|
|
|
+ ftracedis = fdismin;
|
|
|
+ index = xindexmin;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ index = -1;
|
|
|
+ ftracedis = 100000.0;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+#include <QTime>
|
|
|
+
|
|
|
+extern void pausecomm();
|
|
|
+extern void continuecomm();
|
|
|
+static void UseMap(int index)
|
|
|
+{
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr xmap;
|
|
|
+
|
|
|
+ xmap = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
+
|
|
|
+
|
|
|
+ QTime xTime;
|
|
|
+ xTime.start();
|
|
|
+ if(0 == pcl::io::loadPCDFile(gvector_trace.at(index).mstrpcdpath.data(),*xmap))
|
|
|
+ {
|
|
|
+
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ std::cout<<"map size is 0"<<std::endl;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ qDebug("load map time is %d",xTime.elapsed());
|
|
|
+
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(*xmap));
|
|
|
+// gvectoranh.push_back(new_anh_gpu_ndt_ptr);
|
|
|
+
|
|
|
+ qDebug("ndt load finish time is %d",xTime.elapsed());
|
|
|
+
|
|
|
+ gcurmapindex = index;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr localndtraw(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
|
|
|
+
|
|
|
+
|
|
|
+ localndtraw->setResolution(ndt_res);
|
|
|
+ localndtraw->setInputTarget(map_ptr);
|
|
|
+ localndtraw->setMaximumIterations(max_iter);
|
|
|
+ localndtraw->setStepSize(step_size);
|
|
|
+ localndtraw->setTransformationEpsilon(trans_eps);
|
|
|
+
|
|
|
+ pthread_mutex_lock(&mutex);
|
|
|
+// ndt = glocal_ndt;
|
|
|
+ glocalndtraw = localndtraw;
|
|
|
+ pthread_mutex_unlock(&mutex);
|
|
|
+
|
|
|
+ gbNeedUpdate = true;
|
|
|
+
|
|
|
+
|
|
|
+ std::cout<<"change map, index is "<<index<<std::endl;
|
|
|
+}
|
|
|
+
|
|
|
+void LocalMapUpdate(int n)
|
|
|
+{
|
|
|
+ std::cout<<"LocalMap n is "<<n<<std::endl;
|
|
|
+
|
|
|
+ int index;
|
|
|
+ double ftracedis;
|
|
|
+ int ncurindex = -1;
|
|
|
+
|
|
|
+ int i;
|
|
|
+ for(i=0;i<gvector_trace.size();i++)
|
|
|
+ {
|
|
|
+// UseMap(i);
|
|
|
+// std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
|
|
+ }
|
|
|
+ while(gbLMRun)
|
|
|
+ {
|
|
|
+
|
|
|
+ getmindistrace(index,ftracedis);
|
|
|
+
|
|
|
+ if(g_hasmap == false)
|
|
|
+ {
|
|
|
+
|
|
|
+ if(index >= 0)
|
|
|
+ {
|
|
|
+ if(ftracedis < 30)
|
|
|
+ {
|
|
|
+ UseMap(index);
|
|
|
+ ncurindex = index;
|
|
|
+ g_hasmap = true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if(index != ncurindex)
|
|
|
+ {
|
|
|
+ pose posex = CalcPose(gvector_trace[ncurindex].mndtorigin,gcurndtgpspos);
|
|
|
+ double fnowdis = gettracedis(ncurindex,posex);
|
|
|
+ if((fnowdis - ftracedis)>5)
|
|
|
+ {
|
|
|
+ UseMap(index);
|
|
|
+ ncurindex = index;
|
|
|
+ g_hasmap = true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(ftracedis > 50)
|
|
|
+ {
|
|
|
+ std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" trace dis is "<<ftracedis<<std::endl;
|
|
|
+ std::cout<<" Out Range."<<std::endl;
|
|
|
+ g_hasmap = false;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void SaveMonitor(bool * pbRun)
|
|
|
+{
|
|
|
+ iv::gpspos xoldgpspos;
|
|
|
+ xoldgpspos.lat = 39;
|
|
|
+ xoldgpspos.lon = 117;
|
|
|
+ while(*pbRun)
|
|
|
+ {
|
|
|
+ if(gpFileLastPos != 0)
|
|
|
+ {
|
|
|
+ if(gbFileNDTUpdate)
|
|
|
+ {
|
|
|
+ if((fabs(xoldgpspos.lat - gcurndtgpspos.lat)>0.0000005)||((fabs(xoldgpspos.lon - gcurndtgpspos.lon)>0.0000005)))
|
|
|
+ {
|
|
|
+ xoldgpspos = gcurndtgpspos;
|
|
|
+ char str[1000];
|
|
|
+ snprintf(str,1000,"%11.7f\t%11.7f\t%9.3f\t%6.2f\t%6.2f\t%6.2f\n ",
|
|
|
+ xoldgpspos.lon,xoldgpspos.lat,xoldgpspos.height,
|
|
|
+ xoldgpspos.heading,xoldgpspos.pitch,xoldgpspos.roll);
|
|
|
+ gpFileLastPos->seek(0);
|
|
|
+ gpFileLastPos->write(str,strnlen(str,1000));
|
|
|
+ gpFileLastPos->flush();
|
|
|
+ }
|
|
|
+ gbFileNDTUpdate = false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void GPSPosMonitor(bool * pbRun)
|
|
|
+{
|
|
|
+ if(gbGPSFix == false)
|
|
|
+ {
|
|
|
+ gcurndtgpspos = glastndtgpspos;
|
|
|
+ }
|
|
|
+ while(*pbRun)
|
|
|
+ {
|
|
|
+ int64_t nmsnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
|
|
|
+ if(abs(nmsnow - gnLastGPSTime)>1000)
|
|
|
+ {
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ if(gbGPSFix == true)
|
|
|
+ {
|
|
|
+ double x0,y0;
|
|
|
+ double x,y;
|
|
|
+ GaussProjCal(gcurndtgpspos.lon,gcurndtgpspos.lat,&x0,&y0);
|
|
|
+ GaussProjCal(gcurgpsgpspos.lon,gcurgpsgpspos.lat,&x,&y);
|
|
|
+ double dis = sqrt(pow(x-x0,2)+ pow(y-y0,2));
|
|
|
+ double headdiff = fabs(gcurgpsgpspos.heading - gcurndtgpspos.heading);
|
|
|
+ double zdiff = fabs(gcurgpsgpspos.height - gcurndtgpspos.height);
|
|
|
+ if((dis> 10)||((headdiff>10)&&(headdiff<350))||(zdiff>5)||(g_hasmap == false)||(gbNeedGPSUpdateNDT))
|
|
|
+ {
|
|
|
+ givlog->info("NDTFIX","gps fix ndt pos. dis:%f headdiff:%f zdiff:%f map %d ndt %d",
|
|
|
+ dis,headdiff,zdiff,g_hasmap,gbNeedGPSUpdateNDT);
|
|
|
+// gcurndtgpspos = gcurgpsgpspos;
|
|
|
+ gbgpsupdatendt = true;
|
|
|
+// current_velocity_x = 0;
|
|
|
+// current_velocity_y = 0;
|
|
|
+// current_velocity_z = 0;
|
|
|
+// angular_velocity = 0;
|
|
|
+// gbNeedGPSUpdateNDT = false;
|
|
|
+ if(g_hasmap == true)
|
|
|
+ {
|
|
|
+ int index = gcurmapindex;
|
|
|
+ if((index >=0)&&(index < gvector_trace.size()))
|
|
|
+ {
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// if(gbgpsupdatendt)
|
|
|
+// {
|
|
|
+// gcurndtgpspos = gcurgpsgpspos;
|
|
|
+// gbgpsupdatendt = true;
|
|
|
+// current_velocity_x = 0;
|
|
|
+// current_velocity_y = 0;
|
|
|
+// current_velocity_z = 0;
|
|
|
+// angular_velocity = 0;
|
|
|
+// }
|
|
|
+// }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void ndt_match_Init(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd)
|
|
|
+{
|
|
|
+ _tf_x = 0;
|
|
|
+ _tf_y = 0;
|
|
|
+ _tf_z = 0;
|
|
|
+ _tf_roll = 0;
|
|
|
+ _tf_pitch = 0;
|
|
|
+ _tf_yaw = 0;
|
|
|
+
|
|
|
+ Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z); // tl: translation
|
|
|
+ Eigen::AngleAxisf rot_x_btol(_tf_roll, Eigen::Vector3f::UnitX()); // rot: rotation
|
|
|
+ Eigen::AngleAxisf rot_y_btol(_tf_pitch, Eigen::Vector3f::UnitY());
|
|
|
+ Eigen::AngleAxisf rot_z_btol(_tf_yaw, Eigen::Vector3f::UnitZ());
|
|
|
+ tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ init_pos_set = 1;
|
|
|
+
|
|
|
+ initial_pose.x = 0;
|
|
|
+ initial_pose.y = 0;
|
|
|
+ initial_pose.z = 0;
|
|
|
+ initial_pose.roll = 0;
|
|
|
+ initial_pose.pitch = 0;
|
|
|
+ initial_pose.yaw = 0;
|
|
|
+
|
|
|
+ localizer_pose.x = initial_pose.x;
|
|
|
+ localizer_pose.y = initial_pose.y;
|
|
|
+ localizer_pose.z = initial_pose.z;
|
|
|
+ localizer_pose.roll = initial_pose.roll;
|
|
|
+ localizer_pose.pitch = initial_pose.pitch;
|
|
|
+ localizer_pose.yaw = initial_pose.yaw;
|
|
|
+
|
|
|
+ previous_pose.x = initial_pose.x;
|
|
|
+ previous_pose.y = initial_pose.y;
|
|
|
+ previous_pose.z = initial_pose.z;
|
|
|
+ previous_pose.roll = initial_pose.roll;
|
|
|
+ previous_pose.pitch = initial_pose.pitch;
|
|
|
+ previous_pose.yaw = initial_pose.yaw;
|
|
|
+
|
|
|
+ current_pose.x = initial_pose.x;
|
|
|
+ current_pose.y = initial_pose.y;
|
|
|
+ current_pose.z = initial_pose.z;
|
|
|
+ current_pose.roll = initial_pose.roll;
|
|
|
+ current_pose.pitch = initial_pose.pitch;
|
|
|
+ current_pose.yaw = initial_pose.yaw;
|
|
|
+
|
|
|
+ current_velocity = 0;
|
|
|
+ current_velocity_x = 0;
|
|
|
+ current_velocity_y = 0;
|
|
|
+ current_velocity_z = 0;
|
|
|
+ angular_velocity = 0;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+// ndt.align(*output_cloud, Eigen::Matrix4f::Identity());
|
|
|
+
|
|
|
+
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(*mappcd));
|
|
|
+
|
|
|
+
|
|
|
+ std::cout<<"map size is"<<mappcd->size()<<std::endl;
|
|
|
+ std::cout<<"ptr size is "<<map_ptr->size()<<std::endl;;
|
|
|
+
|
|
|
+ global_map_ptr = map_ptr;
|
|
|
+
|
|
|
+ // SetLocalMap();
|
|
|
+
|
|
|
+
|
|
|
+// pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan(new pcl::PointCloud<pcl::PointXYZ>());
|
|
|
+
|
|
|
+// pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter;
|
|
|
+// voxel_grid_filter.setLeafSize(1.0,1.0,1.0);
|
|
|
+// voxel_grid_filter.setInputCloud(map_ptr);
|
|
|
+// voxel_grid_filter.filter(*filtered_scan);
|
|
|
+
|
|
|
+// std::cout<<"filter map size is "<<filtered_scan->size()<<std::endl;;
|
|
|
+
|
|
|
+ // ndt.setInputTarget(map_ptr);
|
|
|
+// pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
+// ndt.align(*output_cloud, Eigen::Matrix4f::Identity());
|
|
|
+
|
|
|
+
|
|
|
+ gpset = iv::modulecomm::RegisterRecv("ndtposeset",ListenPoseSet);
|
|
|
+ gppose = iv::modulecomm::RegisterSend("ndtpose",10000,3);
|
|
|
+
|
|
|
+ if(map_ptr->size() == 0)
|
|
|
+ {
|
|
|
+ gbNDTRun = false;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
|
|
|
+ pcl::PointXYZ dummy_point;
|
|
|
+ dummy_scan_ptr->push_back(dummy_point);
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if(map_ptr->size()>0)map_loaded = 1;
|
|
|
+
|
|
|
+ // gpmapthread = new std::thread(LocalMapUpdate,1);
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void ndt_match_Init_nomap()
|
|
|
+{
|
|
|
+ _tf_x = 0;
|
|
|
+ _tf_y = 0;
|
|
|
+ _tf_z = 0;
|
|
|
+ _tf_roll = 0;
|
|
|
+ _tf_pitch = 0;
|
|
|
+ _tf_yaw = 0;
|
|
|
+
|
|
|
+ Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z); // tl: translation
|
|
|
+ Eigen::AngleAxisf rot_x_btol(_tf_roll, Eigen::Vector3f::UnitX()); // rot: rotation
|
|
|
+ Eigen::AngleAxisf rot_y_btol(_tf_pitch, Eigen::Vector3f::UnitY());
|
|
|
+ Eigen::AngleAxisf rot_z_btol(_tf_yaw, Eigen::Vector3f::UnitZ());
|
|
|
+ tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ init_pos_set = 1;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ initial_pose.x = 0;
|
|
|
+ initial_pose.y = 0;
|
|
|
+ initial_pose.z = 0;
|
|
|
+ initial_pose.roll = 0;
|
|
|
+ initial_pose.pitch = 0;
|
|
|
+ initial_pose.yaw = 0;
|
|
|
+
|
|
|
+ localizer_pose.x = initial_pose.x;
|
|
|
+ localizer_pose.y = initial_pose.y;
|
|
|
+ localizer_pose.z = initial_pose.z;
|
|
|
+ localizer_pose.roll = initial_pose.roll;
|
|
|
+ localizer_pose.pitch = initial_pose.pitch;
|
|
|
+ localizer_pose.yaw = initial_pose.yaw;
|
|
|
+
|
|
|
+ previous_pose.x = initial_pose.x;
|
|
|
+ previous_pose.y = initial_pose.y;
|
|
|
+ previous_pose.z = initial_pose.z;
|
|
|
+ previous_pose.roll = initial_pose.roll;
|
|
|
+ previous_pose.pitch = initial_pose.pitch;
|
|
|
+ previous_pose.yaw = initial_pose.yaw;
|
|
|
+
|
|
|
+ current_pose.x = initial_pose.x;
|
|
|
+ current_pose.y = initial_pose.y;
|
|
|
+ current_pose.z = initial_pose.z;
|
|
|
+ current_pose.roll = initial_pose.roll;
|
|
|
+ current_pose.pitch = initial_pose.pitch;
|
|
|
+ current_pose.yaw = initial_pose.yaw;
|
|
|
+
|
|
|
+ current_velocity = 0;
|
|
|
+ current_velocity_x = 0;
|
|
|
+ current_velocity_y = 0;
|
|
|
+ current_velocity_z = 0;
|
|
|
+ angular_velocity = 0;
|
|
|
+
|
|
|
+
|
|
|
+ gpmapthread = new std::thread(LocalMapUpdate,1);
|
|
|
+ gbGFRun = true;
|
|
|
+ gpgpsfixthread = new std::thread(GPSPosMonitor,&gbGFRun);
|
|
|
+ gpsavelastthread = new std::thread(SaveMonitor,&gbGFRun);
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void ndt_match_SetMap(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd)
|
|
|
+{
|
|
|
+ if(mappcd->size() == 0 )return;
|
|
|
+
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(*mappcd));
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
|
|
|
+ pcl::PointXYZ dummy_point;
|
|
|
+ dummy_scan_ptr->push_back(dummy_point);
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if(map_ptr->size()>0)map_loaded = 1;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+static double wrapToPm(double a_num, const double a_max)
|
|
|
+{
|
|
|
+ if (a_num >= a_max)
|
|
|
+ {
|
|
|
+ a_num -= 2.0 * a_max;
|
|
|
+ }
|
|
|
+ return a_num;
|
|
|
+}
|
|
|
+
|
|
|
+static double wrapToPmPi(const double a_angle_rad)
|
|
|
+{
|
|
|
+ return wrapToPm(a_angle_rad, M_PI);
|
|
|
+}
|
|
|
+
|
|
|
+static double calcDiffForRadian(const double lhs_rad, const double rhs_rad)
|
|
|
+{
|
|
|
+ double diff_rad = lhs_rad - rhs_rad;
|
|
|
+ if (diff_rad >= M_PI)
|
|
|
+ diff_rad = diff_rad - 2 * M_PI;
|
|
|
+ else if (diff_rad < -M_PI)
|
|
|
+ diff_rad = diff_rad + 2 * M_PI;
|
|
|
+ return diff_rad;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+static unsigned int g_seq_old = 0;
|
|
|
+
|
|
|
+#include <QTime>
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+bool isfirst = true;
|
|
|
+
|
|
|
+#include <QMutex>
|
|
|
+std::vector<iv::imudata> gvectorimu;
|
|
|
+QMutex gMuteximu;
|
|
|
+
|
|
|
+static void lidar_imu_calc(qint64 current_lidar_time)
|
|
|
+{
|
|
|
+ int nsize;
|
|
|
+
|
|
|
+ int i;
|
|
|
+ gMuteximu.lock();
|
|
|
+ nsize = gvectorimu.size();
|
|
|
+ for(i=0;i<nsize;i++)
|
|
|
+ {
|
|
|
+ iv::imudata ximudata = gvectorimu[i];
|
|
|
+ double ximu_angular_velocity_x,ximu_angular_velocity_y,ximu_angular_velocity_z;
|
|
|
+ double ximu_linear_acceleration_x,ximu_linear_acceleration_y,ximu_linear_acceleration_z;
|
|
|
+ if(current_lidar_time > ximudata.imutime)
|
|
|
+ {
|
|
|
+ ximu_angular_velocity_x = ximudata.imu_angular_velocity_x;
|
|
|
+ ximu_angular_velocity_y = ximudata.imu_angular_velocity_y;
|
|
|
+ ximu_angular_velocity_z = ximudata.imu_angular_velocity_z;
|
|
|
+ ximu_linear_acceleration_x = ximudata.imu_linear_acceleration_x;
|
|
|
+ ximu_linear_acceleration_y = ximudata.imu_linear_acceleration_y;
|
|
|
+ ximu_linear_acceleration_z = ximudata.imu_linear_acceleration_z;
|
|
|
+ qint64 current_time_imu = ximudata.imutime;
|
|
|
+// givlog->verbose("NDT","imu time is %ld",current_time_imu);
|
|
|
+ static qint64 previous_time_imu = current_time_imu;
|
|
|
+ double diff_time = (current_time_imu - previous_time_imu);
|
|
|
+ diff_time = diff_time/1000.0;
|
|
|
+
|
|
|
+ if(diff_time < 0)diff_time = 0.000001;
|
|
|
+ if(diff_time > 0.1)diff_time = 0.1;
|
|
|
+
|
|
|
+ if(current_time_imu < previous_time_imu)
|
|
|
+ {
|
|
|
+
|
|
|
+ std::cout<<"current time is old "<<current_time_imu<<std::endl;
|
|
|
+ previous_time_imu = current_time_imu;
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+
|
|
|
+ double diff_imu_roll = ximu_angular_velocity_x * diff_time;
|
|
|
+ double diff_imu_pitch = ximu_angular_velocity_y * diff_time;
|
|
|
+ double diff_imu_yaw = ximu_angular_velocity_z * diff_time;
|
|
|
+
|
|
|
+ current_pose_imu.roll += diff_imu_roll;
|
|
|
+ current_pose_imu.pitch += diff_imu_pitch;
|
|
|
+ current_pose_imu.yaw += diff_imu_yaw;
|
|
|
+
|
|
|
+ double accX1 = ximu_linear_acceleration_x;
|
|
|
+ double accY1 = std::cos(current_pose_imu.roll) * ximu_linear_acceleration_y -
|
|
|
+ std::sin(current_pose_imu.roll) * ximu_linear_acceleration_z;
|
|
|
+ double accZ1 = std::sin(current_pose_imu.roll) * ximu_linear_acceleration_y +
|
|
|
+ std::cos(current_pose_imu.roll) * ximu_linear_acceleration_z;
|
|
|
+
|
|
|
+ double accX2 = std::sin(current_pose_imu.pitch) * accZ1 + std::cos(current_pose_imu.pitch) * accX1;
|
|
|
+ double accY2 = accY1;
|
|
|
+ double accZ2 = std::cos(current_pose_imu.pitch) * accZ1 - std::sin(current_pose_imu.pitch) * accX1;
|
|
|
+
|
|
|
+ double accX = std::cos(current_pose_imu.yaw) * accX2 - std::sin(current_pose_imu.yaw) * accY2;
|
|
|
+ double accY = std::sin(current_pose_imu.yaw) * accX2 + std::cos(current_pose_imu.yaw) * accY2;
|
|
|
+ double accZ = accZ2;
|
|
|
+
|
|
|
+ offset_imu_x += current_velocity_imu_x * diff_time + accX * diff_time * diff_time / 2.0;
|
|
|
+ offset_imu_y += current_velocity_imu_y * diff_time + accY * diff_time * diff_time / 2.0;
|
|
|
+ offset_imu_z += current_velocity_imu_z * diff_time + accZ * diff_time * diff_time / 2.0;
|
|
|
+
|
|
|
+ current_velocity_imu_x += accX * diff_time;
|
|
|
+ current_velocity_imu_y += accY * diff_time;
|
|
|
+ current_velocity_imu_z += accZ * diff_time;
|
|
|
+
|
|
|
+ offset_imu_roll += diff_imu_roll;
|
|
|
+ offset_imu_pitch += diff_imu_pitch;
|
|
|
+ offset_imu_yaw += diff_imu_yaw;
|
|
|
+
|
|
|
+ // guess_pose_imu.x = previous_pose.x + offset_imu_x;
|
|
|
+ // guess_pose_imu.y = previous_pose.y + offset_imu_y;
|
|
|
+ // guess_pose_imu.z = previous_pose.z + offset_imu_z;
|
|
|
+ // guess_pose_imu.roll = previous_pose.roll + offset_imu_roll;
|
|
|
+ // guess_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
|
|
|
+ // guess_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;
|
|
|
+
|
|
|
+ predict_pose_imu.x = previous_pose.x + offset_imu_x;
|
|
|
+ predict_pose_imu.y = previous_pose.y + offset_imu_y;
|
|
|
+ predict_pose_imu.z = previous_pose.z + offset_imu_z;
|
|
|
+ predict_pose_imu.roll = previous_pose.roll + offset_imu_roll;
|
|
|
+ predict_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
|
|
|
+ predict_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;
|
|
|
+
|
|
|
+ givlog->verbose("NDT","imu x:%f y:%f z:%f yaw:%f",offset_imu_x,
|
|
|
+ offset_imu_y,offset_imu_z,offset_imu_yaw);
|
|
|
+
|
|
|
+ previous_time_imu = current_time_imu;
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ break;;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ if(i>0)
|
|
|
+ {
|
|
|
+ gvectorimu.erase(gvectorimu.begin(),gvectorimu.begin()+i);
|
|
|
+ }
|
|
|
+
|
|
|
+ gMuteximu.unlock();
|
|
|
+
|
|
|
+// for(i=0;i<gvectorimu.size();i++)
|
|
|
+// {
|
|
|
+// iv::imudata ximudata = gvectorimu[i];
|
|
|
+// double ximu_angular_velocity_x,ximu_angular_velocity_y,ximu_angular_velocity_z;
|
|
|
+// double ximu_linear_acceleration_x,ximu_linear_acceleration_y,ximu_linear_acceleration_z;
|
|
|
+// if(current_lidar_time > ximudata.imutime)
|
|
|
+// {
|
|
|
+
|
|
|
+// gvectorimu.erase(gvectorimu.begin())
|
|
|
+// }
|
|
|
+// }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+static void imu_calc(qint64 current_time_imu)
|
|
|
+{
|
|
|
+
|
|
|
+ static qint64 previous_time_imu = current_time_imu;
|
|
|
+ double diff_time = (current_time_imu - previous_time_imu);
|
|
|
+ diff_time = diff_time/1000.0;
|
|
|
+
|
|
|
+ if(current_time_imu < previous_time_imu)
|
|
|
+ {
|
|
|
+ std::cout<<"current time is old "<<current_time_imu<<std::endl;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ double diff_imu_roll = imu_angular_velocity_x * diff_time;
|
|
|
+ double diff_imu_pitch = imu_angular_velocity_y * diff_time;
|
|
|
+ double diff_imu_yaw = imu_angular_velocity_z * diff_time;
|
|
|
+
|
|
|
+ current_pose_imu.roll += diff_imu_roll;
|
|
|
+ current_pose_imu.pitch += diff_imu_pitch;
|
|
|
+ current_pose_imu.yaw += diff_imu_yaw;
|
|
|
+
|
|
|
+ double accX1 = imu_linear_acceleration_x;
|
|
|
+ double accY1 = std::cos(current_pose_imu.roll) * imu_linear_acceleration_y -
|
|
|
+ std::sin(current_pose_imu.roll) * imu_linear_acceleration_z;
|
|
|
+ double accZ1 = std::sin(current_pose_imu.roll) * imu_linear_acceleration_y +
|
|
|
+ std::cos(current_pose_imu.roll) * imu_linear_acceleration_z;
|
|
|
+
|
|
|
+ double accX2 = std::sin(current_pose_imu.pitch) * accZ1 + std::cos(current_pose_imu.pitch) * accX1;
|
|
|
+ double accY2 = accY1;
|
|
|
+ double accZ2 = std::cos(current_pose_imu.pitch) * accZ1 - std::sin(current_pose_imu.pitch) * accX1;
|
|
|
+
|
|
|
+ double accX = std::cos(current_pose_imu.yaw) * accX2 - std::sin(current_pose_imu.yaw) * accY2;
|
|
|
+ double accY = std::sin(current_pose_imu.yaw) * accX2 + std::cos(current_pose_imu.yaw) * accY2;
|
|
|
+ double accZ = accZ2;
|
|
|
+
|
|
|
+ offset_imu_x += current_velocity_imu_x * diff_time + accX * diff_time * diff_time / 2.0;
|
|
|
+ offset_imu_y += current_velocity_imu_y * diff_time + accY * diff_time * diff_time / 2.0;
|
|
|
+ offset_imu_z += current_velocity_imu_z * diff_time + accZ * diff_time * diff_time / 2.0;
|
|
|
+
|
|
|
+ current_velocity_imu_x += accX * diff_time;
|
|
|
+ current_velocity_imu_y += accY * diff_time;
|
|
|
+ current_velocity_imu_z += accZ * diff_time;
|
|
|
+
|
|
|
+ offset_imu_roll += diff_imu_roll;
|
|
|
+ offset_imu_pitch += diff_imu_pitch;
|
|
|
+ offset_imu_yaw += diff_imu_yaw;
|
|
|
+
|
|
|
+// guess_pose_imu.x = previous_pose.x + offset_imu_x;
|
|
|
+// guess_pose_imu.y = previous_pose.y + offset_imu_y;
|
|
|
+// guess_pose_imu.z = previous_pose.z + offset_imu_z;
|
|
|
+// guess_pose_imu.roll = previous_pose.roll + offset_imu_roll;
|
|
|
+// guess_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
|
|
|
+// guess_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;
|
|
|
+
|
|
|
+ predict_pose_imu.x = previous_pose.x + offset_imu_x;
|
|
|
+ predict_pose_imu.y = previous_pose.y + offset_imu_y;
|
|
|
+ predict_pose_imu.z = previous_pose.z + offset_imu_z;
|
|
|
+ predict_pose_imu.roll = previous_pose.roll + offset_imu_roll;
|
|
|
+ predict_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
|
|
|
+ predict_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;
|
|
|
+
|
|
|
+ givlog->verbose("NDT","imu x:%f y:%f z:%f yaw:%f",offset_imu_x,
|
|
|
+ offset_imu_y,offset_imu_z,offset_imu_yaw);
|
|
|
+
|
|
|
+ previous_time_imu = current_time_imu;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void imu_update(qint64 current_time_imu,double imu_roll,double imu_pitch,double imu_yaw,
|
|
|
+ double acceleration_x,double acceleration_y,
|
|
|
+ double acceleration_z)
|
|
|
+{
|
|
|
+ // std::cout << __func__ << std::endl;
|
|
|
+
|
|
|
+// double imu_angular_velocity_x,imu_angular_velocity_y,imu_angular_velocity_z;
|
|
|
+
|
|
|
+ static double previous_time_imu = current_time_imu;
|
|
|
+ double diff_time = (current_time_imu - previous_time_imu);
|
|
|
+ diff_time = diff_time/1000.0;
|
|
|
+
|
|
|
+
|
|
|
+ imu_roll = imu_roll * M_PI/180.0;
|
|
|
+ imu_pitch = imu_pitch * M_PI/180.0;
|
|
|
+ imu_yaw = (-1.0)*imu_yaw * M_PI/180.0;
|
|
|
+
|
|
|
+ imu_yaw = imu_yaw + 2.0*M_PI;
|
|
|
+
|
|
|
+// imu_pitch = 0;
|
|
|
+// imu_roll = 0;
|
|
|
+
|
|
|
+ imu_roll = wrapToPmPi(imu_roll);
|
|
|
+ imu_pitch = wrapToPmPi(imu_pitch);
|
|
|
+ imu_yaw = wrapToPmPi(imu_yaw);
|
|
|
+
|
|
|
+ static double previous_imu_roll = imu_roll, previous_imu_pitch = imu_pitch, previous_imu_yaw = imu_yaw;
|
|
|
+ const double diff_imu_roll = imu_roll - previous_imu_roll;
|
|
|
+
|
|
|
+ const double diff_imu_pitch = imu_pitch - previous_imu_pitch;
|
|
|
+
|
|
|
+ double diff_imu_yaw;
|
|
|
+ if (fabs(imu_yaw - previous_imu_yaw) > M_PI)
|
|
|
+ {
|
|
|
+ if (imu_yaw > 0)
|
|
|
+ diff_imu_yaw = (imu_yaw - previous_imu_yaw) - M_PI * 2;
|
|
|
+ else
|
|
|
+ diff_imu_yaw = -M_PI * 2 - (imu_yaw - previous_imu_yaw);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ diff_imu_yaw = imu_yaw - previous_imu_yaw;
|
|
|
+
|
|
|
+
|
|
|
+ imu_linear_acceleration_x = acceleration_x;
|
|
|
+// imu_linear_acceleration_y = acceleration_y;
|
|
|
+// imu_linear_acceleration_z = acceleration_z;
|
|
|
+ imu_linear_acceleration_y = 0;
|
|
|
+ imu_linear_acceleration_z = 0;
|
|
|
+
|
|
|
+ if (diff_time != 0)
|
|
|
+ {
|
|
|
+ imu_angular_velocity_x = diff_imu_roll / diff_time;
|
|
|
+ imu_angular_velocity_y = diff_imu_pitch / diff_time;
|
|
|
+ imu_angular_velocity_z = diff_imu_yaw / diff_time;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ imu_angular_velocity_x = 0;
|
|
|
+ imu_angular_velocity_y = 0;
|
|
|
+ imu_angular_velocity_z = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ iv::imudata ximudata;
|
|
|
+ ximudata.imutime = current_time_imu;
|
|
|
+ ximudata.imu_linear_acceleration_x = imu_linear_acceleration_x;
|
|
|
+ ximudata.imu_linear_acceleration_y = imu_linear_acceleration_y;
|
|
|
+ ximudata.imu_linear_acceleration_z = imu_linear_acceleration_z;
|
|
|
+ ximudata.imu_angular_velocity_x = imu_angular_velocity_x;
|
|
|
+ ximudata.imu_angular_velocity_y = imu_angular_velocity_y;
|
|
|
+ ximudata.imu_angular_velocity_z = imu_angular_velocity_z;
|
|
|
+
|
|
|
+ gMuteximu.lock();
|
|
|
+ gvectorimu.push_back(ximudata);
|
|
|
+ gMuteximu.unlock();
|
|
|
+// imu_calc(current_time_imu);
|
|
|
+
|
|
|
+ previous_time_imu = current_time_imu;
|
|
|
+ previous_imu_roll = imu_roll;
|
|
|
+ previous_imu_pitch = imu_pitch;
|
|
|
+ previous_imu_yaw = imu_yaw;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+#ifdef TEST_CALCTIME
|
|
|
+int ncalc = 0;
|
|
|
+int gncalctotal = 0;
|
|
|
+#endif
|
|
|
+void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
|
|
|
+{
|
|
|
+
|
|
|
+ static bool bNeedUpdateVel = false; //if gps update ndt, need update velocity
|
|
|
+ qint64 current_scan_time = raw_scan->header.stamp;
|
|
|
+ static qint64 old_scan_time = current_scan_time;
|
|
|
+ if(gbNDTRun == false)return;
|
|
|
+
|
|
|
+ bool bNotChangev = false;
|
|
|
+
|
|
|
+ if(gbgpsupdatendt == true)
|
|
|
+ {
|
|
|
+
|
|
|
+ std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" update ndt pos use gps pos. "<<std::endl;
|
|
|
+ gcurndtgpspos = gcurgpsgpspos;
|
|
|
+ gbgpsupdatendt = false;
|
|
|
+ gbNeedGPSUpdateNDT = false;
|
|
|
+ current_velocity_x = 0;
|
|
|
+ current_velocity_y = 0;
|
|
|
+ current_velocity_z = 0;
|
|
|
+ angular_velocity = 0;
|
|
|
+ bNeedUpdateVel = true;
|
|
|
+
|
|
|
+//
|
|
|
+//
|
|
|
+// gcurndtgpspos = gcurgpsgpspos;
|
|
|
+// current_pose = CalcPose(gvector_trace[gusemapindex].mndtorigin,gcurndtgpspos);
|
|
|
+// predict_pose_for_ndt = current_pose;
|
|
|
+ current_velocity_imu_x = 0;
|
|
|
+ current_velocity_imu_y = 0;
|
|
|
+ current_velocity_imu_z = 0;
|
|
|
+ gMuteximu.lock();
|
|
|
+ gvectorimu.erase(gvectorimu.begin(),gvectorimu.begin() + gvectorimu.size());
|
|
|
+ gMuteximu.unlock();
|
|
|
+ bNotChangev = true;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+// gbgpsupdatendt = false;
|
|
|
+
|
|
|
+ if(g_hasmap == false)
|
|
|
+ {
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ if(gbNeedUpdate)
|
|
|
+ {
|
|
|
+ pthread_mutex_lock(&mutex);
|
|
|
+ ndtraw = glocalndtraw;
|
|
|
+ pthread_mutex_unlock(&mutex);
|
|
|
+ gusemapindex = gcurmapindex;
|
|
|
+ gbNeedUpdate = false;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ previous_pose = CalcPose(gvector_trace[gusemapindex].mndtorigin,gcurndtgpspos);
|
|
|
+ if(bNeedUpdateVel == true)
|
|
|
+ {
|
|
|
+ bNeedUpdateVel = false;
|
|
|
+ current_velocity_x = previous_pose.vx;
|
|
|
+ current_velocity_y = previous_pose.vy;
|
|
|
+ current_velocity_imu_x = current_velocity_x;
|
|
|
+ current_velocity_imu_y = current_velocity_y;
|
|
|
+ }
|
|
|
+ givlog->verbose("previos pose is %f %f",previous_pose.x,previous_pose.y);
|
|
|
+
|
|
|
+// if(map_loaded == 0)
|
|
|
+// {
|
|
|
+// std::cout<<BOLDRED<<" point_ndt not set map."<<RESET<<std::endl;
|
|
|
+// return;
|
|
|
+// }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ pthread_mutex_lock(&mutex_pose);
|
|
|
+
|
|
|
+ QTime xTime;
|
|
|
+ xTime.start();
|
|
|
+// std::cout<<"time is "<<xTime.elapsed()<<std::endl;
|
|
|
+ double voxel_leaf_size = 2.0;
|
|
|
+ double measurement_range = 200.0;
|
|
|
+
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan(new pcl::PointCloud<pcl::PointXYZ>());
|
|
|
+
|
|
|
+ pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter;
|
|
|
+ voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);
|
|
|
+ voxel_grid_filter.setInputCloud(raw_scan);
|
|
|
+ voxel_grid_filter.filter(*filtered_scan);
|
|
|
+
|
|
|
+// std::cout<<"voxed time is "<<xTime.elapsed()<<std::endl;
|
|
|
+
|
|
|
+// std::cout<<" seq is "<<raw_scan->header.seq<<std::endl;
|
|
|
+ // qDebug("seq = %d stamp = %d ",raw_scan->header.seq,raw_scan->header.stamp);
|
|
|
+ // std::cout<<"raw scan size is "<<raw_scan->size()<<" filtered scan size is "<<filtered_scan->size()<<std::endl;
|
|
|
+
|
|
|
+ // return;
|
|
|
+ matching_start = std::chrono::system_clock::now();
|
|
|
+
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(*filtered_scan));
|
|
|
+
|
|
|
+ int scan_points_num = filtered_scan_ptr->size();
|
|
|
+
|
|
|
+ Eigen::Matrix4f t(Eigen::Matrix4f::Identity()); // base_link
|
|
|
+ Eigen::Matrix4f t2(Eigen::Matrix4f::Identity()); // localizer
|
|
|
+
|
|
|
+ std::chrono::time_point<std::chrono::system_clock> align_start, align_end, getFitnessScore_start,
|
|
|
+ getFitnessScore_end;
|
|
|
+ static double align_time, getFitnessScore_time = 0.0;
|
|
|
+
|
|
|
+// std::cout<<"run hear"<<std::endl;
|
|
|
+ pthread_mutex_lock(&mutex);
|
|
|
+
|
|
|
+// if (_method_type == MethodType::PCL_GENERIC)
|
|
|
+ // ndt.setInputSource(filtered_scan_ptr);
|
|
|
+
|
|
|
+
|
|
|
+ ndtraw->setInputSource(filtered_scan_ptr);
|
|
|
+
|
|
|
+ // Guess the initial gross estimation of the transformation
|
|
|
+// double diff_time = (current_scan_time - previous_scan_time).toSec();
|
|
|
+
|
|
|
+
|
|
|
+ double diff_time = 0.0;
|
|
|
+
|
|
|
+ if(g_seq_old != 0)
|
|
|
+ {
|
|
|
+ if((raw_scan->header.seq - g_seq_old)>0)
|
|
|
+ {
|
|
|
+ diff_time = raw_scan->header.seq - g_seq_old;
|
|
|
+ diff_time = diff_time * 0.1;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ g_seq_old = raw_scan->header.seq;
|
|
|
+
|
|
|
+ diff_time = current_scan_time -old_scan_time;
|
|
|
+ diff_time = diff_time/1000.0;
|
|
|
+ old_scan_time = current_scan_time;
|
|
|
+
|
|
|
+ if(diff_time<=0)diff_time = 0.1;
|
|
|
+ if(diff_time>1.0)diff_time = 0.1;
|
|
|
+
|
|
|
+// std::cout<<"diff time is "<<diff_time<<std::endl;
|
|
|
+
|
|
|
+// std::cout<<" diff time is "<<diff_time<<std::endl;
|
|
|
+
|
|
|
+// diff_time = 0.0;
|
|
|
+
|
|
|
+// if (_offset == "linear")
|
|
|
+// {
|
|
|
+
|
|
|
+ if(diff_time<0.1)diff_time = 0.1;
|
|
|
+ offset_x = current_velocity_x * diff_time;
|
|
|
+ offset_y = current_velocity_y * diff_time;
|
|
|
+ offset_z = current_velocity_z * diff_time;
|
|
|
+ offset_yaw = angular_velocity * diff_time;
|
|
|
+// }
|
|
|
+ predict_pose.x = previous_pose.x + offset_x;
|
|
|
+ predict_pose.y = previous_pose.y + offset_y;
|
|
|
+ predict_pose.z = previous_pose.z + offset_z;
|
|
|
+ predict_pose.roll = previous_pose.roll;
|
|
|
+ predict_pose.pitch = previous_pose.pitch;
|
|
|
+ predict_pose.yaw = previous_pose.yaw + offset_yaw;
|
|
|
+
|
|
|
+ pose predict_pose_for_ndt;
|
|
|
+
|
|
|
+
|
|
|
+ givlog->verbose("NDT","previous x:%f y:%f z:%f yaw:%f",previous_pose.x,
|
|
|
+ previous_pose.y,previous_pose.z,previous_pose.yaw);
|
|
|
+// if (_use_imu == true && _use_odom == true)
|
|
|
+// imu_odom_calc(current_scan_time);
|
|
|
+ if (_use_imu == true && _use_odom == false)
|
|
|
+ {
|
|
|
+ lidar_imu_calc((current_scan_time-0));
|
|
|
+ }
|
|
|
+// if (_use_imu == false && _use_odom == true)
|
|
|
+// odom_calc(current_scan_time);
|
|
|
+
|
|
|
+// if (_use_imu == true && _use_odom == true)
|
|
|
+// predict_pose_for_ndt = predict_pose_imu_odom;
|
|
|
+// else
|
|
|
+// if (_use_imu == true && _use_odom == false)
|
|
|
+// predict_pose_for_ndt = predict_pose_imu;
|
|
|
+// else if (_use_imu == false && _use_odom == true)
|
|
|
+// predict_pose_for_ndt = predict_pose_odom;
|
|
|
+// else
|
|
|
+// predict_pose_for_ndt = predict_pose;
|
|
|
+
|
|
|
+ if (_use_imu == true && _use_odom == false)
|
|
|
+ {
|
|
|
+ predict_pose_for_ndt = predict_pose_imu;
|
|
|
+ // predict_pose_for_ndt = predict_pose;
|
|
|
+ // predict_pose_for_ndt.yaw = predict_pose_imu.yaw;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ predict_pose_for_ndt = predict_pose;
|
|
|
+ }
|
|
|
+
|
|
|
+ predict_pose_for_ndt = predict_pose; //not use predict_pose_imu in shelan.
|
|
|
+
|
|
|
+ if(fabs(predict_pose_for_ndt.x -previous_pose.x)>10)
|
|
|
+ {
|
|
|
+ predict_pose_for_ndt = previous_pose;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(fabs(predict_pose_for_ndt.y -previous_pose.y)>10)
|
|
|
+ {
|
|
|
+ predict_pose_for_ndt = previous_pose;
|
|
|
+ }
|
|
|
+
|
|
|
+ // predict_pose_for_ndt = predict_pose;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ givlog->verbose("NDT","pre x:%f y:%f z:%f yaw:%f",predict_pose_for_ndt.x,
|
|
|
+ predict_pose_for_ndt.y,predict_pose_for_ndt.z,predict_pose_for_ndt.yaw);
|
|
|
+// predict_pose_for_ndt = predict_pose;
|
|
|
+
|
|
|
+ Eigen::Translation3f init_translation(predict_pose_for_ndt.x, predict_pose_for_ndt.y, predict_pose_for_ndt.z);
|
|
|
+ Eigen::AngleAxisf init_rotation_x(predict_pose_for_ndt.roll, Eigen::Vector3f::UnitX());
|
|
|
+ Eigen::AngleAxisf init_rotation_y(predict_pose_for_ndt.pitch, Eigen::Vector3f::UnitY());
|
|
|
+ Eigen::AngleAxisf init_rotation_z(predict_pose_for_ndt.yaw, Eigen::Vector3f::UnitZ());
|
|
|
+ Eigen::Matrix4f init_guess = (init_translation * init_rotation_z * init_rotation_y * init_rotation_x) * tf_btol;
|
|
|
+
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
+
|
|
|
+ // std::cout<<"before ndt time is "<<xTime.elapsed()<<std::endl;
|
|
|
+// std::cout<<"time is "<<xTime.elapsed()<<std::endl;
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>());
|
|
|
+ align_start = std::chrono::system_clock::now();
|
|
|
+ ndtraw->align(*aligned,init_guess);
|
|
|
+ align_end = std::chrono::system_clock::now();
|
|
|
+
|
|
|
+ if(xTime.elapsed()<90)
|
|
|
+ std::cout<<GREEN<<"after ndt time is "<<xTime.elapsed()<<RESET<<std::endl;
|
|
|
+ else
|
|
|
+ std::cout<<RED<<"after ndt time is "<<xTime.elapsed()<<RESET<<std::endl;
|
|
|
+
|
|
|
+
|
|
|
+#ifdef TEST_CALCTIME
|
|
|
+
|
|
|
+ gncalctotal = gncalctotal + xTime.elapsed();
|
|
|
+ ncalc++;
|
|
|
+ if(ncalc>0)
|
|
|
+ {
|
|
|
+ std::cout<<" average calc time: "<<gncalctotal/ncalc<<std::endl;
|
|
|
+
|
|
|
+ }
|
|
|
+#endif
|
|
|
+
|
|
|
+ has_converged = ndtraw->hasConverged();
|
|
|
+
|
|
|
+ t = ndtraw->getFinalTransformation();
|
|
|
+ iteration = ndtraw->getFinalNumIteration();
|
|
|
+
|
|
|
+ getFitnessScore_start = std::chrono::system_clock::now();
|
|
|
+ fitness_score = ndtraw->getFitnessScore();
|
|
|
+ getFitnessScore_end = std::chrono::system_clock::now();
|
|
|
+
|
|
|
+ trans_probability = ndtraw->getTransformationProbability();
|
|
|
+
|
|
|
+ std::cout<<"score: "<<fitness_score<<" trans_probability:"<<trans_probability<<std::endl;
|
|
|
+ // std::cout<<" scoure is "<<fitness_score<<std::endl;
|
|
|
+
|
|
|
+// std::cout<<" iter is "<<iteration<<std::endl;
|
|
|
+
|
|
|
+ t2 = t * tf_btol.inverse();
|
|
|
+
|
|
|
+ getFitnessScore_time =
|
|
|
+ std::chrono::duration_cast<std::chrono::microseconds>(getFitnessScore_end - getFitnessScore_start).count() /
|
|
|
+ 1000.0;
|
|
|
+
|
|
|
+ pthread_mutex_unlock(&mutex);
|
|
|
+
|
|
|
+ tf::Matrix3x3 mat_l; // localizer
|
|
|
+ mat_l.setValue(static_cast<double>(t(0, 0)), static_cast<double>(t(0, 1)), static_cast<double>(t(0, 2)),
|
|
|
+ static_cast<double>(t(1, 0)), static_cast<double>(t(1, 1)), static_cast<double>(t(1, 2)),
|
|
|
+ static_cast<double>(t(2, 0)), static_cast<double>(t(2, 1)), static_cast<double>(t(2, 2)));
|
|
|
+
|
|
|
+ // Update localizer_pose
|
|
|
+ localizer_pose.x = t(0, 3);
|
|
|
+ localizer_pose.y = t(1, 3);
|
|
|
+ localizer_pose.z = t(2, 3);
|
|
|
+ mat_l.getRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw, 1);
|
|
|
+
|
|
|
+
|
|
|
+// std::cout<<" local pose x is "<<localizer_pose.x<<std::endl;
|
|
|
+
|
|
|
+ tf::Matrix3x3 mat_b; // base_link
|
|
|
+ mat_b.setValue(static_cast<double>(t2(0, 0)), static_cast<double>(t2(0, 1)), static_cast<double>(t2(0, 2)),
|
|
|
+ static_cast<double>(t2(1, 0)), static_cast<double>(t2(1, 1)), static_cast<double>(t2(1, 2)),
|
|
|
+ static_cast<double>(t2(2, 0)), static_cast<double>(t2(2, 1)), static_cast<double>(t2(2, 2)));
|
|
|
+
|
|
|
+ // Update ndt_pose
|
|
|
+ ndt_pose.x = t2(0, 3);
|
|
|
+ ndt_pose.y = t2(1, 3);
|
|
|
+ ndt_pose.z = t2(2, 3);
|
|
|
+
|
|
|
+// ndt_pose.x = localizer_pose.x;
|
|
|
+// ndt_pose.y = localizer_pose.y;
|
|
|
+// ndt_pose.z = localizer_pose.z;
|
|
|
+ mat_b.getRPY(ndt_pose.roll, ndt_pose.pitch, ndt_pose.yaw, 1);
|
|
|
+
|
|
|
+
|
|
|
+ givlog->verbose("NDT","calc x:%f y:%f z:%f yaw:%f",ndt_pose.x,
|
|
|
+ ndt_pose.y,ndt_pose.z,ndt_pose.yaw);
|
|
|
+
|
|
|
+
|
|
|
+ // Calculate the difference between ndt_pose and predict_pose
|
|
|
+ predict_pose_error = sqrt((ndt_pose.x - predict_pose_for_ndt.x) * (ndt_pose.x - predict_pose_for_ndt.x) +
|
|
|
+ (ndt_pose.y - predict_pose_for_ndt.y) * (ndt_pose.y - predict_pose_for_ndt.y) +
|
|
|
+ (ndt_pose.z - predict_pose_for_ndt.z) * (ndt_pose.z - predict_pose_for_ndt.z));
|
|
|
+
|
|
|
+
|
|
|
+ // std::cout<<" pose error is "<<predict_pose_error<<std::endl;
|
|
|
+ // std::cout<<"ndt pose x is "<<ndt_pose.x<<std::endl;
|
|
|
+ if (predict_pose_error <= (PREDICT_POSE_THRESHOLD*10*diff_time))
|
|
|
+ {
|
|
|
+ use_predict_pose = 0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ use_predict_pose = 1;
|
|
|
+ }
|
|
|
+ use_predict_pose = 0;
|
|
|
+
|
|
|
+ if (use_predict_pose == 0)
|
|
|
+ {
|
|
|
+ current_pose.x = ndt_pose.x;
|
|
|
+ current_pose.y = ndt_pose.y;
|
|
|
+ current_pose.z = ndt_pose.z;
|
|
|
+ current_pose.roll = ndt_pose.roll;
|
|
|
+ current_pose.pitch = ndt_pose.pitch;
|
|
|
+ current_pose.yaw = ndt_pose.yaw;
|
|
|
+
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ givlog->verbose("NDT","USE PREDICT POSE");
|
|
|
+ current_pose.x = predict_pose_for_ndt.x;
|
|
|
+ current_pose.y = predict_pose_for_ndt.y;
|
|
|
+ current_pose.z = predict_pose_for_ndt.z;
|
|
|
+ current_pose.roll = predict_pose_for_ndt.roll;
|
|
|
+ current_pose.pitch = predict_pose_for_ndt.pitch;
|
|
|
+ current_pose.yaw = predict_pose_for_ndt.yaw;
|
|
|
+ }
|
|
|
+
|
|
|
+ // std::cout<<" current pose x is "<<current_pose.x<<std::endl;
|
|
|
+
|
|
|
+ // Compute the velocity and acceleration
|
|
|
+ diff_x = current_pose.x - previous_pose.x;
|
|
|
+ diff_y = current_pose.y - previous_pose.y;
|
|
|
+ diff_z = current_pose.z - previous_pose.z;
|
|
|
+ diff_yaw = calcDiffForRadian(current_pose.yaw, previous_pose.yaw);
|
|
|
+ diff = sqrt(diff_x * diff_x + diff_y * diff_y + diff_z * diff_z);
|
|
|
+
|
|
|
+ current_velocity = (diff_time > 0) ? (diff / diff_time) : 0;
|
|
|
+ current_velocity_x = (diff_time > 0) ? (diff_x / diff_time) : 0;
|
|
|
+ current_velocity_y = (diff_time > 0) ? (diff_y / diff_time) : 0;
|
|
|
+ current_velocity_z = (diff_time > 0) ? (diff_z / diff_time) : 0;
|
|
|
+ angular_velocity = (diff_time > 0) ? (diff_yaw / diff_time) : 0;
|
|
|
+
|
|
|
+ current_pose.vx = current_velocity_x;
|
|
|
+ current_pose.vy = current_velocity_y;
|
|
|
+
|
|
|
+ current_velocity_smooth = (current_velocity + previous_velocity + previous_previous_velocity) / 3.0;
|
|
|
+ if (current_velocity_smooth < 0.2)
|
|
|
+ {
|
|
|
+ current_velocity_smooth = 0.0;
|
|
|
+ }
|
|
|
+
|
|
|
+ // bNotChangev = true;
|
|
|
+ if((diff_time > 0)&&(bNotChangev == false))
|
|
|
+ {
|
|
|
+ double aa = (current_velocity -previous_velocity)/diff_time;
|
|
|
+ if(fabs(aa)>5.0)
|
|
|
+ {
|
|
|
+ givlog->verbose("NDT","aa is %f",aa);
|
|
|
+ aa = fabs(5.0/aa);
|
|
|
+ current_velocity = previous_velocity + aa*(current_velocity -previous_velocity);
|
|
|
+ current_velocity_x = previous_velocity_x + aa *((current_velocity_x -previous_velocity_x));
|
|
|
+ current_velocity_y = previous_velocity_y + aa *((current_velocity_y -previous_velocity_y));
|
|
|
+ current_velocity_z = previous_velocity_z + aa *((current_velocity_z -previous_velocity_z));
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ current_accel = (diff_time > 0) ? ((current_velocity - previous_velocity) / diff_time) : 0;
|
|
|
+ current_accel_x = (diff_time > 0) ? ((current_velocity_x - previous_velocity_x) / diff_time) : 0;
|
|
|
+ current_accel_y = (diff_time > 0) ? ((current_velocity_y - previous_velocity_y) / diff_time) : 0;
|
|
|
+ current_accel_z = (diff_time > 0) ? ((current_velocity_z - previous_velocity_z) / diff_time) : 0;
|
|
|
+
|
|
|
+ // std::cout<<"fit time is "<<getFitnessScore_time<<std::endl;
|
|
|
+
|
|
|
+ gcurndtgpspos = PoseToGPS(gvector_trace[gcurmapindex].mndtorigin,current_pose);
|
|
|
+// std::cout<<" pre x:"<<previous_pose.x<<" pre y:"<<previous_pose.y<<std::endl;
|
|
|
+ std::cout<<" x: "<<current_pose.x<<" y: "<<current_pose.y
|
|
|
+ <<" z: "<<current_pose.z<<" yaw:"<<current_pose.yaw
|
|
|
+ <<" vel: "<<current_velocity<<std::endl;
|
|
|
+
|
|
|
+
|
|
|
+ std::cout<<"NDT ve:"<<gcurndtgpspos.ve<<" vn:"<<gcurndtgpspos.vn
|
|
|
+ <<" GPS ve:"<<gcurgpsgpspos.ve<<" vn:"<<gcurgpsgpspos.vn<<std::endl;
|
|
|
+
|
|
|
+ gbFileNDTUpdate = true;
|
|
|
+
|
|
|
+// gw->Updatexyyaw(current_pose.x,current_pose.y,current_pose.yaw);
|
|
|
+
|
|
|
+ current_pose_imu.x = current_pose.x;
|
|
|
+ current_pose_imu.y = current_pose.y;
|
|
|
+ current_pose_imu.z = current_pose.z;
|
|
|
+ current_pose_imu.roll = current_pose.roll;
|
|
|
+ current_pose_imu.pitch = current_pose.pitch;
|
|
|
+ current_pose_imu.yaw = current_pose.yaw;
|
|
|
+
|
|
|
+ current_velocity_imu_x = current_velocity_x;
|
|
|
+ current_velocity_imu_y = current_velocity_y;
|
|
|
+ current_velocity_imu_z = current_velocity_z;
|
|
|
+
|
|
|
+ current_velocity_imu_z = 0;
|
|
|
+
|
|
|
+ offset_imu_x = 0.0;
|
|
|
+ offset_imu_y = 0.0;
|
|
|
+ offset_imu_z = 0.0;
|
|
|
+ offset_imu_roll = 0.0;
|
|
|
+ offset_imu_pitch = 0.0;
|
|
|
+ offset_imu_yaw = 0.0;
|
|
|
+
|
|
|
+// std::cout<<" vx is "<<current_velocity_x<<" vy is "<<current_velocity_y<<std::endl;
|
|
|
+ previous_pose.x = current_pose.x;
|
|
|
+ previous_pose.y = current_pose.y;
|
|
|
+ previous_pose.z = current_pose.z;
|
|
|
+ previous_pose.roll = current_pose.roll;
|
|
|
+ previous_pose.pitch = current_pose.pitch;
|
|
|
+ previous_pose.yaw = current_pose.yaw;
|
|
|
+
|
|
|
+// previous_scan_time = current_scan_time;
|
|
|
+
|
|
|
+ previous_previous_velocity = previous_velocity;
|
|
|
+ previous_velocity = current_velocity;
|
|
|
+ previous_velocity_x = current_velocity_x;
|
|
|
+ previous_velocity_y = current_velocity_y;
|
|
|
+ previous_velocity_z = current_velocity_z;
|
|
|
+ previous_accel = current_accel;
|
|
|
+
|
|
|
+ pthread_mutex_lock(&mutex_read);
|
|
|
+ gindex++;
|
|
|
+ glidar_pose.accel = current_accel;
|
|
|
+ glidar_pose.accel_x = current_accel_x;
|
|
|
+ glidar_pose.accel_y = current_accel_y;
|
|
|
+ glidar_pose.accel_z = current_accel_z;
|
|
|
+ glidar_pose.vel = current_velocity;
|
|
|
+ glidar_pose.vel_x = current_velocity_x;
|
|
|
+ glidar_pose.vel_y = current_velocity_y;
|
|
|
+ glidar_pose.vel_z = current_velocity_z;
|
|
|
+ glidar_pose.mpose = current_pose;
|
|
|
+ pthread_mutex_unlock(&mutex_read);
|
|
|
+
|
|
|
+ pthread_mutex_unlock(&mutex_pose);
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ double hdg_o = (90 - gvector_trace[gcurmapindex].mndtorigin.heading)*M_PI/180.0;
|
|
|
+ double ndt_vn = current_velocity_x * cos(hdg_o) - current_velocity_y * sin(hdg_o);
|
|
|
+ double ndt_ve = current_velocity_x * sin(hdg_o) + current_velocity_y * cos(hdg_o);
|
|
|
+ double ndt_vd = current_accel_z;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ std::cout<<std::setprecision(9)<<"gps lat:"<<gcurndtgpspos.lat<<" lon:"
|
|
|
+ <<gcurndtgpspos.lon<<" z:"<<gcurndtgpspos.height<<" heading:"
|
|
|
+ <<gcurndtgpspos.heading<<std::endl;
|
|
|
+ std::cout<<std::setprecision(6)<<std::endl;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ double x0,y0;
|
|
|
+ double x,y;
|
|
|
+ GaussProjCal(gcurndtgpspos.lon,gcurndtgpspos.lat,&x0,&y0);
|
|
|
+ GaussProjCal(gcurgpsgpspos.lon,gcurgpsgpspos.lat,&x,&y);
|
|
|
+ double dis = sqrt(pow(x-x0,2)+ pow(y-y0,2));
|
|
|
+ double headdiff = fabs(gcurgpsgpspos.heading - gcurndtgpspos.heading);
|
|
|
+ double zdiff = fabs(gcurgpsgpspos.height - gcurndtgpspos.height);
|
|
|
+ std::cout<<" dis:"<<dis<<" headdiff:"<<headdiff<<" zdiff:"<<zdiff<<std::endl;
|
|
|
+
|
|
|
+ iv::lidar::ndtpos ndtpos;
|
|
|
+ ndtpos.set_lidartime(raw_scan->header.stamp/1000);
|
|
|
+ ndtpos.set_ndt_time(QDateTime::currentMSecsSinceEpoch());
|
|
|
+ ndtpos.set_accel(current_accel);
|
|
|
+ ndtpos.set_accel_x(current_accel_x);
|
|
|
+ ndtpos.set_accel_y(current_accel_y);
|
|
|
+ ndtpos.set_accel_z(current_accel_z);
|
|
|
+ ndtpos.set_vel(current_velocity);
|
|
|
+ ndtpos.set_vel_x(current_velocity_x);
|
|
|
+ ndtpos.set_vel_y(current_velocity_y);
|
|
|
+ ndtpos.set_vel_z(current_velocity_z);
|
|
|
+ ndtpos.set_fitness_score(fitness_score);
|
|
|
+ ndtpos.set_has_converged(has_converged);
|
|
|
+ ndtpos.set_id(0);
|
|
|
+ ndtpos.set_iteration(iteration);
|
|
|
+ ndtpos.set_trans_probability(trans_probability);
|
|
|
+ ndtpos.set_pose_pitch(current_pose.pitch);
|
|
|
+ ndtpos.set_pose_roll(current_pose.roll);
|
|
|
+ ndtpos.set_pose_yaw(current_pose.yaw);
|
|
|
+ ndtpos.set_pose_x(current_pose.x);
|
|
|
+ ndtpos.set_pose_y(current_pose.y);
|
|
|
+ ndtpos.set_pose_z(current_pose.z);
|
|
|
+ ndtpos.set_comp_time(xTime.elapsed());
|
|
|
+
|
|
|
+
|
|
|
+ int ndatasize = ndtpos.ByteSize();
|
|
|
+ char * strser = new char[ndatasize];
|
|
|
+ bool bSer = ndtpos.SerializeToArray(strser,ndatasize);
|
|
|
+ if(bSer)
|
|
|
+ {
|
|
|
+ iv::modulecomm::ModuleSendMsg(gpc,strser,ndatasize);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ std::cout<<BOLDRED<<"ndtpos serialize error."<<RESET<<std::endl;
|
|
|
+ }
|
|
|
+ delete strser;
|
|
|
+
|
|
|
+ iv::lidar::ndtgpspos xndtgpspos;
|
|
|
+ xndtgpspos.set_lat(gcurndtgpspos.lat);
|
|
|
+ xndtgpspos.set_lon(gcurndtgpspos.lon);
|
|
|
+ xndtgpspos.set_heading(gcurndtgpspos.heading);
|
|
|
+ xndtgpspos.set_height(gcurndtgpspos.height);
|
|
|
+ xndtgpspos.set_pitch(gcurndtgpspos.pitch);
|
|
|
+ xndtgpspos.set_roll(gcurndtgpspos.roll);
|
|
|
+ xndtgpspos.set_tras_prob(trans_probability);
|
|
|
+ xndtgpspos.set_score(fitness_score);
|
|
|
+ xndtgpspos.set_vn(ndt_vn);
|
|
|
+ xndtgpspos.set_ve(ndt_ve);
|
|
|
+ xndtgpspos.set_vd(ndt_vd);
|
|
|
+
|
|
|
+ givlog->debug("ndtgpspos","lat:%11.7f lon:%11.7f height:%11.3f heading:%6.3f pitch:%6.3f roll:%6.3f vn:%6.3f ve:%6.3f vd:%6.3f trans_prob:%6.3f score:%6.3f",
|
|
|
+ xndtgpspos.lat(),xndtgpspos.lon(),xndtgpspos.height(),
|
|
|
+ xndtgpspos.heading(),xndtgpspos.pitch(),xndtgpspos.roll(),
|
|
|
+ xndtgpspos.vn(),xndtgpspos.ve(),xndtgpspos.vd(),
|
|
|
+ xndtgpspos.tras_prob(),xndtgpspos.score());
|
|
|
+
|
|
|
+ ndatasize = xndtgpspos.ByteSize();
|
|
|
+ strser = new char[ndatasize];
|
|
|
+ bSer = xndtgpspos.SerializeToArray(strser,ndatasize);
|
|
|
+ if(bSer)
|
|
|
+ {
|
|
|
+ std::cout<<"share msg."<<std::endl;
|
|
|
+ iv::modulecomm::ModuleSendMsg(gpd,strser,ndatasize);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ std::cout<<BOLDRED<<"ndtgpspos serialize error."<<RESET<<std::endl;
|
|
|
+ }
|
|
|
+ delete strser;
|
|
|
+
|
|
|
+ if(trans_probability < 0.5)gbNeedGPSUpdateNDT = true;
|
|
|
+ else gbNeedGPSUpdateNDT = false;
|
|
|
+
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+int GetPose(int & curindex,iv::lidar_pose & xlidar_pose)
|
|
|
+{
|
|
|
+ pthread_mutex_lock(&mutex_read);
|
|
|
+ if(curindex == gindex)
|
|
|
+ {
|
|
|
+ pthread_mutex_unlock(&mutex_read);
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+ curindex = gindex;
|
|
|
+ xlidar_pose = glidar_pose;
|
|
|
+ pthread_mutex_unlock(&mutex_read);
|
|
|
+ return 1;
|
|
|
+}
|
|
|
+
|
|
|
+void SetCurPose(double x0, double y0, double yaw0, double z0, double pitch0, double roll0)
|
|
|
+{
|
|
|
+
|
|
|
+ std::cout<<"set pose"<<std::endl;
|
|
|
+ pthread_mutex_lock(&mutex_pose);
|
|
|
+
|
|
|
+ initial_pose.x = x0;
|
|
|
+ initial_pose.y = y0;
|
|
|
+ initial_pose.z = z0;
|
|
|
+ initial_pose.roll = roll0;
|
|
|
+ initial_pose.pitch = pitch0;
|
|
|
+ initial_pose.yaw = yaw0;
|
|
|
+
|
|
|
+ localizer_pose.x = initial_pose.x;
|
|
|
+ localizer_pose.y = initial_pose.y;
|
|
|
+ localizer_pose.z = initial_pose.z;
|
|
|
+ localizer_pose.roll = initial_pose.roll;
|
|
|
+ localizer_pose.pitch = initial_pose.pitch;
|
|
|
+ localizer_pose.yaw = initial_pose.yaw;
|
|
|
+
|
|
|
+ previous_pose.x = initial_pose.x;
|
|
|
+ previous_pose.y = initial_pose.y;
|
|
|
+ previous_pose.z = initial_pose.z;
|
|
|
+ previous_pose.roll = initial_pose.roll;
|
|
|
+ previous_pose.pitch = initial_pose.pitch;
|
|
|
+ previous_pose.yaw = initial_pose.yaw;
|
|
|
+
|
|
|
+ current_pose.x = initial_pose.x;
|
|
|
+ current_pose.y = initial_pose.y;
|
|
|
+ current_pose.z = initial_pose.z;
|
|
|
+ current_pose.roll = initial_pose.roll;
|
|
|
+ current_pose.pitch = initial_pose.pitch;
|
|
|
+ current_pose.yaw = initial_pose.yaw;
|
|
|
+
|
|
|
+ current_velocity = 0;
|
|
|
+ current_velocity_x = 0;
|
|
|
+ current_velocity_y = 0;
|
|
|
+ current_velocity_z = 0;
|
|
|
+ angular_velocity = 0;
|
|
|
+
|
|
|
+ pthread_mutex_unlock(&mutex_pose);
|
|
|
+
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void SetRunState(bool bRun)
|
|
|
+{
|
|
|
+ std::cout<<"set state."<<std::endl;
|
|
|
+ gbNDTRun = bRun;
|
|
|
+}
|
|
|
+
|
|
|
+void setuseimu(bool bUse)
|
|
|
+{
|
|
|
+ _use_imu = bUse;
|
|
|
+}
|
|
|
+
|
